fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 10:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

accel and gyro calibration refactor and cleanup

Browse Source 
- remove all remaining IOCTLs for accel and gyro and handle all calibration entirely in sensors module with parameters
 - sensor_accel and sensor_gyro are now always raw sensor data
 - calibration procedures no longer need to first clear existing values before starting
 - temperature calibration (TC) remove all scale (SCL) parameters
    - gyro and baro scale are completely unused
    - regular accel calibration scale can be used (CAL_ACC*_xSCALE) instead of TC scale
This commit is contained in:
Daniel Agar
2020-06-17 22:50:09 -04:00
committed by GitHub
parent 588d551098
commit f55ed0992c
99 changed files with 933 additions and 2229 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+211 -510
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -120,8 +120,6 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
// FIXME: Can some of these headers move out with detect_ move?
#include "accelerometer_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
@@ -130,316 +128,184 @@
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
#include <math.h>
#include <float.h>
#include <mathlib/mathlib.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <lib/mathlib/mathlib.h>
#include <lib/ecl/geo/geo.h>
#include <matrix/math.hpp>
#include <conversion/rotation.h>
#include <parameters/param.h>
#include <lib/conversion/rotation.h>
#include <lib/parameters/param.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionBlocking.hpp>
using namespace time_literals;
using namespace matrix;
using math::radians;
static const char *sensor_name = "accel";
static constexpr char sensor_name[] {"accel"};
static int32_t device_id[max_accel_sens];
static int device_prio_max = 0;
static int32_t device_id_primary = 0;
static constexpr unsigned MAX_ACCEL_SENS = 3;
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);
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);
int mat_invert3(float src[3][3], float dst[3][3]);
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);
static calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub,
Vector3f(&accel_offs)[MAX_ACCEL_SENS],
Matrix3f(&accel_T)[MAX_ACCEL_SENS], unsigned active_sensors);
static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3],
unsigned orient, unsigned samples_num);
static calibrate_return calculate_calibration_values(unsigned sensor,
float (&accel_ref)[MAX_ACCEL_SENS][detect_orientation_side_count][3], Matrix3f(&accel_T)[MAX_ACCEL_SENS],
Vector3f(&accel_offs)[MAX_ACCEL_SENS]);
/// Data passed to calibration worker routine
typedef struct {
orb_advert_t *mavlink_log_pub;
unsigned done_count;
int subs[max_accel_sens];
float accel_ref[max_accel_sens][detect_orientation_side_count][3];
int sensor_correction_sub;
orb_advert_t *mavlink_log_pub{nullptr};
unsigned done_count{0};
float accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {};
} accel_worker_data_t;
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
{
#if 1 // TODO: replace all IOCTL usage
int fd;
#endif
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct accel_calibration_s accel_scale;
accel_scale.x_offset = 0.0f;
accel_scale.x_scale = 1.0f;
accel_scale.y_offset = 0.0f;
accel_scale.y_scale = 1.0f;
accel_scale.z_offset = 0.0f;
accel_scale.z_scale = 1.0f;
int res = PX4_OK;
char str[30];
/* reset all sensors */
for (unsigned s = 0; s < max_accel_sens; s++) {
#if 1 // TODO: replace all IOCTL usage
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */
fd = px4_open(str, 0);
if (fd < 0) {
continue;
}
device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
}
#else
(void)sprintf(str, "CAL_ACC%u_XOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
param_notify_changes();
#endif
}
float accel_offs[max_accel_sens][3];
float accel_T[max_accel_sens][3][3];
int32_t device_id[MAX_ACCEL_SENS] {};
int device_prio_max = 0;
int32_t device_id_primary = 0;
unsigned active_sensors = 0;
/* measure and calculate offsets & scales */
if (res == PX4_OK) {
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned orb_accel_count = orb_group_count(ORB_ID(sensor_accel));
if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do
return PX4_ERROR;
// Warn that we will not calibrate more than max_accels accelerometers
if (orb_accel_count > MAX_ACCEL_SENS) {
calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count,
MAX_ACCEL_SENS);
}
} else if (cal_return == calibrate_return_ok) {
res = PX4_OK;
for (uint8_t cur_accel = 0; cur_accel < orb_accel_count && cur_accel < MAX_ACCEL_SENS; cur_accel++) {
uORB::SubscriptionData<sensor_accel_s> accel_sub{ORB_ID(sensor_accel), cur_accel};
device_id[cur_accel] = accel_sub.get().device_id;
if (device_id[cur_accel] != 0) {
// Get priority
int32_t prio = accel_sub.get_priority();
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_id[cur_accel];
}
active_sensors++;
} else {
res = PX4_ERROR;
calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel);
return PX4_ERROR;
}
}
if (res != PX4_OK) {
if (active_sensors == 0) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
}
/* measure and calculate offsets & scales */
Vector3f accel_offs[MAX_ACCEL_SENS] {};
Matrix3f accel_T[MAX_ACCEL_SENS] {};
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, active_sensors);
if (cal_return != calibrate_return_ok) {
// Cancel message already displayed, nothing left to do
return PX4_ERROR;
}
if (active_sensors == 0) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
return PX4_ERROR;
}
/* measurements completed successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
Dcmf board_rotation = get_rot_matrix(board_rotation_id);
int32_t board_rotation_int = 0;
param_get(param_find("SENS_BOARD_ROT"), &board_rotation_int);
const Dcmf board_rotation = get_rot_matrix((enum Rotation)board_rotation_int);
const Dcmf board_rotation_t = board_rotation.transpose();
Dcmf board_rotation_t = board_rotation.transpose();
param_set_no_notification(param_find("CAL_ACC_PRIME"), &device_id_primary);
bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator
for (unsigned uorb_index = 0; uorb_index < MAX_ACCEL_SENS; uorb_index++) {
for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) {
if (uorb_index < active_sensors) {
/* handle individual sensors, one by one */
const Vector3f accel_offs_rotated = board_rotation_t *accel_offs[uorb_index];
const Matrix3f accel_T_rotated = board_rotation_t *accel_T[uorb_index] * board_rotation;
/* handle individual sensors, one by one */
Vector3f accel_offs_vec(accel_offs[uorb_index]);
Vector3f accel_offs_rotated = board_rotation_t *accel_offs_vec;
Matrix3f accel_T_mat(accel_T[uorb_index]);
Matrix3f accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_offs_rotated(0), (double)accel_offs_rotated(1), (double)accel_offs_rotated(2));
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
accel_scale.y_offset = accel_offs_rotated(1);
accel_scale.y_scale = accel_T_rotated(1, 1);
accel_scale.z_offset = accel_offs_rotated(2);
accel_scale.z_scale = accel_T_rotated(2, 2);
PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_T_rotated(0, 0), (double)accel_T_rotated(1, 1), (double)accel_T_rotated(2, 2));
bool failed = false;
char str[30] {};
failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
// calibration offsets
float x_offset = accel_offs_rotated(0);
sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
param_set_no_notification(param_find(str), &x_offset);
float y_offset = accel_offs_rotated(1);
sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
param_set_no_notification(param_find(str), &y_offset);
float z_offset = accel_offs_rotated(2);
sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
param_set_no_notification(param_find(str), &z_offset);
PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_offset,
(double)accel_scale.y_offset,
(double)accel_scale.z_offset);
PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_scale,
(double)accel_scale.y_scale,
(double)accel_scale.z_scale);
// calibration scale
float x_scale = accel_T_rotated(0, 0);
sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
param_set_no_notification(param_find(str), &x_scale);
/* check if thermal compensation is enabled */
int32_t tc_enabled_int;
param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int));
float y_scale = accel_T_rotated(1, 1);
sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
param_set_no_notification(param_find(str), &y_scale);
if (tc_enabled_int == 1) {
/* Get struct containing sensor thermal compensation data */
sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */
uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
sensor_correction_sub.copy(&sensor_correction);
float z_scale = accel_T_rotated(2, 2);
sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
param_set_no_notification(param_find(str), &z_scale);
/* don't allow a parameter instance to be calibrated more than once by another uORB instance */
if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {
tc_locked[sensor_correction.accel_mapping[uorb_index]] = true;
/* update the _X0_ terms to include the additional offset */
int32_t handle;
float val;
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 0.0f;
(void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
param_get(handle, &val);
if (axis_index == 0) {
val += accel_scale.x_offset;
} else if (axis_index == 1) {
val += accel_scale.y_offset;
} else if (axis_index == 2) {
val += accel_scale.z_offset;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
/* update the _SCL_ terms to include the scale factor */
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 1.0f;
(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
if (axis_index == 0) {
val = accel_scale.x_scale;
} else if (axis_index == 1) {
val = accel_scale.y_scale;
} else if (axis_index == 2) {
val = accel_scale.z_scale;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
param_notify_changes();
}
// Ensure the calibration values used by the driver are at default settings when we are using thermal calibration data
accel_scale.x_offset = 0.f;
accel_scale.y_offset = 0.f;
accel_scale.z_offset = 0.f;
accel_scale.x_scale = 1.f;
accel_scale.y_scale = 1.f;
accel_scale.z_scale = 1.f;
}
// save the driver level calibration data
(void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index])));
if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
return PX4_ERROR;
}
#if 1 // TODO: replace all IOCTL usage
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, uorb_index);
fd = px4_open(str, 0);
if (fd < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = PX4_ERROR;
// calibration device ID
sprintf(str, "CAL_ACC%u_ID", uorb_index);
param_set_no_notification(param_find(str), &device_id[uorb_index]);
} else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
}
char str[30] {};
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG);
}
// reset calibration offsets
sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
param_reset(param_find(str));
sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
param_reset(param_find(str));
sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
param_reset(param_find(str));
#endif
// reset calibration scale
sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
param_reset(param_find(str));
sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
param_reset(param_find(str));
sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
param_reset(param_find(str));
// reset calibration device ID
sprintf(str, "CAL_ACC%u_ID", uorb_index);
param_reset(param_find(str));
}
}
param_notify_changes();
if (res == PX4_OK) {
/* if there is a any preflight-check system response, let the barrage of messages through */
px4_usleep(200000);
@@ -464,8 +330,7 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
detect_orientation_str(orientation));
read_accelerometer_avg(worker_data->sensor_correction_sub, worker_data->subs, worker_data->accel_ref, orientation,
samples_num);
read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num);
calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]",
detect_orientation_str(orientation),
@@ -479,126 +344,28 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
return calibrate_return_ok;
}
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)
static calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub,
Vector3f(&accel_offs)[MAX_ACCEL_SENS], Matrix3f(&accel_T)[MAX_ACCEL_SENS], unsigned active_sensors)
{
calibrate_return result = calibrate_return_ok;
*active_sensors = 0;
accel_worker_data_t worker_data;
accel_worker_data_t worker_data{};
worker_data.mavlink_log_pub = mavlink_log_pub;
worker_data.done_count = 0;
bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };
// Initialise sub to sensor thermal compensation data
worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
// Initialize subs to error condition so we know which ones are open and which are not
for (size_t i = 0; i < max_accel_sens; i++) {
worker_data.subs[i] = -1;
}
uint64_t timestamps[max_accel_sens] = {};
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned orb_accel_count = orb_group_count(ORB_ID(sensor_accel));
// Warn that we will not calibrate more than max_accels accelerometers
if (orb_accel_count > max_accel_sens) {
calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count,
max_accel_sens);
}
for (unsigned cur_accel = 0; cur_accel < orb_accel_count && cur_accel < max_accel_sens; cur_accel++) {
// Lock in to correct ORB instance
bool found_cur_accel = false;
for (unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) {
worker_data.subs[cur_accel] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
sensor_accel_s report = {};
orb_copy(ORB_ID(sensor_accel), worker_data.subs[cur_accel], &report);
#if 1 // TODO: replace all IOCTL usage
// For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL
// and match it up with the one from the uORB subscription, because the
// instance ordering of uORB and the order of the FDs may not be the same.
if (report.device_id == (uint32_t)device_id[cur_accel]) {
// Device IDs match, correct ORB instance for this accel
found_cur_accel = true;
// store initial timestamp - used to infer which sensors are active
timestamps[cur_accel] = report.timestamp;
} else {
orb_unsubscribe(worker_data.subs[cur_accel]);
}
#else
// For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report.
device_id[cur_accel] = report.device_id;
found_cur_accel = true;
#endif
}
if (!found_cur_accel) {
calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]);
result = calibrate_return_error;
break;
}
if (device_id[cur_accel] != 0) {
// Get priority
ORB_PRIO prio = ORB_PRIO_UNINITIALIZED;
orb_priority(worker_data.subs[cur_accel], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_id[cur_accel];
}
} else {
calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel);
result = calibrate_return_error;
break;
}
}
bool data_collected[detect_orientation_side_count] {};
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data,
false /* normal still */);
false);
calibrate_cancel_unsubscribe(cancel_sub);
}
/* close all subscriptions */
for (unsigned i = 0; i < max_accel_sens; i++) {
if (worker_data.subs[i] >= 0) {
/* figure out which sensors were active */
sensor_accel_s arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
(*active_sensors)++;
}
px4_close(worker_data.subs[i]);
}
}
orb_unsubscribe(worker_data.sensor_correction_sub);
if (result == calibrate_return_ok) {
/* calculate offsets and transform matrix */
for (unsigned i = 0; i < (*active_sensors); i++) {
result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
for (unsigned i = 0; i < active_sensors; i++) {
result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs);
if (result != calibrate_return_ok) {
calibration_log_critical(mavlink_log_pub, "ERROR: calibration calculation error");
@@ -613,93 +380,69 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
/*
* Read specified number of accelerometer samples, calculate average and dispersion.
*/
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)
static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3],
unsigned orient, unsigned samples_num)
{
/* get total sensor board rotation matrix */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
param_t board_offset_x = param_find("SENS_BOARD_X_OFF");
param_t board_offset_y = param_find("SENS_BOARD_Y_OFF");
param_t board_offset_z = param_find("SENS_BOARD_Z_OFF");
float board_offset[3] {};
param_get(param_find("SENS_BOARD_X_OFF"), &board_offset[0]);
param_get(param_find("SENS_BOARD_Y_OFF"), &board_offset[1]);
param_get(param_find("SENS_BOARD_Z_OFF"), &board_offset[2]);
float board_offset[3];
param_get(board_offset_x, &board_offset[0]);
param_get(board_offset_y, &board_offset[1]);
param_get(board_offset_z, &board_offset[2]);
const Dcmf board_rotation_offset{Eulerf{math::radians(board_offset[0]), math::radians(board_offset[1]), math::radians(board_offset[2])}};
Dcmf board_rotation_offset = Eulerf(
M_DEG_TO_RAD_F * board_offset[0],
M_DEG_TO_RAD_F * board_offset[1],
M_DEG_TO_RAD_F * board_offset[2]);
int32_t board_rotation_int = 0;
param_get(param_find("SENS_BOARD_ROT"), &board_rotation_int);
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
const Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int);
Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int);
px4_pollfd_struct_t fds[max_accel_sens];
for (unsigned i = 0; i < max_accel_sens; i++) {
fds[i].fd = subs[i];
fds[i].events = POLLIN;
}
unsigned counts[max_accel_sens] = { 0 };
float accel_sum[max_accel_sens][3] {};
Vector3f accel_sum[MAX_ACCEL_SENS] {};
unsigned counts[MAX_ACCEL_SENS] {};
unsigned errcount = 0;
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
/* try to get latest thermal corrections */
if (orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction) != 0) {
/* use default values */
memset(&sensor_correction, 0, sizeof(sensor_correction));
// sensor thermal corrections
uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
sensor_correction_s sensor_correction{};
sensor_correction_sub.copy(&sensor_correction);
for (unsigned i = 0; i < 3; i++) {
sensor_correction.accel_scale_0[i] = 1.0f;
sensor_correction.accel_scale_1[i] = 1.0f;
sensor_correction.accel_scale_2[i] = 1.0f;
}
}
uORB::SubscriptionBlocking<sensor_accel_s> accel_sub[MAX_ACCEL_SENS] {
{ORB_ID(sensor_accel), 0, 0},
{ORB_ID(sensor_accel), 0, 1},
{ORB_ID(sensor_accel), 0, 2},
};
/* use the first sensor to pace the readout, but do per-sensor counts */
while (counts[0] < samples_num) {
int poll_ret = px4_poll(&fds[0], max_accel_sens, 1000);
if (accel_sub[0].updatedBlocking(100000)) {
for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
sensor_accel_s arp;
if (poll_ret > 0) {
if (accel_sub[accel_index].update(&arp)) {
// fetch optional thermal offset corrections in sensor/board frame
Vector3f offset{0, 0, 0};
sensor_correction_sub.update(&sensor_correction);
for (unsigned s = 0; s < max_accel_sens; s++) {
bool changed;
orb_check(subs[s], &changed);
if (changed) {
sensor_accel_s arp;
orb_copy(ORB_ID(sensor_accel), subs[s], &arp);
// Apply thermal offset corrections in sensor/board frame
if (s == 0) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]);
} else if (s == 1) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]);
} else if (s == 2) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]);
} else {
accel_sum[s][0] += arp.x;
accel_sum[s][1] += arp.y;
accel_sum[s][2] += arp.z;
if (sensor_correction.timestamp > 0 && arp.device_id != 0) {
for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) {
if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) {
switch (correction_index) {
case 0:
offset = Vector3f{sensor_correction.accel_offset_0};
break;
case 1:
offset = Vector3f{sensor_correction.accel_offset_1};
break;
case 2:
offset = Vector3f{sensor_correction.accel_offset_2};
break;
}
}
}
}
counts[s]++;
accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset;
counts[accel_index]++;
}
}
@@ -714,79 +457,44 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m
}
// rotate sensor measurements from sensor to body frame using board rotation matrix
for (unsigned i = 0; i < max_accel_sens; i++) {
Vector3f accel_sum_vec(&accel_sum[i][0]);
accel_sum_vec = board_rotation * accel_sum_vec;
for (size_t j = 0; j < 3; j++) {
accel_sum[i][j] = accel_sum_vec(j);
}
for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
accel_sum[s] = board_rotation * accel_sum[s];
}
for (unsigned s = 0; s < max_accel_sens; s++) {
for (unsigned i = 0; i < 3; i++) {
accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
}
for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
const auto sum = accel_sum[s] / counts[s];
sum.copyTo(accel_avg[s][orient]);
}
return calibrate_return_ok;
}
int mat_invert3(float src[3][3], float dst[3][3])
{
float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
if (fabsf(det) < FLT_EPSILON) {
return PX4_ERROR; // Singular matrix
}
dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det;
dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det;
dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det;
dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
return PX4_OK;
}
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)
static calibrate_return calculate_calibration_values(unsigned sensor,
float (&accel_ref)[MAX_ACCEL_SENS][detect_orientation_side_count][3], Matrix3f(&accel_T)[MAX_ACCEL_SENS],
Vector3f(&accel_offs)[MAX_ACCEL_SENS])
{
/* calculate offsets */
for (unsigned i = 0; i < 3; i++) {
accel_offs[sensor][i] = (accel_ref[sensor][i * 2][i] + accel_ref[sensor][i * 2 + 1][i]) / 2;
accel_offs[sensor](i) = (accel_ref[sensor][i * 2][i] + accel_ref[sensor][i * 2 + 1][i]) / 2;
}
/* fill matrix A for linear equations system*/
float mat_A[3][3];
memset(mat_A, 0, sizeof(mat_A));
Matrix3f mat_A;
for (unsigned i = 0; i < 3; i++) {
for (unsigned j = 0; j < 3; j++) {
float a = accel_ref[sensor][i * 2][j] - accel_offs[sensor][j];
mat_A[i][j] = a;
mat_A(i, j) = accel_ref[sensor][i * 2][j] - accel_offs[sensor](j);
}
}
/* calculate inverse matrix for A */
float mat_A_inv[3][3];
if (mat_invert3(mat_A, mat_A_inv) != PX4_OK) {
return calibrate_return_error;
}
const Matrix3f mat_A_inv = mat_A.I();
/* copy results to accel_T */
for (unsigned i = 0; i < 3; i++) {
for (unsigned j = 0; j < 3; j++) {
/* simplify matrices mult because b has only one non-zero element == g at index i */
accel_T[sensor][j][i] = mat_A_inv[j][i] * g;
accel_T[sensor](j, i) = mat_A_inv(j, i) * CONSTANTS_ONE_G;
}
}
@@ -796,38 +504,31 @@ calibrate_return calculate_calibration_values(unsigned sensor,
int do_level_calibration(orb_advert_t *mavlink_log_pub)
{
bool success = false;
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
vehicle_attitude_s att{};
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level");
param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF");
param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF");
param_t board_rot_handle = param_find("SENS_BOARD_ROT");
// get old values
float roll_offset_current;
float pitch_offset_current;
int32_t board_rot_current = 0;
float roll_offset_current = 0.f;
float pitch_offset_current = 0.f;
param_get(roll_offset_handle, &roll_offset_current);
param_get(pitch_offset_handle, &pitch_offset_current);
param_get(board_rot_handle, &board_rot_current);
Dcmf board_rotation_offset = Eulerf(
math::radians(roll_offset_current),
math::radians(pitch_offset_current),
0.f);
int32_t board_rot_current = 0;
param_get(param_find("SENS_BOARD_ROT"), &board_rot_current);
px4_pollfd_struct_t fds[1];
fds[0].fd = att_sub;
fds[0].events = POLLIN;
const Dcmf board_rotation_offset{Eulerf{radians(roll_offset_current), radians(pitch_offset_current), 0.f}};
float roll_mean = 0.0f;
float pitch_mean = 0.0f;
float roll_mean = 0.f;
float pitch_mean = 0.f;
unsigned counter = 0;
bool had_motion = true;
int num_retries = 0;
uORB::SubscriptionBlocking<vehicle_attitude_s> att_sub{ORB_ID(vehicle_attitude)};
while (had_motion && num_retries++ < 50) {
Vector2f min_angles{100.f, 100.f};
Vector2f max_angles{-100.f, -100.f};
@@ -839,9 +540,10 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub)
const hrt_abstime start = hrt_absolute_time();
while (hrt_elapsed_time(&start) < calibration_duration) {
int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
if (pollret <= 0) {
vehicle_attitude_s att{};
if (!att_sub.updateBlocking(att, 100000)) {
// attitude estimator is not running
calibration_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
@@ -855,8 +557,7 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub)
last_progress_report = progress;
}
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
Eulerf att_euler = Quatf(att.q);
Eulerf att_euler{Quatf{att.q}};
// keep min + max angles
for (int i = 0; i < 2; ++i) {
@@ -866,7 +567,8 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub)
}
att_euler(2) = 0.f; // ignore yaw
att_euler = Eulerf(board_rotation_offset * Dcmf(att_euler)); // subtract existing board rotation
att_euler = Eulerf{board_rotation_offset *Dcmf{att_euler}}; // subtract existing board rotation
roll_mean += att_euler.phi();
pitch_mean += att_euler.theta();
++counter;
@@ -876,6 +578,7 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub)
// The difference is typically <0.1 deg while at rest
if (max_angles(0) - min_angles(0) < math::radians(0.5f) &&
max_angles(1) - min_angles(1) < math::radians(0.5f)) {
had_motion = false;
}
}
@@ -895,18 +598,16 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub)
calibration_log_critical(mavlink_log_pub, "excess pitch angle");
} else {
roll_mean *= (float)M_RAD_TO_DEG;
pitch_mean *= (float)M_RAD_TO_DEG;
param_set_no_notification(roll_offset_handle, &roll_mean);
param_set_no_notification(pitch_offset_handle, &pitch_mean);
float roll_mean_degrees = math::degrees(roll_mean);
float pitch_mean_degrees = math::degrees(pitch_mean);
param_set_no_notification(roll_offset_handle, &roll_mean_degrees);
param_set_no_notification(pitch_offset_handle, &pitch_mean_degrees);
param_notify_changes();
success = true;
}
out:
orb_unsubscribe(att_sub);
if (success) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
return 0;
src/modules/commander/calibration_routines.cpp
+28 -48
View File
@@ -41,26 +41,25 @@
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include <float.h>
#include <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h>
#include <drivers/drv_tone_alarm.h>
#include <lib/ecl/geo/geo.h>
#include <string.h>
#include <mathlib/mathlib.h>
#include <lib/mathlib/mathlib.h>
#include <lib/systemlib/mavlink_log.h>
#include <matrix/math.hpp>
#include <uORB/topics/vehicle_command.h>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_tone_alarm.h>
#include <uORB/topics/vehicle_command.h>
#include "calibration_routines.h"
#include "calibration_messages.h"
#include "commander_helper.h"
using namespace time_literals;
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
float *sphere_radius)
@@ -523,28 +522,22 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
}
}
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub,
bool lenient_still_position)
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, bool lenient_still_position)
{
static constexpr unsigned ndim = 3;
float accel_ema[ndim] = { 0.0f }; // exponential moving average of accel
float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; // max-hold dispersion of accel
static constexpr float ema_len = 0.5f; // EMA time constant in seconds
static constexpr float normal_still_thr = 0.25; // normal still threshold
float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
static constexpr float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2
const hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us
float accel_ema[ndim] {}; // exponential moving average of accel
float accel_disp[3] {}; // max-hold dispersion of accel
static constexpr float ema_len = 0.5f; // EMA time constant in seconds
static constexpr float normal_still_thr = 0.25; // normal still threshold
float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
static constexpr float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2
const hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us
px4_pollfd_struct_t fds[1];
fds[0].fd = accel_sub;
fds[0].events = POLLIN;
/* set timeout to 90s */
static constexpr hrt_abstime timeout = 90_s;
const hrt_abstime t_start = hrt_absolute_time();
/* set timeout to 30s */
static constexpr hrt_abstime timeout = 90000000;
hrt_abstime t_timeout = t_start + timeout;
hrt_abstime t = t_start;
hrt_abstime t_prev = t_start;
@@ -552,13 +545,13 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
unsigned poll_errcount = 0;
while (true) {
/* wait blocking for new data */
int poll_ret = px4_poll(fds, 1, 1000);
// Setup subscriptions to onboard accel sensor
uORB::SubscriptionBlocking<sensor_combined_s> sensor_sub{ORB_ID(sensor_combined)};
if (poll_ret) {
struct sensor_combined_s sensor;
orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor);
while (true) {
sensor_combined_s sensor;
if (sensor_sub.updateBlocking(sensor, 100000)) {
t = hrt_absolute_time();
float dt = (t - t_prev) / 1000000.0f;
t_prev = t;
@@ -586,6 +579,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
if (accel_disp[0] < still_thr2 &&
accel_disp[1] < still_thr2 &&
accel_disp[2] < still_thr2) {
/* is still now */
if (t_still == 0) {
/* first time */
@@ -612,7 +606,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
}
}
} else if (poll_ret == 0) {
} else {
poll_errcount++;
}
@@ -691,15 +685,6 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
{
calibrate_return result = calibrate_return_ok;
// Setup subscriptions to onboard accel sensor
int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
if (sub_accel < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
unsigned orientation_failures = 0;
// Rotate through all requested orientation
@@ -744,8 +729,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
px4_usleep(20000);
calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
px4_usleep(20000);
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel,
lenient_still_position);
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, lenient_still_position);
if (orient == DETECT_ORIENTATION_ERROR) {
orientation_failures++;
@@ -792,10 +776,6 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
px4_usleep(200000);
}
if (sub_accel >= 0) {
px4_close(sub_accel);
}
return result;
}
src/modules/commander/calibration_routines.h
-4
View File
@@ -72,9 +72,6 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
bool inverse4x4(float m[], float invOut[]);
bool mat_inverse(float *A, float *inv, uint8_t n);
// FIXME: Change the name
static const unsigned max_accel_sens = 3;
// The order of these cannot change since the calibration calculations depend on them in this order
enum detect_orientation_return {
DETECT_ORIENTATION_TAIL_DOWN,
@@ -92,7 +89,6 @@ static const unsigned detect_orientation_side_count = 6;
/// and ready for measurements
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
int accel_sub, ///< Orb subcription to accel sensor
bool lenient_still_detection); ///< true: Use more lenient still position detection
/// Returns the human readable string representation of the orientation
src/modules/commander/gyro_calibration.cpp
+126 -311
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -46,32 +46,27 @@
#include <px4_platform_common/posix.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/time.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <math.h>
#include <mathlib/mathlib.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_correction.h>
#include <drivers/drv_gyro.h>
#include <systemlib/mavlink_log.h>
#include <parameters/param.h>
#include <systemlib/err.h>
#include <uORB/topics/sensor_gyro.h>
static const char *sensor_name = "gyro";
static constexpr char sensor_name[] {"gyro"};
static constexpr unsigned MAX_GYROS = 3;
static constexpr unsigned max_gyros = 3;
using matrix::Vector3f;
/// Data passed to calibration worker routine
struct gyro_worker_data_t {
orb_advert_t *mavlink_log_pub;
int32_t device_id[max_gyros];
int gyro_sensor_sub[max_gyros];
int sensor_correction_sub;
struct gyro_calibration_s gyro_scale[max_gyros];
orb_advert_t *mavlink_log_pub{nullptr};
int32_t device_id[MAX_GYROS] {};
Vector3f offset[MAX_GYROS] {};
static constexpr int last_num_samples = 9; ///< number of samples for the motion detection median filter
float last_sample_0_x[last_num_samples];
@@ -89,112 +84,88 @@ static int float_cmp(const void *elem1, const void *elem2)
return *(const float *)elem1 > *(const float *)elem2;
}
static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data_t &worker_data)
{
gyro_worker_data_t *worker_data = (gyro_worker_data_t *)(data);
unsigned calibration_counter[max_gyros] = { 0 }, slow_count = 0;
const unsigned calibration_count = 250;
sensor_gyro_s gyro_report;
unsigned poll_errcount = 0;
unsigned calibration_counter[MAX_GYROS] {};
static constexpr unsigned CALIBRATION_COUNT = 250;
unsigned poll_errcount = 0;
struct sensor_correction_s sensor_correction {}; /**< sensor thermal corrections */
uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */
if (orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction) != 0) {
for (unsigned i = 0; i < 3; i++) {
sensor_correction.gyro_scale_0[i] = 1.0f;
sensor_correction.gyro_scale_1[i] = 1.0f;
sensor_correction.gyro_scale_2[i] = 1.0f;
}
}
uORB::SubscriptionBlocking<sensor_gyro_s> gyro_sub[MAX_GYROS] {
{ORB_ID(sensor_gyro), 0, 0},
{ORB_ID(sensor_gyro), 0, 1},
{ORB_ID(sensor_gyro), 0, 2},
};
px4_pollfd_struct_t fds[max_gyros];
for (unsigned s = 0; s < max_gyros; s++) {
fds[s].fd = worker_data->gyro_sensor_sub[s];
fds[s].events = POLLIN;
}
memset(&worker_data->last_sample_0_x, 0, sizeof(worker_data->last_sample_0_x));
memset(&worker_data->last_sample_0_y, 0, sizeof(worker_data->last_sample_0_y));
memset(&worker_data->last_sample_0_z, 0, sizeof(worker_data->last_sample_0_z));
worker_data->last_sample_0_idx = 0;
memset(&worker_data.last_sample_0_x, 0, sizeof(worker_data.last_sample_0_x));
memset(&worker_data.last_sample_0_y, 0, sizeof(worker_data.last_sample_0_y));
memset(&worker_data.last_sample_0_z, 0, sizeof(worker_data.last_sample_0_z));
worker_data.last_sample_0_idx = 0;
/* use slowest gyro to pace, but count correctly per-gyro for statistics */
while (slow_count < calibration_count) {
if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
unsigned slow_count = 0;
while (slow_count < CALIBRATION_COUNT) {
if (calibrate_cancel_check(worker_data.mavlink_log_pub, cancel_sub)) {
return calibrate_return_cancelled;
}
/* check if there are new thermal corrections */
bool updated;
orb_check(worker_data->sensor_correction_sub, &updated);
if (gyro_sub[0].updatedBlocking(100000)) {
unsigned update_count = CALIBRATION_COUNT;
if (updated) {
orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction);
}
int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
if (poll_ret > 0) {
unsigned update_count = calibration_count;
for (unsigned s = 0; s < max_gyros; s++) {
if (calibration_counter[s] >= calibration_count) {
for (unsigned gyro_index = 0; gyro_index < MAX_GYROS; gyro_index++) {
if (calibration_counter[gyro_index] >= CALIBRATION_COUNT) {
// Skip if instance has enough samples
continue;
}
bool changed;
orb_check(worker_data->gyro_sensor_sub[s], &changed);
sensor_gyro_s gyro_report;
if (changed) {
orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report);
float sample[3];
if (gyro_sub[gyro_index].update(&gyro_report)) {
if (s == 0) {
// take a working copy
sample[0] = (gyro_report.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0];
sample[1] = (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1];
sample[2] = (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2];
worker_data->last_sample_0_x[worker_data->last_sample_0_idx] = sample[0];
worker_data->last_sample_0_y[worker_data->last_sample_0_idx] = sample[1];
worker_data->last_sample_0_z[worker_data->last_sample_0_idx] = sample[2];
worker_data->last_sample_0_idx = (worker_data->last_sample_0_idx + 1) % gyro_worker_data_t::last_num_samples;
} else if (s == 1) {
sample[0] = (gyro_report.x - sensor_correction.gyro_offset_1[0]) * sensor_correction.gyro_scale_1[0];
sample[1] = (gyro_report.y - sensor_correction.gyro_offset_1[1]) * sensor_correction.gyro_scale_1[1];
sample[2] = (gyro_report.z - sensor_correction.gyro_offset_1[2]) * sensor_correction.gyro_scale_1[2];
} else if (s == 2) {
sample[0] = (gyro_report.x - sensor_correction.gyro_offset_2[0]) * sensor_correction.gyro_scale_2[0];
sample[1] = (gyro_report.y - sensor_correction.gyro_offset_2[1]) * sensor_correction.gyro_scale_2[1];
sample[2] = (gyro_report.z - sensor_correction.gyro_offset_2[2]) * sensor_correction.gyro_scale_2[2];
} else {
sample[0] = gyro_report.x;
sample[1] = gyro_report.y;
sample[2] = gyro_report.z;
// fetch optional thermal offset corrections in sensor/board frame
Vector3f offset{0, 0, 0};
sensor_correction_sub.update(&sensor_correction);
if (sensor_correction.timestamp > 0 && gyro_report.device_id != 0) {
for (uint8_t correction_index = 0; correction_index < MAX_GYROS; correction_index++) {
if (sensor_correction.gyro_device_ids[correction_index] == gyro_report.device_id) {
switch (correction_index) {
case 0:
offset = Vector3f{sensor_correction.gyro_offset_0};
break;
case 1:
offset = Vector3f{sensor_correction.gyro_offset_1};
break;
case 2:
offset = Vector3f{sensor_correction.gyro_offset_2};
break;
}
}
}
}
worker_data->gyro_scale[s].x_offset += sample[0];
worker_data->gyro_scale[s].y_offset += sample[1];
worker_data->gyro_scale[s].z_offset += sample[2];
calibration_counter[s]++;
worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - offset;
calibration_counter[gyro_index]++;
if (gyro_index == 0) {
worker_data.last_sample_0_x[worker_data.last_sample_0_idx] = gyro_report.x - offset(0);
worker_data.last_sample_0_y[worker_data.last_sample_0_idx] = gyro_report.y - offset(1);
worker_data.last_sample_0_z[worker_data.last_sample_0_idx] = gyro_report.z - offset(2);
worker_data.last_sample_0_idx = (worker_data.last_sample_0_idx + 1) % gyro_worker_data_t::last_num_samples;
}
}
// Maintain the sample count of the slowest sensor
if (calibration_counter[s] && calibration_counter[s] < update_count) {
update_count = calibration_counter[s];
if (calibration_counter[gyro_index] && calibration_counter[gyro_index] < update_count) {
update_count = calibration_counter[gyro_index];
}
}
if (update_count % (calibration_count / 20) == 0) {
calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (update_count * 100) / calibration_count);
if (update_count % (CALIBRATION_COUNT / 20) == 0) {
calibration_log_info(worker_data.mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (update_count * 100) / CALIBRATION_COUNT);
}
// Propagate out the slowest sensor's count
@@ -207,20 +178,18 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
}
if (poll_errcount > 1000) {
calibration_log_critical(worker_data->mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
calibration_log_critical(worker_data.mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
return calibrate_return_error;
}
}
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
calibration_log_critical(worker_data->mavlink_log_pub, "ERROR: missing data, sensor %d", s)
for (unsigned s = 0; s < MAX_GYROS; s++) {
if ((worker_data.device_id[s] != 0) && (calibration_counter[s] < CALIBRATION_COUNT / 2)) {
calibration_log_critical(worker_data.mavlink_log_pub, "ERROR: missing data, sensor %d", s)
return calibrate_return_error;
}
worker_data->gyro_scale[s].x_offset /= calibration_counter[s];
worker_data->gyro_scale[s].y_offset /= calibration_counter[s];
worker_data->gyro_scale[s].z_offset /= calibration_counter[s];
worker_data.offset[s] /= calibration_counter[s];
}
return calibrate_return_ok;
@@ -228,131 +197,35 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
{
int res = PX4_OK;
gyro_worker_data_t worker_data = {};
int res = PX4_OK;
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
gyro_worker_data_t worker_data{};
worker_data.mavlink_log_pub = mavlink_log_pub;
gyro_calibration_s gyro_scale_zero{};
int device_prio_max = 0;
enum ORB_PRIO device_prio_max = ORB_PRIO_UNINITIALIZED;
int32_t device_id_primary = 0;
worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
for (unsigned s = 0; s < max_gyros; s++) {
char str[30];
// Reset gyro ids to unavailable.
worker_data.device_id[s] = 0;
// And set default subscriber values.
worker_data.gyro_sensor_sub[s] = -1;
(void)sprintf(str, "CAL_GYRO%u_ID", s);
res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, "Unable to reset CAL_GYRO%u_ID", s);
return PX4_ERROR;
}
// Reset all offsets to 0
(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
#if 1 // TODO: replace all IOCTL usage
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = px4_open(str, 0);
if (fd >= 0) {
worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
px4_close(fd);
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
return PX4_ERROR;
}
}
#else
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
param_notify_changes();
#endif
}
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned orb_gyro_count = orb_group_count(ORB_ID(sensor_gyro));
// Warn that we will not calibrate more than max_gyros gyroscopes
if (orb_gyro_count > max_gyros) {
calibration_log_critical(mavlink_log_pub, "Detected %u gyros, but will calibrate only %u", orb_gyro_count, max_gyros);
// Warn that we will not calibrate more than MAX_GYROS gyroscopes
if (orb_gyro_count > MAX_GYROS) {
calibration_log_critical(mavlink_log_pub, "Detected %u gyros, but will calibrate only %u", orb_gyro_count, MAX_GYROS);
}
for (unsigned cur_gyro = 0; cur_gyro < orb_gyro_count && cur_gyro < max_gyros; cur_gyro++) {
for (uint8_t cur_gyro = 0; cur_gyro < orb_gyro_count && cur_gyro < MAX_GYROS; cur_gyro++) {
// Lock in to correct ORB instance
bool found_cur_gyro = false;
uORB::Subscription gyro_sensor_sub{ORB_ID(sensor_gyro), cur_gyro};
sensor_gyro_s report{};
gyro_sensor_sub.copy(&report);
for (unsigned i = 0; i < orb_gyro_count && !found_cur_gyro; i++) {
worker_data.gyro_sensor_sub[cur_gyro] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
sensor_gyro_s report{};
orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[cur_gyro], &report);
#if 1 // TODO: replace all IOCTL usage
// For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL
// and match it up with the one from the uORB subscription, because the
// instance ordering of uORB and the order of the FDs may not be the same.
if (report.device_id == (uint32_t)worker_data.device_id[cur_gyro]) {
// Device IDs match, correct ORB instance for this gyro
found_cur_gyro = true;
} else {
orb_unsubscribe(worker_data.gyro_sensor_sub[cur_gyro]);
}
#else
// For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report.
worker_data.device_id[cur_gyro] = report.device_id;
found_cur_gyro = true;
#endif
}
if (!found_cur_gyro) {
calibration_log_critical(mavlink_log_pub, "Gyro #%u (ID %u) no matching uORB devid", cur_gyro,
worker_data.device_id[cur_gyro]);
res = calibrate_return_error;
break;
}
worker_data.device_id[cur_gyro] = report.device_id;
if (worker_data.device_id[cur_gyro] != 0) {
// Get priority
ORB_PRIO prio = ORB_PRIO_UNINITIALIZED;
orb_priority(worker_data.gyro_sensor_sub[cur_gyro], &prio);
enum ORB_PRIO prio = gyro_sensor_sub.get_priority();
if (prio > device_prio_max) {
device_prio_max = prio;
@@ -364,7 +237,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
}
}
int cancel_sub = calibrate_cancel_subscribe();
int cancel_sub = calibrate_cancel_subscribe();
unsigned try_count = 0;
unsigned max_tries = 20;
@@ -372,7 +245,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
do {
// Calibrate gyro and ensure user didn't move
calibrate_return cal_return = gyro_calibration_worker(cancel_sub, &worker_data);
calibrate_return cal_return = gyro_calibration_worker(cancel_sub, worker_data);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already sent, we are done here
@@ -387,22 +260,18 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
qsort(worker_data.last_sample_0_x, gyro_worker_data_t::last_num_samples, sizeof(float), float_cmp);
qsort(worker_data.last_sample_0_y, gyro_worker_data_t::last_num_samples, sizeof(float), float_cmp);
qsort(worker_data.last_sample_0_z, gyro_worker_data_t::last_num_samples, sizeof(float), float_cmp);
float xdiff = worker_data.last_sample_0_x[gyro_worker_data_t::last_num_samples / 2] -
worker_data.gyro_scale[0].x_offset;
float ydiff = worker_data.last_sample_0_y[gyro_worker_data_t::last_num_samples / 2] -
worker_data.gyro_scale[0].y_offset;
float zdiff = worker_data.last_sample_0_z[gyro_worker_data_t::last_num_samples / 2] -
worker_data.gyro_scale[0].z_offset;
float xdiff = worker_data.last_sample_0_x[gyro_worker_data_t::last_num_samples / 2] - worker_data.offset[0](0);
float ydiff = worker_data.last_sample_0_y[gyro_worker_data_t::last_num_samples / 2] - worker_data.offset[0](1);
float zdiff = worker_data.last_sample_0_z[gyro_worker_data_t::last_num_samples / 2] - worker_data.offset[0](2);
/* maximum allowable calibration error */
const float maxoff = math::radians(0.6f);
if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) ||
!PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) ||
!PX4_ISFINITE(worker_data.gyro_scale[0].z_offset) ||
fabsf(xdiff) > maxoff ||
fabsf(ydiff) > maxoff ||
fabsf(zdiff) > maxoff) {
if (!PX4_ISFINITE(worker_data.offset[0](0)) ||
!PX4_ISFINITE(worker_data.offset[0](1)) ||
!PX4_ISFINITE(worker_data.offset[0](2)) ||
fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) {
calibration_log_critical(mavlink_log_pub, "motion, retrying..");
res = PX4_ERROR;
@@ -423,98 +292,44 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
calibrate_cancel_unsubscribe(cancel_sub);
for (unsigned s = 0; s < max_gyros; s++) {
px4_close(worker_data.gyro_sensor_sub[s]);
}
if (res == PX4_OK) {
/* set offset parameters to new values */
bool failed = false;
bool failed = (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &device_id_primary));
failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary)));
for (unsigned uorb_index = 0; uorb_index < MAX_GYROS; uorb_index++) {
bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator
char str[30] {};
for (unsigned uorb_index = 0; uorb_index < max_gyros; uorb_index++) {
if (worker_data.device_id[uorb_index] != 0) {
char str[30];
if (uorb_index < orb_gyro_count) {
float x_offset = worker_data.offset[uorb_index](0);
sprintf(str, "CAL_GYRO%u_XOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &x_offset));
/* check if thermal compensation is enabled */
int32_t tc_enabled_int;
param_get(param_find("TC_G_ENABLE"), &(tc_enabled_int));
float y_offset = worker_data.offset[uorb_index](1);
sprintf(str, "CAL_GYRO%u_YOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &y_offset));
if (tc_enabled_int == 1) {
/* Get struct containing sensor thermal compensation data */
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
memset(&sensor_correction, 0, sizeof(sensor_correction));
orb_copy(ORB_ID(sensor_correction), worker_data.sensor_correction_sub, &sensor_correction);
float z_offset = worker_data.offset[uorb_index](2);
sprintf(str, "CAL_GYRO%u_ZOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &z_offset));
/* don't allow a parameter instance to be calibrated again by another uORB instance */
if (!tc_locked[sensor_correction.gyro_mapping[uorb_index]]) {
tc_locked[sensor_correction.gyro_mapping[uorb_index]] = true;
int32_t device_id = worker_data.device_id[uorb_index];
sprintf(str, "CAL_GYRO%u_ID", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &device_id));
/* update the _X0_ terms to include the additional offset */
int32_t handle;
float val;
} else {
// reset unused calibration offsets
sprintf(str, "CAL_GYRO%u_XOFF", uorb_index);
param_reset(param_find(str));
sprintf(str, "CAL_GYRO%u_YOFF", uorb_index);
param_reset(param_find(str));
sprintf(str, "CAL_GYRO%u_ZOFF", uorb_index);
param_reset(param_find(str));
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 0.0f;
(void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index);
handle = param_find(str);
param_get(handle, &val);
if (axis_index == 0) {
val += worker_data.gyro_scale[uorb_index].x_offset;
} else if (axis_index == 1) {
val += worker_data.gyro_scale[uorb_index].y_offset;
} else if (axis_index == 2) {
val += worker_data.gyro_scale[uorb_index].z_offset;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
param_notify_changes();
}
// Ensure the calibration values used the driver are at default settings
worker_data.gyro_scale[uorb_index].x_offset = 0.f;
worker_data.gyro_scale[uorb_index].y_offset = 0.f;
worker_data.gyro_scale[uorb_index].z_offset = 0.f;
}
(void)sprintf(str, "CAL_GYRO%u_XOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].x_offset)));
(void)sprintf(str, "CAL_GYRO%u_YOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].y_offset)));
(void)sprintf(str, "CAL_GYRO%u_ZOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].z_offset)));
(void)sprintf(str, "CAL_GYRO%u_ID", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.device_id[uorb_index])));
#if 1 // TODO: replace all IOCTL usage
/* apply new scaling and offsets */
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, uorb_index);
int fd = px4_open(str, 0);
if (fd < 0) {
failed = true;
continue;
}
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[uorb_index]);
px4_close(fd);
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG);
}
#endif
// reset unused calibration device ID
sprintf(str, "CAL_GYRO%u_ID", uorb_index);
param_reset(param_find(str));
}
}
@@ -524,6 +339,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
}
}
param_notify_changes();
/* if there is a any preflight-check system response, let the barrage of messages through */
px4_usleep(200000);
@@ -534,8 +351,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
orb_unsubscribe(worker_data.sensor_correction_sub);
/* give this message enough time to propagate */
px4_usleep(600000);
src/modules/commander/mag_calibration.cpp
+1 -3
View File
@@ -52,14 +52,12 @@
#include <math.h>
#include <fcntl.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_tone_alarm.h>
#include <systemlib/mavlink_log.h>
#include <parameters/param.h>
#include <systemlib/err.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_gyro.h>
static const char *sensor_name = "mag";
static constexpr unsigned max_mags = 4;