diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index e22b58ab09..b22b95515d 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -69,7 +69,7 @@ public: uint8_t calibration_count() const { return _calibration_count; } int8_t calibration_index() const { return _calibration_index; } uint32_t device_id() const { return _device_id; } - bool enabled() const { return (_priority > 0); } + bool enabled() const { return (_device_id != 0) && (_priority > 0); } bool external() const { return _external; } const matrix::Vector3f &offset() const { return _offset; } const int32_t &priority() const { return _priority; } diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index 70aa88cd47..b86489925d 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -68,7 +68,7 @@ public: uint8_t calibration_count() const { return _calibration_count; } int8_t calibration_index() const { return _calibration_index; } uint32_t device_id() const { return _device_id; } - bool enabled() const { return (_priority > 0); } + bool enabled() const { return (_device_id != 0) && (_priority > 0); } bool external() const { return _external; } const matrix::Vector3f &offset() const { return _offset; } const int32_t &priority() const { return _priority; } diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 24b9fa0ee0..4332550db3 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -71,7 +71,7 @@ public: uint8_t calibration_count() const { return _calibration_count; } int8_t calibration_index() const { return _calibration_index; } uint32_t device_id() const { return _device_id; } - bool enabled() const { return (_priority > 0); } + bool enabled() const { return (_device_id != 0) && (_priority > 0); } bool external() const { return _external; } const matrix::Vector3f &offset() const { return _offset; } const int32_t &priority() const { return _priority; } diff --git a/src/lib/sensor_calibration/Utilities.hpp b/src/lib/sensor_calibration/Utilities.hpp index 4696f01f36..729c66c0e2 100644 --- a/src/lib/sensor_calibration/Utilities.hpp +++ b/src/lib/sensor_calibration/Utilities.hpp @@ -149,4 +149,77 @@ matrix::Dcmf GetBoardRotationMatrix(); */ bool DeviceExternal(uint32_t device_id); + +struct InFlightCalibration { + uint32_t device_id{0}; + matrix::Vector3f offset{}; + matrix::Vector3f variance{}; + uint8_t estimator_instance{0}; +}; + +template +bool SensorCalibrationApplyLearned(T &calibration, int sensor_index, + InFlightCalibration(&learned_calibrations)[MAX_LEARNED]) +{ + bool updated = false; + + // State variance assumed for bias storage. + // This is a reference variance used to calculate the fraction of learned bias that will be used to update the stored value. + // Larger values cause a larger fraction of the learned biases to be used. + static constexpr float BIAS_VAR_REF = 0.000'001f; + matrix::Vector3f state_variance{BIAS_VAR_REF, BIAS_VAR_REF, BIAS_VAR_REF}; + + // apply all valid saved offsets + // iterate through available bias estimates and fuse them sequentially using a Kalman Filter scheme + for (auto &inflight_cal : learned_calibrations) { + if ((inflight_cal.device_id != 0) && (inflight_cal.device_id == calibration.device_id()) && calibration.enabled()) { + + const matrix::Vector3f offset_orig{calibration.offset()}; + + // new offset + matrix::Vector3f offset_new{}; + + if (calibration.calibrated()) { + // calculate weighting using ratio of variances and update stored bias values + const matrix::Vector3f &obs{inflight_cal.offset}; // observation + const matrix::Vector3f &obs_variance{inflight_cal.variance}; // observation variance + + for (int axis_index = 0; axis_index < 3; axis_index++) { + float innovation = offset_orig(axis_index) - obs(axis_index); + float innovation_variance = state_variance(axis_index) + obs_variance(axis_index); + + float kalman_gain = state_variance(axis_index) / innovation_variance; + + offset_new(axis_index) = offset_orig(axis_index) - (innovation * kalman_gain); + + state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain), 0.f); + } + + } else { + // sensor wasn't calibrated, use full offset + offset_new = inflight_cal.offset; + } + + if (calibration.set_offset(offset_new)) { + PX4_INFO("%s %d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])", + calibration.SensorString(), sensor_index, calibration.device_id(), inflight_cal.estimator_instance, + (double)offset_orig(0), (double)offset_orig(1), (double)offset_orig(2), + (double)offset_new(0), (double)offset_new(1), (double)offset_new(2), + (double)inflight_cal.offset(0), (double)inflight_cal.offset(1), (double)inflight_cal.offset(2)); + + updated = true; + } + + // clear + inflight_cal = {}; + } + } + + if (updated) { + calibration.ParametersSave(sensor_index); + } + + return updated; +} + } // namespace calibration diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 261b09a6ff..8d635ab6ac 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -125,24 +125,20 @@ bool VehicleIMU::ParametersUpdate(bool force) _accel_calibration.ParametersUpdate(); _gyro_calibration.ParametersUpdate(); - if (accel_calibration_count != _accel_calibration.calibration_count()) { + if ((accel_calibration_count != _accel_calibration.calibration_count()) + || (gyro_calibration_count != _gyro_calibration.calibration_count()) + ) { // if calibration changed reset any existing learned calibration - _accel_cal_available = false; - _in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US; - for (auto &learned_cal : _accel_learned_calibration) { learned_cal = {}; } - } - - if (gyro_calibration_count != _gyro_calibration.calibration_count()) { - // if calibration changed reset any existing learned calibration - _gyro_cal_available = false; - _in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US; for (auto &learned_cal : _gyro_learned_calibration) { learned_cal = {}; } + + _in_flight_cal_available = false; + _last_calibration_update = hrt_absolute_time(); } // constrain IMU integration time 1-20 milliseconds (50-1000 Hz) @@ -165,9 +161,94 @@ bool VehicleIMU::ParametersUpdate(bool force) return updated; } +void VehicleIMU::SensorCalibrationUpdate() +{ + int accel_cal_index = 0; + int gyro_cal_index = 0; + + for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) { + estimator_sensor_bias_s estimator_sensor_bias; + + if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias) + && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 10_s) + && (estimator_sensor_bias.timestamp > _last_calibration_update)) { + + // find corresponding accel bias + if ((estimator_sensor_bias.accel_device_id != 0) + && (estimator_sensor_bias.accel_device_id == _accel_calibration.device_id()) + && estimator_sensor_bias.accel_bias_valid + && estimator_sensor_bias.accel_bias_stable + ) { + + if (accel_cal_index < IN_FLIGHT_CAL_COUNT) { + + const Vector3f bias{estimator_sensor_bias.accel_bias}; + const Vector3f bias_variance{estimator_sensor_bias.accel_bias_variance}; + + _accel_learned_calibration[accel_cal_index].device_id = estimator_sensor_bias.accel_device_id; + _accel_learned_calibration[accel_cal_index].offset = _accel_calibration.BiasCorrectedSensorOffset(bias); + _accel_learned_calibration[accel_cal_index].variance = bias_variance; + _accel_learned_calibration[accel_cal_index].estimator_instance = i; + + _in_flight_cal_available = true; + } + + accel_cal_index++; + } + + // find corresponding gyro calibration + if ((estimator_sensor_bias.gyro_device_id != 0) + && (estimator_sensor_bias.gyro_device_id == _gyro_calibration.device_id()) + && estimator_sensor_bias.gyro_bias_valid + && estimator_sensor_bias.gyro_bias_stable + ) { + + if (gyro_cal_index < IN_FLIGHT_CAL_COUNT) { + + const Vector3f bias{estimator_sensor_bias.gyro_bias}; + const Vector3f bias_variance{estimator_sensor_bias.gyro_bias_variance}; + + _gyro_learned_calibration[gyro_cal_index].device_id = estimator_sensor_bias.gyro_device_id; + _gyro_learned_calibration[gyro_cal_index].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias); + _gyro_learned_calibration[gyro_cal_index].variance = bias_variance; + _gyro_learned_calibration[gyro_cal_index].estimator_instance = i; + + _in_flight_cal_available = true; + } + + gyro_cal_index++; + } + } + } +} + +void VehicleIMU::SensorCalibrationApplyAll() +{ + if (_in_flight_cal_available) { + bool calibration_param_save_needed = false; + + if (calibration::SensorCalibrationApplyLearned(_accel_calibration, + _instance, _accel_learned_calibration)) { + calibration_param_save_needed = true; + } + + if (calibration::SensorCalibrationApplyLearned(_gyro_calibration, + _instance, _gyro_learned_calibration)) { + calibration_param_save_needed = true; + } + + if (calibration_param_save_needed) { + _last_calibration_update = hrt_absolute_time(); + param_notify_changes(); + } + + _in_flight_cal_available = false; + } +} + void VehicleIMU::Run() { - const hrt_abstime now_us = hrt_absolute_time(); + const hrt_abstime time_now_us = hrt_absolute_time(); const bool parameters_updated = ParametersUpdate(); @@ -245,7 +326,7 @@ void VehicleIMU::Run() _gyro_update_latency_mean.reset(); } - const float time_run_s = now_us * 1e-6f; + const float time_run_s = time_now_us * 1e-6f; _gyro_update_latency_mean.update(Vector2f{time_run_s - _gyro_timestamp_sample_last * 1e-6f, time_run_s - _gyro_timestamp_last * 1e-6f}); @@ -259,16 +340,19 @@ void VehicleIMU::Run() } } - if (_param_sens_imu_autocal.get() && !parameters_updated) { - if ((_armed || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated()) - && (now_us > _in_flight_calibration_check_timestamp_last + 1_s)) { + if (_param_sens_imu_autocal.get() && !parameters_updated + && (time_now_us > _last_calibration_update + IN_FLIGHT_CALIBRATION_QUIET_PERIOD_US)) { + + bool uncalibrated = !_accel_calibration.calibrated() || !_gyro_calibration.calibrated(); + + if ((_armed || uncalibrated) + && (time_now_us > _in_flight_calibration_check_timestamp_last + 1_s)) { SensorCalibrationUpdate(); - _in_flight_calibration_check_timestamp_last = now_us; + _in_flight_calibration_check_timestamp_last = time_now_us; } else if (!_armed) { - SensorCalibrationSaveAccel(); - SensorCalibrationSaveGyro(); + SensorCalibrationApplyAll(); } } } @@ -771,145 +855,4 @@ void VehicleIMU::PrintStatus() _gyro_calibration.PrintStatus(); } -void VehicleIMU::SensorCalibrationUpdate() -{ - for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) { - estimator_sensor_bias_s estimator_sensor_bias; - - if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias) - && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)) { - - // find corresponding accel bias - if (estimator_sensor_bias.accel_bias_valid && estimator_sensor_bias.accel_bias_stable - && (estimator_sensor_bias.accel_device_id != 0) - && (estimator_sensor_bias.accel_device_id == _accel_calibration.device_id())) { - - const Vector3f bias{estimator_sensor_bias.accel_bias}; - - _accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias); - _accel_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.accel_bias_variance}; - _accel_learned_calibration[i].valid = true; - _accel_cal_available = true; - } - - // find corresponding gyro calibration - if (estimator_sensor_bias.gyro_bias_valid && estimator_sensor_bias.gyro_bias_stable - && (estimator_sensor_bias.gyro_device_id != 0) - && (estimator_sensor_bias.gyro_device_id == _gyro_calibration.device_id())) { - - const Vector3f bias{estimator_sensor_bias.gyro_bias}; - - _gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias); - _gyro_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.gyro_bias_variance}; - _gyro_learned_calibration[i].valid = true; - _gyro_cal_available = true; - } - } - } -} - -void VehicleIMU::SensorCalibrationSaveAccel() -{ - if (_accel_cal_available) { - const Vector3f cal_orig{_accel_calibration.offset()}; - bool initialised = false; - Vector3f offset_estimate{}; - Vector3f bias_variance{}; - - // apply all valid saved offsets - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (_accel_learned_calibration[i].valid) { - if (!initialised) { - bias_variance = _accel_learned_calibration[i].bias_variance; - offset_estimate = _accel_learned_calibration[i].offset; - initialised = true; - - } else { - for (int axis_index = 0; axis_index < 3; axis_index++) { - const float sum_of_variances = _accel_learned_calibration[i].bias_variance(axis_index) + bias_variance(axis_index); - const float k1 = bias_variance(axis_index) / sum_of_variances; - const float k2 = _accel_learned_calibration[i].bias_variance(axis_index) / sum_of_variances; - offset_estimate(axis_index) = k2 * offset_estimate(axis_index) + k1 * _accel_learned_calibration[i].offset(axis_index); - bias_variance(axis_index) *= k2; - } - } - - // reset - _accel_learned_calibration[i] = {}; - } - } - - if (initialised && ((cal_orig - offset_estimate).longerThan(0.05f) || !_accel_calibration.calibrated())) { - if (_accel_calibration.set_offset(offset_estimate)) { - PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])", - _accel_calibration.SensorString(), _instance, _accel_calibration.device_id(), - (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2), - (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2)); - - // save parameters with preferred calibration slot to current sensor index - if (_accel_calibration.ParametersSave(_sensor_accel_sub.get_instance())) { - param_notify_changes(); - } - - _in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US; - } - } - - // reset - _accel_cal_available = false; - } -} - -void VehicleIMU::SensorCalibrationSaveGyro() -{ - if (_gyro_cal_available) { - const Vector3f cal_orig{_gyro_calibration.offset()}; - bool initialised = false; - Vector3f offset_estimate{}; - Vector3f bias_variance{}; - - // apply all valid saved offsets - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (_gyro_learned_calibration[i].valid) { - if (!initialised) { - bias_variance = _gyro_learned_calibration[i].bias_variance; - offset_estimate = _gyro_learned_calibration[i].offset; - initialised = true; - - } else { - for (int axis_index = 0; axis_index < 3; axis_index++) { - const float sum_of_variances = _gyro_learned_calibration[i].bias_variance(axis_index) + bias_variance(axis_index); - const float k1 = bias_variance(axis_index) / sum_of_variances; - const float k2 = _gyro_learned_calibration[i].bias_variance(axis_index) / sum_of_variances; - offset_estimate(axis_index) = k2 * offset_estimate(axis_index) + k1 * _gyro_learned_calibration[i].offset(axis_index); - bias_variance(axis_index) *= k2; - } - } - - // reset - _gyro_learned_calibration[i] = {}; - } - } - - if (initialised && ((cal_orig - offset_estimate).longerThan(0.01f) || !_gyro_calibration.calibrated())) { - if (_gyro_calibration.set_offset(offset_estimate)) { - PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])", - _gyro_calibration.SensorString(), _instance, _gyro_calibration.device_id(), - (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2), - (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2)); - - // save parameters with preferred calibration slot to current sensor index - if (_gyro_calibration.ParametersSave(_sensor_gyro_sub.get_instance())) { - param_notify_changes(); - } - - _in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US; - } - } - - // reset - _gyro_cal_available = false; - } -} - } // namespace sensors diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index f4f5f520c1..a883353f89 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -89,8 +90,7 @@ private: inline void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity); void SensorCalibrationUpdate(); - void SensorCalibrationSaveAccel(); - void SensorCalibrationSaveGyro(); + void SensorCalibrationApplyAll(); uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; @@ -170,21 +170,15 @@ private: bool _armed{false}; - bool _accel_cal_available{false}; - bool _gyro_cal_available{false}; + static constexpr int IN_FLIGHT_CAL_COUNT = 3; + calibration::InFlightCalibration _accel_learned_calibration[IN_FLIGHT_CAL_COUNT] {}; + calibration::InFlightCalibration _gyro_learned_calibration[IN_FLIGHT_CAL_COUNT] {}; - struct InFlightCalibration { - matrix::Vector3f offset{}; - matrix::Vector3f bias_variance{}; - bool valid{false}; - }; - - InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {}; - InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {}; - - static constexpr hrt_abstime INFLIGHT_CALIBRATION_QUIET_PERIOD_US{30_s}; + static constexpr hrt_abstime IN_FLIGHT_CALIBRATION_QUIET_PERIOD_US{10_s}; hrt_abstime _in_flight_calibration_check_timestamp_last{0}; + hrt_abstime _last_calibration_update{0}; + bool _in_flight_cal_available{false}; perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")}; perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")}; diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index c6b5ce97dd..e1df4b8ac8 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -135,11 +135,13 @@ bool VehicleMagnetometer::ParametersUpdate(bool force) _mag_comp_type = mag_comp_typ; - if (!_armed) { - bool calibration_updated = false; + bool calibration_updated = false; + + // update mag priority (CAL_MAGx_PRIO) + for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) { + + if (_calibration[mag].device_id() != 0) { - // update mag priority (CAL_MAGx_PRIO) - for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) { bool clear_online_mag_cal = false; const auto calibration_count = _calibration[mag].calibration_count(); @@ -173,7 +175,7 @@ bool VehicleMagnetometer::ParametersUpdate(bool force) // clear any mag bias estimate _calibration_estimator_bias[mag].zero(); - for (auto &cal : _mag_cal) { + for (auto &cal : _learned_calibration) { // clear any in flight mag calibration if (cal.device_id == _calibration[mag].device_id()) { cal = {}; @@ -181,10 +183,10 @@ bool VehicleMagnetometer::ParametersUpdate(bool force) } } } + } - if (calibration_updated) { - _last_calibration_update = hrt_absolute_time(); - } + if (calibration_updated) { + _last_calibration_update = hrt_absolute_time(); } return true; @@ -195,161 +197,118 @@ bool VehicleMagnetometer::ParametersUpdate(bool force) void VehicleMagnetometer::UpdateMagBiasEstimate() { - if (_magnetometer_bias_estimate_sub.updated()) { - // Continuous mag calibration is running when not armed - magnetometer_bias_estimate_s mag_bias_est; + // Continuous mag calibration is running when not armed + magnetometer_bias_estimate_s mag_bias_est; - if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) { - bool parameters_notify = false; + if (_magnetometer_bias_estimate_sub.update(&mag_bias_est) && !_armed) { + bool parameters_notify = false; - for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { - if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) { + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) { - const Vector3f bias{mag_bias_est.bias_x[mag_index], - mag_bias_est.bias_y[mag_index], - mag_bias_est.bias_z[mag_index]}; + const Vector3f bias{mag_bias_est.bias_x[mag_index], + mag_bias_est.bias_y[mag_index], + mag_bias_est.bias_z[mag_index]}; - _calibration_estimator_bias[mag_index] = bias; + _calibration_estimator_bias[mag_index] = bias; - // set initial mag calibration if disarmed, mag uncalibrated, and valid estimated bias available - if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index] - && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) { + // set initial mag calibration if disarmed, mag uncalibrated, and valid estimated bias available + if (_param_sens_mag_autocal.get() && mag_bias_est.stable[mag_index] + && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) { - const Vector3f old_offset = _calibration[mag_index].offset(); + // set initial mag calibration + const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]); - // set initial mag calibration - const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]); + if (_calibration[mag_index].set_offset(offset)) { + // save parameters with preferred calibration slot to current sensor index + _calibration[mag_index].ParametersSave(mag_index); - if (_calibration[mag_index].set_offset(offset)) { - // save parameters with preferred calibration slot to current sensor index - _calibration[mag_index].ParametersSave(mag_index); + PX4_INFO("mag %d (%" PRIu32 ") setting offset: [%.3f, %.3f, %.3f]", + mag_index, _calibration[mag_index].device_id(), + (double)_calibration[mag_index].offset()(0), + (double)_calibration[mag_index].offset()(1), + (double)_calibration[mag_index].offset()(2)); - PX4_INFO("mag %d (%" PRIu32 ") setting offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f]", - mag_index, _calibration[mag_index].device_id(), - (double)old_offset(0), (double)old_offset(1), (double)old_offset(2), - (double)_calibration[mag_index].offset()(0), - (double)_calibration[mag_index].offset()(1), - (double)_calibration[mag_index].offset()(2)); + _calibration_estimator_bias[mag_index].zero(); - _calibration_estimator_bias[mag_index].zero(); - - parameters_notify = true; + // clear any learned mag bias + for (auto &cal : _learned_calibration) { + if (cal.device_id == _calibration[mag_index].device_id()) { + cal = {}; + } } + + parameters_notify = true; } } } + } - if (parameters_notify) { - param_notify_changes(); - _last_calibration_update = hrt_absolute_time(); + if (parameters_notify) { + _last_calibration_update = hrt_absolute_time(); + param_notify_changes(); + } + } +} + +void VehicleMagnetometer::SensorCalibrationUpdate() +{ + for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) { + estimator_sensor_bias_s estimator_sensor_bias; + + if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)) { + + if ((hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 10_s) + && (estimator_sensor_bias.timestamp > _last_calibration_update) + && estimator_sensor_bias.mag_bias_valid + && estimator_sensor_bias.mag_bias_stable + && (estimator_sensor_bias.mag_device_id != 0)) { + + // find corresponding mag calibration + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + if (_calibration[mag_index].device_id() == estimator_sensor_bias.mag_device_id) { + + const Vector3f bias{estimator_sensor_bias.mag_bias}; + const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance}; + + _learned_calibration[i].device_id = estimator_sensor_bias.mag_device_id; + + // readd estimated bias that was removed before publishing vehicle_magnetometer + _learned_calibration[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias) + + _calibration_estimator_bias[mag_index]; + + _learned_calibration[i].variance = bias_variance; + _learned_calibration[i].estimator_instance = i; + + _in_flight_cal_available = true; + break; + } + } } } } } -void VehicleMagnetometer::UpdateMagCalibration() +void VehicleMagnetometer::SensorCalibrationApplyAll() { - // State variance assumed for magnetometer bias storage. - // This is a reference variance used to calculate the fraction of learned magnetometer bias that will be used to update the stored value. - // Larger values cause a larger fraction of the learned biases to be used. - static constexpr float magb_vref = 2.5e-7f; - static constexpr float min_var_allowed = magb_vref * 0.01f; - static constexpr float max_var_allowed = magb_vref * 100.f; - - if (_armed) { - static constexpr uint8_t mag_cal_size = sizeof(_mag_cal) / sizeof(_mag_cal[0]); - - for (int i = 0; i < math::min(_estimator_sensor_bias_subs.size(), mag_cal_size); i++) { - estimator_sensor_bias_s estimator_sensor_bias; - - if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)) { - - const Vector3f bias{estimator_sensor_bias.mag_bias}; - const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance}; - - const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) - && (estimator_sensor_bias.mag_device_id != 0) && - estimator_sensor_bias.mag_bias_valid && - estimator_sensor_bias.mag_bias_stable && - (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed); - - if (valid) { - // find corresponding mag calibration - for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { - if (_calibration[mag_index].device_id() == estimator_sensor_bias.mag_device_id) { - - _mag_cal[i].device_id = estimator_sensor_bias.mag_device_id; - - // readd estimated bias that was removed before publishing vehicle_magnetometer - _mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias) + - _calibration_estimator_bias[mag_index]; - - _mag_cal[i].variance = bias_variance; - - _in_flight_mag_cal_available = true; - break; - } - } - } - } - } - - } else if (_in_flight_mag_cal_available) { - // not armed and mag cal available + if (_in_flight_cal_available) { bool calibration_param_save_needed = false; - // iterate through available bias estimates and fuse them sequentially using a Kalman Filter scheme - Vector3f state_variance{magb_vref, magb_vref, magb_vref}; for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { - // apply all valid saved offsets - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if ((_calibration[mag_index].device_id() != 0) && (_mag_cal[i].device_id == _calibration[mag_index].device_id())) { - const Vector3f mag_cal_orig{_calibration[mag_index].offset()}; - // calculate weighting using ratio of variances and update stored bias values - const Vector3f &observation{_mag_cal[i].offset}; - const Vector3f &obs_variance{_mag_cal[i].variance}; - - const Vector3f innovation{mag_cal_orig - observation}; - const Vector3f innovation_variance{state_variance + obs_variance}; - const Vector3f kalman_gain{state_variance.edivide(innovation_variance)}; - - // new offset - const Vector3f mag_cal_offset{mag_cal_orig - innovation.emult(kalman_gain)}; - - for (int axis_index = 0; axis_index < 3; axis_index++) { - state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain(axis_index)), 0.f); - } - - if (_calibration[mag_index].set_offset(mag_cal_offset)) { - - PX4_INFO("%d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])", - mag_index, _calibration[mag_index].device_id(), i, - (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2), - (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2), - (double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2)); - - _calibration[mag_index].ParametersSave(); - - _calibration_estimator_bias[mag_index].zero(); - - calibration_param_save_needed = true; - } - } + if (calibration::SensorCalibrationApplyLearned + (_calibration[mag_index], mag_index, _learned_calibration)) { + calibration_param_save_needed = true; + _calibration_estimator_bias[mag_index].zero(); } } if (calibration_param_save_needed) { - param_notify_changes(); _last_calibration_update = hrt_absolute_time(); + param_notify_changes(); } - // clear all - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - _mag_cal[i] = {}; - } - - _in_flight_mag_cal_available = false; + _in_flight_cal_available = false; } } @@ -393,7 +352,7 @@ void VehicleMagnetometer::Run() const hrt_abstime time_now_us = hrt_absolute_time(); - const bool parameter_update = ParametersUpdate(); + const bool parameters_updated = ParametersUpdate(); // check vehicle status for changes to armed state if (_vehicle_control_mode_sub.updated()) { @@ -478,7 +437,7 @@ void VehicleMagnetometer::Run() if (best_index >= 0) { // handle selection change (don't process on same iteration as parameter update) - if ((_selected_sensor_sub_index != best_index) && !parameter_update) { + if ((_selected_sensor_sub_index != best_index) && !parameters_updated) { // clear all registered callbacks for (auto &sub : _sensor_sub) { sub.unregisterCallback(); @@ -557,7 +516,7 @@ void VehicleMagnetometer::Run() } } - if (!parameter_update && _param_sens_mag_mode.get()) { + if (!parameters_updated && _param_sens_mag_mode.get()) { CheckFailover(time_now_us); } @@ -565,7 +524,27 @@ void VehicleMagnetometer::Run() calcMagInconsistency(); } - UpdateMagCalibration(); + if (_param_sens_mag_autocal.get() && !parameters_updated + && (time_now_us > _last_calibration_update + IN_FLIGHT_CALIBRATION_QUIET_PERIOD_US)) { + + bool uncalibrated = false; + + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (_calibration[i].enabled() && !_calibration[i].calibrated()) { + uncalibrated = true; + } + } + + if ((_armed || uncalibrated) + && (time_now_us > _in_flight_calibration_check_timestamp_last + 1_s)) { + + SensorCalibrationUpdate(); + _in_flight_calibration_check_timestamp_last = time_now_us; + + } else if (!_armed) { + SensorCalibrationApplyAll(); + } + } UpdateStatus(); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp index d47fa87428..804b73359b 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -36,6 +36,7 @@ #include "data_validator/DataValidatorGroup.hpp" #include +#include #include #include #include @@ -91,8 +92,10 @@ private: */ void calcMagInconsistency(); + void SensorCalibrationApplyAll(); + void SensorCalibrationUpdate(); + void UpdateMagBiasEstimate(); - void UpdateMagCalibration(); void UpdatePowerCompensation(); static constexpr int MAX_SENSOR_COUNT = 4; @@ -118,13 +121,10 @@ private: // Used to check, save and use learned magnetometer biases uORB::SubscriptionMultiArray _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias}; - bool _in_flight_mag_cal_available{false}; ///< from navigation filter - - struct MagCal { - uint32_t device_id{0}; - matrix::Vector3f offset{}; - matrix::Vector3f variance{}; - } _mag_cal[ORB_MULTI_MAX_INSTANCES] {}; + static constexpr hrt_abstime IN_FLIGHT_CALIBRATION_QUIET_PERIOD_US{10_s}; + calibration::InFlightCalibration _learned_calibration[ORB_MULTI_MAX_INSTANCES] {}; + hrt_abstime _in_flight_calibration_check_timestamp_last{0}; + bool _in_flight_cal_available{false}; ///< from navigation filter uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { {this, ORB_ID(sensor_mag), 0},