From 4f850c7cd0662d5f13d5514150b0d50a6e5e99c7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 16 May 2021 14:22:41 -0400 Subject: [PATCH] sensors: preserve all valid calibration parameters even if sensor currently missing - mark calibration slots active for first N sensors found - calibration procedure don't bother resetting unused slots --- src/lib/sensor_calibration/Accelerometer.cpp | 13 ++- src/lib/sensor_calibration/Gyroscope.cpp | 13 ++- src/lib/sensor_calibration/Magnetometer.cpp | 12 ++- .../commander/accelerometer_calibration.cpp | 18 ++-- src/modules/commander/gyro_calibration.cpp | 23 +++-- src/modules/commander/mag_calibration.cpp | 25 +++--- src/modules/sensors/sensors.cpp | 84 +++++++++++++++++-- 7 files changed, 125 insertions(+), 63 deletions(-) diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index 3f1210f3aa..08233d15fd 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/src/lib/sensor_calibration/Accelerometer.cpp @@ -59,6 +59,11 @@ void Accelerometer::set_device_id(uint32_t device_id, bool external) if (_device_id != device_id || _external != external) { set_external(external); _device_id = device_id; + + if (_device_id != 0) { + _calibration_index = FindCalibrationIndex(SensorString(), _device_id); + } + ParametersUpdate(); SensorCorrectionsUpdate(true); } @@ -161,13 +166,6 @@ void Accelerometer::set_rotation(Rotation rotation) void Accelerometer::ParametersUpdate() { - if (_device_id == 0) { - Reset(); - return; - } - - _calibration_index = FindCalibrationIndex(SensorString(), _device_id); - if (_calibration_index >= 0) { // CAL_ACCx_ROT @@ -234,6 +232,7 @@ void Accelerometer::Reset() _offset.zero(); _scale = Vector3f{1.f, 1.f, 1.f}; + _thermal_offset.zero(); _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index a198bda9f1..861929622f 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor_calibration/Gyroscope.cpp @@ -59,6 +59,11 @@ void Gyroscope::set_device_id(uint32_t device_id, bool external) if (_device_id != device_id || _external != external) { set_external(external); _device_id = device_id; + + if (_device_id != 0) { + _calibration_index = FindCalibrationIndex(SensorString(), _device_id); + } + ParametersUpdate(); SensorCorrectionsUpdate(true); } @@ -146,13 +151,6 @@ void Gyroscope::set_rotation(Rotation rotation) void Gyroscope::ParametersUpdate() { - if (_device_id == 0) { - Reset(); - return; - } - - _calibration_index = FindCalibrationIndex(SensorString(), _device_id); - if (_calibration_index >= 0) { // CAL_GYROx_ROT @@ -215,6 +213,7 @@ void Gyroscope::Reset() } _offset.zero(); + _thermal_offset.zero(); _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index 57fc73c244..f5df47261d 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -59,6 +59,11 @@ void Magnetometer::set_device_id(uint32_t device_id, bool external) if (_device_id != device_id || _external != external) { set_external(external); _device_id = device_id; + + if (_device_id != 0) { + _calibration_index = FindCalibrationIndex(SensorString(), _device_id); + } + ParametersUpdate(); } } @@ -146,13 +151,6 @@ void Magnetometer::set_rotation(Rotation rotation) void Magnetometer::ParametersUpdate() { - if (_device_id == 0) { - Reset(); - return; - } - - _calibration_index = FindCalibrationIndex(SensorString(), _device_id); - if (_calibration_index >= 0) { // CAL_MAGx_ROT diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index b90832ab98..763470da4f 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -418,17 +418,17 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) accel_T_rotated.print(); #endif // DEBUD_BUILD calibrations[i].PrintStatus(); - } - // save all calibrations including empty slots - if (calibrations[i].ParametersSave()) { - param_save = true; - failed = false; - } else { - failed = true; - calibration_log_critical(mavlink_log_pub, "calibration save failed"); - break; + if (calibrations[i].ParametersSave()) { + param_save = true; + failed = false; + + } else { + failed = true; + calibration_log_critical(mavlink_log_pub, "calibration save failed"); + break; + } } } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index b7fc2f83c7..3187feae37 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -260,22 +260,19 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) if (calibration.device_id() != 0) { calibration.set_offset(worker_data.offset[uorb_index]); + calibration.set_calibration_index(uorb_index); + calibration.PrintStatus(); - } else { - calibration.Reset(); - } + if (calibration.ParametersSave()) { + param_save = true; + failed = false; - calibration.set_calibration_index(uorb_index); - - if (calibration.ParametersSave()) { - param_save = true; - failed = false; - - } else { - failed = true; - calibration_log_critical(mavlink_log_pub, "calibration save failed"); - break; + } else { + failed = true; + calibration_log_critical(mavlink_log_pub, "calibration save failed"); + break; + } } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index ff6e8855d7..810bb4a625 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -897,22 +897,19 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma current_cal.set_offdiagonal(offdiag[cur_mag]); } + current_cal.set_calibration_index(cur_mag); + current_cal.PrintStatus(); - } else { - current_cal.Reset(); - } + if (current_cal.ParametersSave()) { + param_save = true; + failed = false; - current_cal.set_calibration_index(cur_mag); - - if (current_cal.ParametersSave()) { - param_save = true; - failed = false; - - } else { - failed = true; - calibration_log_critical(mavlink_log_pub, "calibration save failed"); - break; + } else { + failed = true; + calibration_log_critical(mavlink_log_pub, "calibration save failed"); + break; + } } } @@ -1000,7 +997,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian sensor_mag_s mag{}; mag_sub.copy(&mag); - if (mag_sub.advertised() && (mag.timestamp != 0)) { + if (mag_sub.advertised() && (mag.timestamp != 0) && (mag.device_id != 0)) { calibration::Magnetometer cal{mag.device_id, mag.is_external}; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8cf3faa1f8..28afeb746d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2021 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 @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -179,6 +180,12 @@ private: VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; + uint8_t _n_accel{0}; + uint8_t _n_baro{0}; + uint8_t _n_gps{0}; + uint8_t _n_gyro{0}; + uint8_t _n_mag{0}; + /** * Update our local parameter cache. */ @@ -308,6 +315,54 @@ int Sensors::parameters_update() _voted_sensors_update.parametersUpdate(); + // mark all existing sensor calibrations active even if sensor is missing + // this preserves the calibration in the event of a parameter export while the sensor is missing + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + uint32_t device_id_accel = calibration::GetCalibrationParam("ACC", "ID", i); + uint32_t device_id_gyro = calibration::GetCalibrationParam("GYRO", "ID", i); + uint32_t device_id_mag = calibration::GetCalibrationParam("MAG", "ID", i); + + if (device_id_accel != 0) { + bool external_accel = (calibration::GetCalibrationParam("ACC", "ROT", i) >= 0); + calibration::Accelerometer accel_cal(device_id_accel, external_accel); + } + + if (device_id_gyro != 0) { + bool external_gyro = (calibration::GetCalibrationParam("GYRO", "ROT", i) >= 0); + calibration::Gyroscope gyro_cal(device_id_gyro, external_gyro); + } + + if (device_id_mag != 0) { + bool external_mag = (calibration::GetCalibrationParam("MAG", "ROT", i) >= 0); + calibration::Magnetometer mag_cal(device_id_mag, external_mag); + } + } + + // ensure calibration slots are active for the number of sensors currently available + // this to done to eliminate differences in the active set of parameters before and after sensor calibration + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (orb_exists(ORB_ID(sensor_accel), i) == PX4_OK) { + bool external = (calibration::GetCalibrationParam("ACC", "ROT", i) >= 0); + calibration::Accelerometer cal{0, external}; + cal.set_calibration_index(i); + cal.ParametersUpdate(); + } + + if (orb_exists(ORB_ID(sensor_gyro), i) == PX4_OK) { + bool external = (calibration::GetCalibrationParam("GYRO", "ROT", i) >= 0); + calibration::Gyroscope cal{0, external}; + cal.set_calibration_index(i); + cal.ParametersUpdate(); + } + + if (orb_exists(ORB_ID(sensor_mag), i) == PX4_OK) { + bool external = (calibration::GetCalibrationParam("MAG", "ROT", i) >= 0); + calibration::Magnetometer cal{0, external}; + cal.set_calibration_index(i); + cal.ParametersUpdate(); + } + } + return PX4_OK; } @@ -611,11 +666,28 @@ void Sensors::Run() // keep adding sensors as long as we are not armed, // when not adding sensors poll for param updates if (!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) { - _voted_sensors_update.initializeSensors(); - InitializeVehicleAirData(); - InitializeVehicleIMU(); - InitializeVehicleGPSPosition(); - InitializeVehicleMagnetometer(); + + const int n_accel = orb_group_count(ORB_ID(sensor_accel)); + const int n_baro = orb_group_count(ORB_ID(sensor_baro)); + const int n_gps = orb_group_count(ORB_ID(sensor_gps)); + const int n_gyro = orb_group_count(ORB_ID(sensor_gyro)); + const int n_mag = orb_group_count(ORB_ID(sensor_mag)); + + if ((n_accel != _n_accel) || (n_baro != _n_baro) || (n_gps != _n_gps) || (n_gyro != _n_gyro) || (n_mag != _n_mag)) { + _n_accel = n_accel; + _n_baro = n_baro; + _n_gps = n_gps; + _n_gyro = n_gyro; + _n_mag = n_mag; + parameters_update(); + + _voted_sensors_update.initializeSensors(); + InitializeVehicleAirData(); + InitializeVehicleIMU(); + InitializeVehicleGPSPosition(); + InitializeVehicleMagnetometer(); + } + _last_config_update = hrt_absolute_time(); } else {