PX4 Firmware
PX4 Autopilot Software http://px4.io
task.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2017 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 task.cpp
36  *
37  * Main task handling the temperature calibration process
38  *
39  * @author Beat Küng <beat-kueng@gmx.net>
40  */
41 
44 #include <mathlib/mathlib.h>
45 #include <px4_platform_common/log.h>
46 #include <px4_platform_common/posix.h>
47 #include <px4_platform_common/tasks.h>
48 #include <drivers/drv_hrt.h>
49 #include <drivers/drv_led.h>
50 
51 #include <unistd.h>
52 
53 #include "common.h"
55 #include "accel.h"
56 #include "baro.h"
57 #include "gyro.h"
58 
60 
62 {
64 }
65 
66 
68 {
69 public:
70  /**
71  * Constructor
72  */
73  TemperatureCalibration(bool accel, bool baro, bool gyro);
74 
75  /**
76  * Destructor
77  */
78  ~TemperatureCalibration() = default;
79 
80  /**
81  * Start task.
82  *
83  * @return OK on success.
84  */
85  int start();
86 
87  static int do_temperature_calibration(int argc, char *argv[]);
88 
89  void task_main();
90 
91  void exit_task() { _force_task_exit = true; }
92 
93 private:
95 
96  uORB::PublicationQueued<led_control_s> _led_control_pub{ORB_ID(led_control)};
97 
98  bool _force_task_exit = false;
99  int _control_task = -1; // task handle for task
100 
101  bool _accel; ///< enable accel calibration?
102  bool _baro; ///< enable baro calibration?
103  bool _gyro; ///< enable gyro calibration?
104 };
105 
106 TemperatureCalibration::TemperatureCalibration(bool accel, bool baro, bool gyro)
107  : _accel(accel), _baro(baro), _gyro(gyro)
108 {
109 }
110 
112 {
113  // subscribe to all gyro instances
114  int gyro_sub[SENSOR_COUNT_MAX] = {};
115  px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
116  unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro));
117 
118  if (num_gyro > SENSOR_COUNT_MAX) {
119  num_gyro = SENSOR_COUNT_MAX;
120  }
121 
122  for (unsigned i = 0; i < num_gyro; i++) {
123  gyro_sub[i] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
124  fds[i].fd = gyro_sub[i];
125  fds[i].events = POLLIN;
126  }
127 
128  int32_t min_temp_rise = 24;
129  param_get(param_find("SYS_CAL_TDEL"), &min_temp_rise);
130  PX4_INFO("Waiting for %i degrees difference in sensor temperature", min_temp_rise);
131 
132  int32_t min_start_temp = 5;
133  param_get(param_find("SYS_CAL_TMIN"), &min_start_temp);
134 
135  int32_t max_start_temp = 10;
136  param_get(param_find("SYS_CAL_TMAX"), &max_start_temp);
137 
138  //init calibrators
139  TemperatureCalibrationBase *calibrators[3];
140  bool error_reported[3] = {};
141  int num_calibrators = 0;
142 
143  if (_accel) {
144  calibrators[num_calibrators] = new TemperatureCalibrationAccel(min_temp_rise, min_start_temp, max_start_temp);
145 
146  if (calibrators[num_calibrators]) {
147  ++num_calibrators;
148 
149  } else {
150  PX4_ERR("alloc failed");
151  }
152  }
153 
154  if (_baro) {
155  calibrators[num_calibrators] = new TemperatureCalibrationBaro(min_temp_rise, min_start_temp, max_start_temp);
156 
157  if (calibrators[num_calibrators]) {
158  ++num_calibrators;
159 
160  } else {
161  PX4_ERR("alloc failed");
162  }
163  }
164 
165  if (_gyro) {
166  calibrators[num_calibrators] = new TemperatureCalibrationGyro(min_temp_rise, min_start_temp, max_start_temp, gyro_sub,
167  num_gyro);
168 
169  if (calibrators[num_calibrators]) {
170  ++num_calibrators;
171 
172  } else {
173  PX4_ERR("alloc failed");
174  }
175  }
176 
177  // reset params
178  for (int i = 0; i < num_calibrators; ++i) {
179  calibrators[i]->reset_calibration();
180  }
181 
182  // make sure the system updates the changed parameters
184 
185  px4_usleep(300000); // wait a bit for the system to apply the parameters
186 
187  hrt_abstime next_progress_output = hrt_absolute_time() + 1e6;
188 
189  // control LED's: blink, then turn solid according to progress
191  led_control.led_mask = 0xff;
192  led_control.mode = led_control_s::MODE_BLINK_NORMAL;
193  led_control.priority = led_control_s::MAX_PRIORITY;
194  led_control.color = led_control_s::COLOR_YELLOW;
195  led_control.num_blinks = 0;
196  publish_led_control(led_control);
197  int leds_completed = 0;
198 
199  bool abort_calibration = false;
200 
201  while (!_force_task_exit) {
202  /* we poll on the gyro(s), since this is the sensor with the highest update rate.
203  * Each individual sensor will then check on its own if there's new data.
204  */
205  int ret = px4_poll(fds, num_gyro, 1000);
206 
207  if (ret < 0) {
208  // Poll error, sleep and try again
209  px4_usleep(10000);
210  continue;
211 
212  } else if (ret == 0) {
213  // Poll timeout or no new data, do nothing
214  continue;
215  }
216 
217  //if gyro is not enabled: we must do an orb_copy here, so that poll() does not immediately return again
218  if (!_gyro) {
219  sensor_gyro_s gyro_data;
220 
221  for (unsigned i = 0; i < num_gyro; ++i) {
222  orb_copy(ORB_ID(sensor_gyro), gyro_sub[i], &gyro_data);
223  }
224  }
225 
226  int min_progress = 110;
227 
228  for (int i = 0; i < num_calibrators; ++i) {
229  ret = calibrators[i]->update();
230 
231  if (ret == -TC_ERROR_COMMUNICATION) {
232  abort_calibration = true;
233  PX4_ERR("Calibration won't start - sensor bad or communication error");
234  _force_task_exit = true;
235  break;
236 
237  } else if (ret == -TC_ERROR_INITIAL_TEMP_TOO_HIGH) {
238  abort_calibration = true;
239  PX4_ERR("Calibration won't start - sensor temperature too high");
240  _force_task_exit = true;
241  break;
242 
243  } else if (ret < 0 && !error_reported[i]) {
244  // temperature has decreased so calibration is not being updated
245  error_reported[i] = true;
246  PX4_ERR("Calibration update step failed (%i)", ret);
247 
248  } else if (ret < min_progress) {
249  // temperature is stable or increasing
250  min_progress = ret;
251  }
252  }
253 
254  if (min_progress == 110 || abort_calibration) {
255  break; // we are done
256  }
257 
258  int led_progress = min_progress * BOARD_MAX_LEDS / 100;
259 
260  for (; leds_completed < led_progress; ++leds_completed) {
261  led_control.led_mask = 1 << leds_completed;
262  led_control.mode = led_control_s::MODE_ON;
263  publish_led_control(led_control);
264  }
265 
266  //print progress each second
268 
269  if (now > next_progress_output) {
270  PX4_INFO("Calibration progress: %i%%", min_progress);
271  next_progress_output = now + 1e6;
272  }
273  }
274 
275  if (abort_calibration) {
276  led_control.color = led_control_s::COLOR_RED;
277 
278  } else {
279  PX4_INFO("Sensor Measurments completed");
280 
281  // save params immediately so that we can check the result and don't have to wait for param save timeout
282  param_control_autosave(false);
283 
284  // do final calculations & parameter storage
285  for (int i = 0; i < num_calibrators; ++i) {
286  int ret = calibrators[i]->finish();
287 
288  if (ret < 0) {
289  PX4_ERR("Failed to finish calibration process (%i)", ret);
290  }
291  }
292 
294  int ret = param_save_default();
295 
296  if (ret != 0) {
297  PX4_ERR("Failed to save params (%i)", ret);
298  }
299 
301 
302  led_control.color = led_control_s::COLOR_GREEN;
303  }
304 
305  // blink the LED's according to success/failure
306  led_control.led_mask = 0xff;
307  led_control.mode = led_control_s::MODE_BLINK_FAST;
308  led_control.num_blinks = 0;
309  publish_led_control(led_control);
310 
311  for (int i = 0; i < num_calibrators; ++i) {
312  delete calibrators[i];
313  }
314 
315  for (unsigned i = 0; i < num_gyro; i++) {
316  orb_unsubscribe(gyro_sub[i]);
317  }
318 
321  PX4_INFO("Exiting temperature calibration task");
322 }
323 
325 {
327  return 0;
328 }
329 
331 {
332 
333  _control_task = px4_task_spawn_cmd("temperature_calib",
334  SCHED_DEFAULT,
335  SCHED_PRIORITY_MAX - 5,
336  5800,
338  nullptr);
339 
340  if (_control_task < 0) {
343  PX4_ERR("start failed");
344  return -errno;
345  }
346 
347  return 0;
348 }
349 
351 {
352  led_control.timestamp = hrt_absolute_time();
353  _led_control_pub.publish(led_control);
354 }
355 
356 int run_temperature_calibration(bool accel, bool baro, bool gyro)
357 {
358  PX4_INFO("Starting temperature calibration task (accel=%i, baro=%i, gyro=%i)", (int)accel, (int)baro, (int)gyro);
360 
361  if (temperature_calibration::instance == nullptr) {
362  PX4_ERR("alloc failed");
363  return 1;
364  }
365 
367 }
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
Base class for temperature calibration types with abstract methods (for all different sensor types) ...
Definition: common.h:58
TemperatureCalibration * instance
Definition: task.cpp:63
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
uint8_t priority
Definition: led_control.h:77
#define TC_ERROR_COMMUNICATION
no sensors found
Definition: common.h:53
static led_control_s led_control
static int do_temperature_calibration(int argc, char *argv[])
Definition: task.cpp:324
bool _baro
enable baro calibration?
Definition: task.cpp:102
#define TC_ERROR_INITIAL_TEMP_TOO_HIGH
starting temperature was above the configured allowed temperature
Definition: common.h:52
bool publish(const T &data)
Publish the struct.
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
static void publish_led_control(led_control_s &led_control)
Definition: led_control.cpp:57
uint8_t led_mask
Definition: led_control.h:73
virtual void reset_calibration()=0
reset all driver-level calibration parameters
uint8_t color
Definition: led_control.h:74
High-resolution timer with callouts and timekeeping.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
uint8_t mode
Definition: led_control.h:75
Led device API to control the external LED(s) via uORB interface.
static sensor_accel_s _accel
accel report
int run_temperature_calibration(bool accel, bool baro, bool gyro)
start temperature calibration in a new task for one or multiple sensors
Definition: task.cpp:356
virtual int finish()=0
do final fitting & write the parameters.
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
__EXPORT int param_save_default(void)
Save parameters to the default file.
Definition: parameters.cpp:974
void task_main(int argc, char *argv[])
static sensor_gyro_s _gyro
gyro report
uint64_t timestamp
Definition: led_control.h:72
int start()
Start task.
Definition: task.cpp:330
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
bool _accel
enable accel calibration?
Definition: task.cpp:101
int orb_group_count(const struct orb_metadata *meta)
Get the number of published instances of a topic group.
Definition: uORB.cpp:110
bool _gyro
enable gyro calibration?
Definition: task.cpp:103
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
#define BOARD_MAX_LEDS
Definition: drv_led.h:49
static int start()
Definition: dataman.cpp:1452
uint8_t num_blinks
Definition: led_control.h:76
void publish_led_control(led_control_s &led_control)
Definition: task.cpp:350
#define SENSOR_COUNT_MAX
Definition: common.h:49
__EXPORT void param_notify_changes(void)
Notify the system about parameter changes.
Definition: parameters.cpp:323
__EXPORT void param_control_autosave(bool enable)
Enable/disable the param autosaving.
Definition: parameters.cpp:687
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
Definition: uORB.cpp:80
virtual int update()=0
check & update new sensor data.
uORB::PublicationQueued< led_control_s > _led_control_pub
Definition: task.cpp:96
TemperatureCalibration(bool accel, bool baro, bool gyro)
Constructor.
Definition: task.cpp:106
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).