PX4 Firmware
PX4 Autopilot Software http://px4.io
airspeed_calibration.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file airspeed_calibration.cpp
36  * Airspeed sensor calibration routine
37  */
38 
39 #include "airspeed_calibration.h"
40 #include "calibration_messages.h"
41 #include "calibration_routines.h"
42 #include "commander_helper.h"
43 
44 #include <px4_platform_common/defines.h>
45 #include <px4_platform_common/posix.h>
46 #include <px4_platform_common/time.h>
47 #include <stdio.h>
48 #include <unistd.h>
49 #include <fcntl.h>
50 #include <poll.h>
51 #include <math.h>
52 #include <drivers/drv_hrt.h>
53 #include <drivers/drv_airspeed.h>
55 #include <systemlib/mavlink_log.h>
56 #include <parameters/param.h>
57 #include <systemlib/err.h>
58 
59 static const char *sensor_name = "airspeed";
60 
62 {
63  px4_sleep(5);
65 }
66 
68 {
69  int result = PX4_OK;
70  unsigned calibration_counter = 0;
71  const unsigned maxcount = 2400;
72 
73  /* give directions */
75 
76  const unsigned calibration_count = (maxcount * 2) / 3;
77 
78  int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
79  struct differential_pressure_s diff_pres;
80 
81  float diff_pres_offset = 0.0f;
82 
83  /* Reset sensor parameters */
84  struct airspeed_scale airscale = {
85  diff_pres_offset,
86  1.0f,
87  };
88 
89  bool paramreset_successful = false;
91 
92  if (fd > 0) {
93  if (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
94  paramreset_successful = true;
95 
96  } else {
97  calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed");
98  }
99 
100  px4_close(fd);
101  }
102 
103  int cancel_sub = calibrate_cancel_subscribe();
104 
105  if (!paramreset_successful) {
106 
107  /* only warn if analog scaling is zero */
108  float analog_scaling = 0.0f;
109  param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
110 
111  if (fabsf(analog_scaling) < 0.1f) {
112  calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
113  goto error_return;
114  }
115 
116  /* set scaling offset parameter */
117  if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
119  goto error_return;
120  }
121  }
122 
123  calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
124  px4_usleep(500 * 1000);
125 
126  while (calibration_counter < calibration_count) {
127 
128  if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
129  goto error_return;
130  }
131 
132  /* wait blocking for new data */
133  px4_pollfd_struct_t fds[1];
134  fds[0].fd = diff_pres_sub;
135  fds[0].events = POLLIN;
136 
137  int poll_ret = px4_poll(fds, 1, 1000);
138 
139  if (poll_ret) {
140  orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
141 
142  diff_pres_offset += diff_pres.differential_pressure_raw_pa;
143  calibration_counter++;
144 
145  /* any differential pressure failure a reason to abort */
146  if (diff_pres.error_count != 0) {
147  calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu64 ")",
148  diff_pres.error_count);
149  calibration_log_critical(mavlink_log_pub, "[cal] Check your wiring before trying again");
150  feedback_calibration_failed(mavlink_log_pub);
151  goto error_return;
152  }
153 
154  if (calibration_counter % (calibration_count / 20) == 0) {
155  calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
156  }
157 
158  } else if (poll_ret == 0) {
159  /* any poll failure for 1s is a reason to abort */
160  feedback_calibration_failed(mavlink_log_pub);
161  goto error_return;
162  }
163  }
164 
165  diff_pres_offset = diff_pres_offset / calibration_count;
166 
167  if (PX4_ISFINITE(diff_pres_offset)) {
168 
169  int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
170  airscale.offset_pa = diff_pres_offset;
171 
172  if (fd_scale > 0) {
173  if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
174  calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
175  }
176 
177  px4_close(fd_scale);
178  }
179 
180  // Prevent a completely zero param
181  // since this is used to detect a missing calibration
182  // This value is numerically down in the noise and has
183  // no effect on the sensor performance.
184  if (fabsf(diff_pres_offset) < 0.00000001f) {
185  diff_pres_offset = 0.00000001f;
186  }
187 
188  if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
190  goto error_return;
191  }
192 
193  } else {
194  feedback_calibration_failed(mavlink_log_pub);
195  goto error_return;
196  }
197 
198  calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
199 
200  /* wait 500 ms to ensure parameter propagated through the system */
201  px4_usleep(500 * 1000);
202 
203  calibration_log_critical(mavlink_log_pub, "[cal] Blow across front of pitot without touching");
204 
205  calibration_counter = 0;
206 
207  /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
208  while (calibration_counter < maxcount) {
209 
210  if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
211  goto error_return;
212  }
213 
214  /* wait blocking for new data */
215  px4_pollfd_struct_t fds[1];
216  fds[0].fd = diff_pres_sub;
217  fds[0].events = POLLIN;
218 
219  int poll_ret = px4_poll(fds, 1, 1000);
220 
221  if (poll_ret) {
222  orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
223 
224  if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) {
225  if (diff_pres.differential_pressure_filtered_pa > 0) {
226  calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)",
227  (int)diff_pres.differential_pressure_filtered_pa);
228  break;
229 
230  } else {
231  /* do not allow negative values */
232  calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
233  (int)diff_pres.differential_pressure_filtered_pa);
234  calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
235 
236  /* the user setup is wrong, wipe the calibration to force a proper re-calibration */
237  diff_pres_offset = 0.0f;
238 
239  if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
241  goto error_return;
242  }
243 
244  /* save */
245  calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
247 
248  feedback_calibration_failed(mavlink_log_pub);
249  goto error_return;
250  }
251  }
252 
253  if (calibration_counter % 500 == 0) {
254  calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
255  (int)diff_pres.differential_pressure_filtered_pa);
256  tune_neutral(true);
257  }
258 
259  calibration_counter++;
260 
261  } else if (poll_ret == 0) {
262  /* any poll failure for 1s is a reason to abort */
263  feedback_calibration_failed(mavlink_log_pub);
264  goto error_return;
265  }
266  }
267 
268  if (calibration_counter == maxcount) {
269  feedback_calibration_failed(mavlink_log_pub);
270  goto error_return;
271  }
272 
273  calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
274 
276  tune_neutral(true);
277 
278  /* Wait 2sec for the airflow to stop and ensure the driver filter has caught up, otherwise
279  * the followup preflight checks might fail. */
280  px4_usleep(2e6);
281 
282 normal_return:
283  calibrate_cancel_unsubscribe(cancel_sub);
284  px4_close(diff_pres_sub);
285 
286  // This give a chance for the log messages to go out of the queue before someone else stomps on then
287  px4_sleep(1);
288 
289  return result;
290 
291 error_return:
292  result = PX4_ERROR;
293  goto normal_return;
294 }
void tune_neutral(bool use_buzzer)
Blink white LED and play neutral tune (if use_buzzer == true).
static orb_advert_t * mavlink_log_pub
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
#define CAL_QGC_DONE_MSG
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
#define AIRSPEED0_DEVICE_PATH
Definition: drv_airspeed.h:52
Common calibration messages.
__EXPORT int param_set(param_t param, const void *val)
Set the value of a parameter.
Definition: parameters.cpp:814
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
High-resolution timer with callouts and timekeeping.
Global flash based parameter store.
void calibrate_cancel_unsubscribe(int cmd_sub)
Called to cancel the subscription to the cancel command.
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
Commander helper functions definitions.
airspeed scaling factors; out = (in * Vscale) + offset
Definition: drv_airspeed.h:67
static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
#define CAL_QGC_STARTED_MSG
#define CAL_ERROR_SET_PARAMS_MSG
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
__EXPORT int param_save_default(void)
Save parameters to the default file.
Definition: parameters.cpp:974
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int fd
Definition: dataman.cpp:146
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
#define AIRSPEEDIOCSSCALE
Definition: drv_airspeed.h:64
int px4_open(const char *path, int flags,...)
bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub)
Used to periodically check for a cancel command.
#define calibration_log_critical(_pub, _text,...)
static const char * sensor_name
#define CAL_QGC_PROGRESS_MSG
#define CAL_QGC_FAILED_MSG
Airspeed driver interface.
int calibrate_cancel_subscribe()
Called at the beginning of calibration in order to subscribe to the cancel command.
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
#define calibration_log_info(_pub, _text,...)
int px4_close(int fd)
int px4_ioctl(int fd, int cmd, unsigned long arg)