PX4 Firmware
PX4 Autopilot Software http://px4.io
accelerometer_calibration.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-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 accelerometer_calibration.cpp
36  *
37  * Implementation of accelerometer calibration.
38  *
39  * Transform acceleration vector to true orientation, scale and offset
40  *
41  * ===== Model =====
42  * accel_corr = accel_T * (accel_raw - accel_offs)
43  *
44  * accel_corr[3] - fully corrected acceleration vector in body frame
45  * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform
46  * accel_raw[3] - raw acceleration vector
47  * accel_offs[3] - acceleration offset vector
48  *
49  * ===== Calibration =====
50  *
51  * Reference vectors
52  * accel_corr_ref[6][3] = [ g 0 0 ] // nose up
53  * | -g 0 0 | // nose down
54  * | 0 g 0 | // left side down
55  * | 0 -g 0 | // right side down
56  * | 0 0 g | // on back
57  * [ 0 0 -g ] // level
58  * accel_raw_ref[6][3]
59  *
60  * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5
61  *
62  * 6 reference vectors * 3 axes = 18 equations
63  * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
64  *
65  * Find accel_offs
66  *
67  * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
68  *
69  * Find accel_T
70  *
71  * 9 unknown constants
72  * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
73  *
74  * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
75  *
76  * Solve separate system for each row of accel_T:
77  *
78  * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2
79  *
80  * A * x = b
81  *
82  * x = [ accel_T[0][i] ]
83  * | accel_T[1][i] |
84  * [ accel_T[2][i] ]
85  *
86  * b = [ accel_corr_ref[0][i] ] // One measurement per side is enough
87  * | accel_corr_ref[2][i] |
88  * [ accel_corr_ref[4][i] ]
89  *
90  * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2
91  *
92  * Matrix A is common for all three systems:
93  * A = [ a[0][0] a[0][1] a[0][2] ]
94  * | a[2][0] a[2][1] a[2][2] |
95  * [ a[4][0] a[4][1] a[4][2] ]
96  *
97  * x = A^-1 * b
98  *
99  * accel_T = A^-1 * g
100  * g = 9.80665
101  *
102  * ===== Rotation =====
103  *
104  * Calibrating using model:
105  * accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r)
106  *
107  * Actual correction:
108  * accel_corr = rot * accel_T * (accel_raw - accel_offs)
109  *
110  * Known: accel_T_r, accel_offs_r, rot
111  * Unknown: accel_T, accel_offs
112  *
113  * Solution:
114  * accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs)
115  * rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs)
116  * rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs)
117  * => accel_T = rot^-1 * accel_T_r * rot
118  * => accel_offs = rot^-1 * accel_offs_r
119  *
120  * @author Anton Babushkin <anton.babushkin@me.com>
121  */
122 
123 // FIXME: Can some of these headers move out with detect_ move?
124 
126 #include "calibration_messages.h"
127 #include "calibration_routines.h"
128 #include "commander_helper.h"
129 
130 #include <px4_platform_common/defines.h>
131 #include <px4_platform_common/posix.h>
132 #include <px4_platform_common/time.h>
133 #include <unistd.h>
134 #include <stdio.h>
135 #include <poll.h>
136 #include <fcntl.h>
137 #include <math.h>
138 #include <poll.h>
139 #include <float.h>
140 #include <mathlib/mathlib.h>
141 #include <string.h>
142 #include <drivers/drv_hrt.h>
143 #include <drivers/drv_accel.h>
144 #include <lib/ecl/geo/geo.h>
145 #include <conversion/rotation.h>
146 #include <parameters/param.h>
147 #include <systemlib/err.h>
148 #include <systemlib/mavlink_log.h>
151 #include <uORB/Subscription.hpp>
152 
153 static const char *sensor_name = "accel";
154 
155 static int32_t device_id[max_accel_sens];
156 static int device_prio_max = 0;
157 static int32_t device_id_primary = 0;
158 
160  float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
161 calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens],
162  float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
163 int mat_invert3(float src[3][3], float dst[3][3]);
165  float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3],
166  float (&accel_offs)[max_accel_sens][3], float g);
167 
168 /// Data passed to calibration worker routine
169 typedef struct {
171  unsigned done_count;
172  int subs[max_accel_sens];
176 
177 int do_accel_calibration(orb_advert_t *mavlink_log_pub)
178 {
179 #ifdef __PX4_NUTTX
180  int fd;
181 #endif
182 
184 
185  struct accel_calibration_s accel_scale;
186  accel_scale.x_offset = 0.0f;
187  accel_scale.x_scale = 1.0f;
188  accel_scale.y_offset = 0.0f;
189  accel_scale.y_scale = 1.0f;
190  accel_scale.z_offset = 0.0f;
191  accel_scale.z_scale = 1.0f;
192 
193  int res = PX4_OK;
194 
195  char str[30];
196 
197  /* reset all sensors */
198  for (unsigned s = 0; s < max_accel_sens; s++) {
199 #ifdef __PX4_NUTTX
200  sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
201  /* reset all offsets to zero and all scales to one */
202  fd = px4_open(str, 0);
203 
204  if (fd < 0) {
205  continue;
206  }
207 
208  device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
209 
210  res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
211  px4_close(fd);
212 
213  if (res != PX4_OK) {
215  }
216 
217 #else
218  (void)sprintf(str, "CAL_ACC%u_XOFF", s);
219  res = param_set_no_notification(param_find(str), &accel_scale.x_offset);
220 
221  if (res != PX4_OK) {
222  PX4_ERR("unable to reset %s", str);
223  }
224 
225  (void)sprintf(str, "CAL_ACC%u_YOFF", s);
226  res = param_set_no_notification(param_find(str), &accel_scale.y_offset);
227 
228  if (res != PX4_OK) {
229  PX4_ERR("unable to reset %s", str);
230  }
231 
232  (void)sprintf(str, "CAL_ACC%u_ZOFF", s);
233  res = param_set_no_notification(param_find(str), &accel_scale.z_offset);
234 
235  if (res != PX4_OK) {
236  PX4_ERR("unable to reset %s", str);
237  }
238 
239  (void)sprintf(str, "CAL_ACC%u_XSCALE", s);
240  res = param_set_no_notification(param_find(str), &accel_scale.x_scale);
241 
242  if (res != PX4_OK) {
243  PX4_ERR("unable to reset %s", str);
244  }
245 
246  (void)sprintf(str, "CAL_ACC%u_YSCALE", s);
247  res = param_set_no_notification(param_find(str), &accel_scale.y_scale);
248 
249  if (res != PX4_OK) {
250  PX4_ERR("unable to reset %s", str);
251  }
252 
253  (void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
254  res = param_set_no_notification(param_find(str), &accel_scale.z_scale);
255 
256  if (res != PX4_OK) {
257  PX4_ERR("unable to reset %s", str);
258  }
259 
261 #endif
262  }
263 
264  float accel_offs[max_accel_sens][3];
265  float accel_T[max_accel_sens][3][3];
266  unsigned active_sensors = 0;
267 
268  /* measure and calculate offsets & scales */
269  if (res == PX4_OK) {
270  calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
271 
272  if (cal_return == calibrate_return_cancelled) {
273  // Cancel message already displayed, nothing left to do
274  return PX4_ERROR;
275 
276  } else if (cal_return == calibrate_return_ok) {
277  res = PX4_OK;
278 
279  } else {
280  res = PX4_ERROR;
281  }
282  }
283 
284  if (res != PX4_OK) {
285  if (active_sensors == 0) {
287  }
288 
289  return PX4_ERROR;
290  }
291 
292  /* measurements completed successfully, rotate calibration values */
293  param_t board_rotation_h = param_find("SENS_BOARD_ROT");
294  int32_t board_rotation_int;
295  param_get(board_rotation_h, &(board_rotation_int));
296  enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
297  matrix::Dcmf board_rotation = get_rot_matrix(board_rotation_id);
298 
299  matrix::Dcmf board_rotation_t = board_rotation.transpose();
300 
301  bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator
302 
303  for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) {
304 
305  /* handle individual sensors, one by one */
306  matrix::Vector3f accel_offs_vec(accel_offs[uorb_index]);
307  matrix::Vector3f accel_offs_rotated = board_rotation_t *accel_offs_vec;
308  matrix::Matrix3f accel_T_mat(accel_T[uorb_index]);
309  matrix::Matrix3f accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
310 
311  accel_scale.x_offset = accel_offs_rotated(0);
312  accel_scale.x_scale = accel_T_rotated(0, 0);
313  accel_scale.y_offset = accel_offs_rotated(1);
314  accel_scale.y_scale = accel_T_rotated(1, 1);
315  accel_scale.z_offset = accel_offs_rotated(2);
316  accel_scale.z_scale = accel_T_rotated(2, 2);
317 
318  bool failed = false;
319 
320  failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
321 
322 
323  PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
324  (double)accel_scale.x_offset,
325  (double)accel_scale.y_offset,
326  (double)accel_scale.z_offset);
327  PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
328  (double)accel_scale.x_scale,
329  (double)accel_scale.y_scale,
330  (double)accel_scale.z_scale);
331 
332  /* check if thermal compensation is enabled */
333  int32_t tc_enabled_int;
334  param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int));
335 
336  if (tc_enabled_int == 1) {
337  /* Get struct containing sensor thermal compensation data */
338  sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */
339  uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
340  sensor_correction_sub.copy(&sensor_correction);
341 
342  /* don't allow a parameter instance to be calibrated more than once by another uORB instance */
343  if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {
344  tc_locked[sensor_correction.accel_mapping[uorb_index]] = true;
345 
346  /* update the _X0_ terms to include the additional offset */
347  int32_t handle;
348  float val;
349 
350  for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
351  val = 0.0f;
352  (void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
353  handle = param_find(str);
354  param_get(handle, &val);
355 
356  if (axis_index == 0) {
357  val += accel_scale.x_offset;
358 
359  } else if (axis_index == 1) {
360  val += accel_scale.y_offset;
361 
362  } else if (axis_index == 2) {
363  val += accel_scale.z_offset;
364  }
365 
366  failed |= (PX4_OK != param_set_no_notification(handle, &val));
367  }
368 
369  /* update the _SCL_ terms to include the scale factor */
370  for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
371  val = 1.0f;
372  (void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
373  handle = param_find(str);
374 
375  if (axis_index == 0) {
376  val = accel_scale.x_scale;
377 
378  } else if (axis_index == 1) {
379  val = accel_scale.y_scale;
380 
381  } else if (axis_index == 2) {
382  val = accel_scale.z_scale;
383  }
384 
385  failed |= (PX4_OK != param_set_no_notification(handle, &val));
386  }
387 
389  }
390 
391  // Ensure the calibration values used by the driver are at default settings when we are using thermal calibration data
392  accel_scale.x_offset = 0.f;
393  accel_scale.y_offset = 0.f;
394  accel_scale.z_offset = 0.f;
395  accel_scale.x_scale = 1.f;
396  accel_scale.y_scale = 1.f;
397  accel_scale.z_scale = 1.f;
398  }
399 
400  // save the driver level calibration data
401  (void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
402  failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
403  (void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
404  failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
405  (void)sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
406  failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
407  (void)sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
408  failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
409  (void)sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
410  failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
411  (void)sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
412  failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
413  (void)sprintf(str, "CAL_ACC%u_ID", uorb_index);
414  failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index])));
415 
416  if (failed) {
417  calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, uorb_index);
418  return PX4_ERROR;
419  }
420 
421 #ifdef __PX4_NUTTX
422  sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, uorb_index);
423  fd = px4_open(str, 0);
424 
425  if (fd < 0) {
426  calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
427  res = PX4_ERROR;
428 
429  } else {
430  res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
431  px4_close(fd);
432  }
433 
434  if (res != PX4_OK) {
435  calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, uorb_index);
436  }
437 
438 #endif
439  }
440 
441  if (res == PX4_OK) {
442  /* if there is a any preflight-check system response, let the barrage of messages through */
443  px4_usleep(200000);
444 
446 
447  } else {
449  }
450 
451  /* give this message enough time to propagate */
452  px4_usleep(600000);
453 
454  return res;
455 }
456 
458 {
459  const unsigned samples_num = 750;
460  accel_worker_data_t *worker_data = (accel_worker_data_t *)(data);
461 
462  calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
463  detect_orientation_str(orientation));
464 
465  read_accelerometer_avg(worker_data->sensor_correction_sub, worker_data->subs, worker_data->accel_ref, orientation,
466  samples_num);
467 
468  calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]",
469  detect_orientation_str(orientation),
470  (double)worker_data->accel_ref[0][orientation][0],
471  (double)worker_data->accel_ref[0][orientation][1],
472  (double)worker_data->accel_ref[0][orientation][2]);
473 
474  worker_data->done_count++;
475  calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
476 
477  return calibrate_return_ok;
478 }
479 
481  float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
482 {
484 
485  *active_sensors = 0;
486 
487  accel_worker_data_t worker_data;
488 
489  worker_data.mavlink_log_pub = mavlink_log_pub;
490  worker_data.done_count = 0;
491 
492  bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };
493 
494  // Initialise sub to sensor thermal compensation data
495  worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
496 
497  // Initialize subs to error condition so we know which ones are open and which are not
498  for (size_t i = 0; i < max_accel_sens; i++) {
499  worker_data.subs[i] = -1;
500  }
501 
502  uint64_t timestamps[max_accel_sens] = {};
503 
504  // We should not try to subscribe if the topic doesn't actually exist and can be counted.
505  const unsigned orb_accel_count = orb_group_count(ORB_ID(sensor_accel));
506 
507  // Warn that we will not calibrate more than max_accels accelerometers
508  if (orb_accel_count > max_accel_sens) {
509  calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count,
510  max_accel_sens);
511  }
512 
513  for (unsigned cur_accel = 0; cur_accel < orb_accel_count && cur_accel < max_accel_sens; cur_accel++) {
514 
515  // Lock in to correct ORB instance
516  bool found_cur_accel = false;
517 
518  for (unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) {
519  worker_data.subs[cur_accel] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
520 
521  sensor_accel_s report = {};
522  orb_copy(ORB_ID(sensor_accel), worker_data.subs[cur_accel], &report);
523 
524 #ifdef __PX4_NUTTX
525 
526  // For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL
527  // and match it up with the one from the uORB subscription, because the
528  // instance ordering of uORB and the order of the FDs may not be the same.
529 
530  if (report.device_id == (uint32_t)device_id[cur_accel]) {
531  // Device IDs match, correct ORB instance for this accel
532  found_cur_accel = true;
533  // store initial timestamp - used to infer which sensors are active
534  timestamps[cur_accel] = report.timestamp;
535 
536  } else {
537  orb_unsubscribe(worker_data.subs[cur_accel]);
538  }
539 
540 #else
541 
542  // For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report.
543  device_id[cur_accel] = report.device_id;
544  found_cur_accel = true;
545 
546 #endif
547  }
548 
549  if (!found_cur_accel) {
550  calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]);
551  result = calibrate_return_error;
552  break;
553  }
554 
555  if (device_id[cur_accel] != 0) {
556  // Get priority
557  int32_t prio;
558  orb_priority(worker_data.subs[cur_accel], &prio);
559 
560  if (prio > device_prio_max) {
561  device_prio_max = prio;
562  device_id_primary = device_id[cur_accel];
563  }
564 
565  } else {
566  calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel);
567  result = calibrate_return_error;
568  break;
569  }
570  }
571 
572  if (result == calibrate_return_ok) {
573  int cancel_sub = calibrate_cancel_subscribe();
574  result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data,
575  false /* normal still */);
576  calibrate_cancel_unsubscribe(cancel_sub);
577  }
578 
579  /* close all subscriptions */
580  for (unsigned i = 0; i < max_accel_sens; i++) {
581  if (worker_data.subs[i] >= 0) {
582  /* figure out which sensors were active */
583  sensor_accel_s arp = {};
584  (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
585 
586  if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
587  (*active_sensors)++;
588  }
589 
590  px4_close(worker_data.subs[i]);
591  }
592  }
593 
595 
596  if (result == calibrate_return_ok) {
597  /* calculate offsets and transform matrix */
598  for (unsigned i = 0; i < (*active_sensors); i++) {
599  result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
600 
601  if (result != calibrate_return_ok) {
602  calibration_log_critical(mavlink_log_pub, "ERROR: calibration calculation error");
603  break;
604  }
605  }
606  }
607 
608  return result;
609 }
610 
611 /*
612  * Read specified number of accelerometer samples, calculate average and dispersion.
613  */
614 calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens],
615  float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
616 {
617  /* get total sensor board rotation matrix */
618  param_t board_rotation_h = param_find("SENS_BOARD_ROT");
619  param_t board_offset_x = param_find("SENS_BOARD_X_OFF");
620  param_t board_offset_y = param_find("SENS_BOARD_Y_OFF");
621  param_t board_offset_z = param_find("SENS_BOARD_Z_OFF");
622 
623  float board_offset[3];
624  param_get(board_offset_x, &board_offset[0]);
625  param_get(board_offset_y, &board_offset[1]);
626  param_get(board_offset_z, &board_offset[2]);
627 
628  matrix::Dcmf board_rotation_offset = matrix::Eulerf(
629  M_DEG_TO_RAD_F * board_offset[0],
630  M_DEG_TO_RAD_F * board_offset[1],
631  M_DEG_TO_RAD_F * board_offset[2]);
632 
633  int32_t board_rotation_int;
634  param_get(board_rotation_h, &(board_rotation_int));
635 
636  matrix::Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int);
637 
638  px4_pollfd_struct_t fds[max_accel_sens];
639 
640  for (unsigned i = 0; i < max_accel_sens; i++) {
641  fds[i].fd = subs[i];
642  fds[i].events = POLLIN;
643  }
644 
645  unsigned counts[max_accel_sens] = { 0 };
646  float accel_sum[max_accel_sens][3] {};
647 
648  unsigned errcount = 0;
649  struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
650 
651  /* try to get latest thermal corrections */
652  if (orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction) != 0) {
653  /* use default values */
654  memset(&sensor_correction, 0, sizeof(sensor_correction));
655 
656  for (unsigned i = 0; i < 3; i++) {
657  sensor_correction.accel_scale_0[i] = 1.0f;
658  sensor_correction.accel_scale_1[i] = 1.0f;
659  sensor_correction.accel_scale_2[i] = 1.0f;
660  }
661  }
662 
663  /* use the first sensor to pace the readout, but do per-sensor counts */
664  while (counts[0] < samples_num) {
665  int poll_ret = px4_poll(&fds[0], max_accel_sens, 1000);
666 
667  if (poll_ret > 0) {
668 
669  for (unsigned s = 0; s < max_accel_sens; s++) {
670  bool changed;
671  orb_check(subs[s], &changed);
672 
673  if (changed) {
674 
675  sensor_accel_s arp;
676  orb_copy(ORB_ID(sensor_accel), subs[s], &arp);
677 
678  // Apply thermal offset corrections in sensor/board frame
679  if (s == 0) {
680  accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]);
681  accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]);
682  accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]);
683 
684  } else if (s == 1) {
685  accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]);
686  accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]);
687  accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]);
688 
689  } else if (s == 2) {
690  accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]);
691  accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]);
692  accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]);
693 
694  } else {
695  accel_sum[s][0] += arp.x;
696  accel_sum[s][1] += arp.y;
697  accel_sum[s][2] += arp.z;
698  }
699 
700  counts[s]++;
701  }
702  }
703 
704  } else {
705  errcount++;
706  continue;
707  }
708 
709  if (errcount > samples_num / 10) {
710  return calibrate_return_error;
711  }
712  }
713 
714  // rotate sensor measurements from sensor to body frame using board rotation matrix
715  for (unsigned i = 0; i < max_accel_sens; i++) {
716  matrix::Vector3f accel_sum_vec(&accel_sum[i][0]);
717  accel_sum_vec = board_rotation * accel_sum_vec;
718 
719  for (size_t j = 0; j < 3; j++) {
720  accel_sum[i][j] = accel_sum_vec(j);
721  }
722  }
723 
724  for (unsigned s = 0; s < max_accel_sens; s++) {
725  for (unsigned i = 0; i < 3; i++) {
726  accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
727  }
728  }
729 
730  return calibrate_return_ok;
731 }
732 
733 int mat_invert3(float src[3][3], float dst[3][3])
734 {
735  float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
736  src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
737  src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
738 
739  if (fabsf(det) < FLT_EPSILON) {
740  return PX4_ERROR; // Singular matrix
741  }
742 
743  dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
744  dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
745  dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
746  dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det;
747  dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det;
748  dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det;
749  dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
750  dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
751  dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
752 
753  return PX4_OK;
754 }
755 
757  float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3],
758  float (&accel_offs)[max_accel_sens][3], float g)
759 {
760  /* calculate offsets */
761  for (unsigned i = 0; i < 3; i++) {
762  accel_offs[sensor][i] = (accel_ref[sensor][i * 2][i] + accel_ref[sensor][i * 2 + 1][i]) / 2;
763  }
764 
765  /* fill matrix A for linear equations system*/
766  float mat_A[3][3];
767  memset(mat_A, 0, sizeof(mat_A));
768 
769  for (unsigned i = 0; i < 3; i++) {
770  for (unsigned j = 0; j < 3; j++) {
771  float a = accel_ref[sensor][i * 2][j] - accel_offs[sensor][j];
772  mat_A[i][j] = a;
773  }
774  }
775 
776  /* calculate inverse matrix for A */
777  float mat_A_inv[3][3];
778 
779  if (mat_invert3(mat_A, mat_A_inv) != PX4_OK) {
780  return calibrate_return_error;
781  }
782 
783  /* copy results to accel_T */
784  for (unsigned i = 0; i < 3; i++) {
785  for (unsigned j = 0; j < 3; j++) {
786  /* simplify matrices mult because b has only one non-zero element == g at index i */
787  accel_T[sensor][j][i] = mat_A_inv[j][i] * g;
788  }
789  }
790 
791  return calibrate_return_ok;
792 }
793 
794 int do_level_calibration(orb_advert_t *mavlink_log_pub)
795 {
796  const unsigned cal_time = 5;
797  const unsigned cal_hz = 100;
798  unsigned settle_time = 30;
799 
800  bool success = false;
801  int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
802  struct vehicle_attitude_s att;
803  memset(&att, 0, sizeof(att));
804 
805  calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level");
806 
807  param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF");
808  param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF");
809  param_t board_rot_handle = param_find("SENS_BOARD_ROT");
810 
811  // save old values if calibration fails
812  float roll_offset_current;
813  float pitch_offset_current;
814  int32_t board_rot_current = 0;
815  param_get(roll_offset_handle, &roll_offset_current);
816  param_get(pitch_offset_handle, &pitch_offset_current);
817  param_get(board_rot_handle, &board_rot_current);
818 
819  // give attitude some time to settle if there have been changes to the board rotation parameters
820  if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON) {
821  settle_time = 0;
822  }
823 
824  float zero = 0.0f;
825  param_set_no_notification(roll_offset_handle, &zero);
826  param_set_no_notification(pitch_offset_handle, &zero);
828 
829  px4_pollfd_struct_t fds[1];
830 
831  fds[0].fd = att_sub;
832  fds[0].events = POLLIN;
833 
834  float roll_mean = 0.0f;
835  float pitch_mean = 0.0f;
836  unsigned counter = 0;
837 
838  // sleep for some time
840 
841  while (hrt_elapsed_time(&start) < settle_time * 1000000) {
843  (int)(90 * hrt_elapsed_time(&start) / 1e6f / (float)settle_time));
844  px4_sleep(settle_time / 10);
845  }
846 
847  start = hrt_absolute_time();
848 
849  // average attitude for 5 seconds
850  while (hrt_elapsed_time(&start) < cal_time * 1000000) {
851  int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
852 
853  if (pollret <= 0) {
854  // attitude estimator is not running
855  calibration_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot");
856  calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
857  goto out;
858  }
859 
860  orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
861  matrix::Eulerf euler = matrix::Quatf(att.q);
862  roll_mean += euler.phi();
863  pitch_mean += euler.theta();
864  counter++;
865  }
866 
867  calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
868 
869  if (counter > (cal_time * cal_hz / 2)) {
870  roll_mean /= counter;
871  pitch_mean /= counter;
872 
873  } else {
874  calibration_log_info(mavlink_log_pub, "not enough measurements taken");
875  success = false;
876  goto out;
877  }
878 
879  if (fabsf(roll_mean) > 0.8f) {
880  calibration_log_critical(mavlink_log_pub, "excess roll angle");
881 
882  } else if (fabsf(pitch_mean) > 0.8f) {
883  calibration_log_critical(mavlink_log_pub, "excess pitch angle");
884 
885  } else {
886  roll_mean *= (float)M_RAD_TO_DEG;
887  pitch_mean *= (float)M_RAD_TO_DEG;
888  param_set_no_notification(roll_offset_handle, &roll_mean);
889  param_set_no_notification(pitch_offset_handle, &pitch_mean);
891  success = true;
892  }
893 
894 out:
895 
896  if (success) {
897  calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
898  return 0;
899 
900  } else {
901  // set old parameters
902  param_set_no_notification(roll_offset_handle, &roll_offset_current);
903  param_set_no_notification(pitch_offset_handle, &pitch_offset_current);
905  calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
906  return 1;
907  }
908 }
#define CAL_ERROR_SENSOR_MSG
static orb_advert_t * mavlink_log_pub
calibrate_return calculate_calibration_values(unsigned sensor, float(&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float(&accel_T)[max_accel_sens][3][3], float(&accel_offs)[max_accel_sens][3], float g)
Accelerometer driver interface.
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
#define CAL_QGC_DONE_MSG
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
uint64_t timestamp
Definition: sensor_accel.h:53
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float(&accel_offs)[max_accel_sens][3], float(&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
Type theta() const
Definition: Euler.hpp:132
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
Definition of geo / math functions to perform geodesic calculations.
__EXPORT int param_set_no_notification(param_t param, const void *val)
Set the value of a parameter, but do not notify the system about the change.
Definition: parameters.cpp:820
Common calibration messages.
int do_level_calibration(orb_advert_t *mavlink_log_pub)
detect_orientation_return
int orb_priority(int handle, int32_t *priority)
Definition: uORB.cpp:121
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, bool side_data_collected[detect_orientation_side_count], calibration_from_orientation_worker_t calibration_worker, void *worker_data, bool lenient_still_position)
Perform calibration sequence which require a rest orientation detection prior to calibration.
float accel_ref[max_accel_sens][detect_orientation_side_count][3]
#define CAL_ERROR_RESET_CAL_MSG
static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void *data)
static const unsigned max_accel_sens
static const char * sensor_name
static int32_t device_id[max_accel_sens]
Type phi() const
Definition: Euler.hpp:128
Vector rotation library.
#define FLT_EPSILON
High-resolution timer with callouts and timekeeping.
Matrix< Type, N, M > transpose() const
Definition: Matrix.hpp:353
static constexpr float CONSTANTS_ONE_G
Definition: geo.h:51
accel scaling factors; Vout = Vscale * (Vin + Voffset)
Definition: drv_accel.h:54
Global flash based parameter store.
#define ACCEL_BASE_DEVICE_PATH
Definition: drv_accel.h:49
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
const char * detect_orientation_str(enum detect_orientation_return orientation)
Returns the human readable string representation of the orientation.
Commander helper functions definitions.
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
uint8_t * data
Definition: dataman.cpp:149
#define CAL_ERROR_APPLY_CAL_MSG
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
calibrate_return
#define CAL_QGC_STARTED_MSG
Euler< float > Eulerf
Definition: Euler.hpp:156
#define CAL_ERROR_SET_PARAMS_MSG
static int device_prio_max
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
static unsigned counter
Definition: safety.c:56
Euler angles class.
Definition: AxisAngle.hpp:18
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int fd
Definition: dataman.cpp:146
int orb_group_count(const struct orb_metadata *meta)
Get the number of published instances of a topic group.
Definition: uORB.cpp:110
static int32_t device_id_primary
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
int mat_invert3(float src[3][3], float dst[3][3])
static int start()
Definition: dataman.cpp:1452
calibrate_return read_accelerometer_avg(int sensor_correction_sub, int(&subs)[max_accel_sens], float(&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
#define ACCELIOCSSCALE
set the accel scaling constants to the structure pointed to by (arg)
Definition: drv_accel.h:73
int px4_open(const char *path, int flags,...)
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
__EXPORT void param_notify_changes(void)
Notify the system about parameter changes.
Definition: parameters.cpp:323
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
Definition: uORB.cpp:80
Definition of accelerometer calibration.
uint32_t device_id
Definition: sensor_accel.h:55
__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot)
Get the rotation matrix.
Definition: rotation.cpp:45
#define calibration_log_critical(_pub, _text,...)
Data passed to calibration worker routine.
static const unsigned detect_orientation_side_count
#define CAL_QGC_PROGRESS_MSG
#define CAL_QGC_FAILED_MSG
int calibrate_cancel_subscribe()
Called at the beginning of calibration in order to subscribe to the cancel command.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
Definition: param.h:98
#define calibration_log_info(_pub, _text,...)
int px4_close(int fd)
int px4_ioctl(int fd, int cmd, unsigned long arg)