From 68026eadeba152ef95108eb5781c7ca1c63f2cae Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 7 Nov 2021 15:34:27 -0500 Subject: [PATCH] save significant IMU bias changes learned by the EKF * ekf2: make publishing of learned accel biases more robust * ekf2: reset accel bias if calibration updated * msg: add separate accel and gyro calibration counters * ekf2: use separate accel and gyro calibration counters * ekf2: rework logic to reset biases when calibration counters increment * sensors: add saving of learned accel biases * ekf2: generalized saving accel/gyro/mag in flight sensor calibration * boards: holybro kakutef7 disable systemcmds/perf and systemcmds/top to save flash Co-authored-by: Paul Riseborough --- boards/holybro/kakutef7/default.px4board | 2 - msg/estimator_sensor_bias.msg | 3 + msg/vehicle_imu.msg | 3 +- src/lib/sensor_calibration/Accelerometer.hpp | 6 + src/lib/sensor_calibration/Gyroscope.hpp | 6 + src/modules/ekf2/EKF2.cpp | 176 +++++++++++++--- src/modules/ekf2/EKF2.hpp | 32 ++- .../sensors/vehicle_imu/VehicleIMU.cpp | 195 +++++++++++++++++- .../sensors/vehicle_imu/VehicleIMU.hpp | 27 +++ .../VehicleMagnetometer.cpp | 6 +- 10 files changed, 406 insertions(+), 50 deletions(-) diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index ec4361a416..309cff6f41 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -41,8 +41,6 @@ CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_MIXER=y CONFIG_SYSTEMCMDS_PARAM=y -CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_PWM=y CONFIG_SYSTEMCMDS_REBOOT=y -CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_VER=y diff --git a/msg/estimator_sensor_bias.msg b/msg/estimator_sensor_bias.msg index 6999bea7a4..f42e1aa87c 100644 --- a/msg/estimator_sensor_bias.msg +++ b/msg/estimator_sensor_bias.msg @@ -13,15 +13,18 @@ float32[3] gyro_bias # gyroscope in-run bias in body frame (rad/s) float32 gyro_bias_limit # magnitude of maximum gyroscope in-run bias in body frame (rad/s) float32[3] gyro_bias_variance bool gyro_bias_valid +bool gyro_bias_stable # true when the gyro bias estimate is stable enough to use for calibration uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles float32[3] accel_bias # accelerometer in-run bias in body frame (m/s^2) float32 accel_bias_limit # magnitude of maximum accelerometer in-run bias in body frame (m/s^2) float32[3] accel_bias_variance bool accel_bias_valid +bool accel_bias_stable # true when the accel bias estimate is stable enough to use for calibration uint32 mag_device_id # unique device ID for the sensor that does not change between power cycles float32[3] mag_bias # magnetometer in-run bias in body frame (Gauss) float32 mag_bias_limit # magnitude of maximum magnetometer in-run bias in body frame (Gauss) float32[3] mag_bias_variance bool mag_bias_valid +bool mag_bias_stable # true when the mag bias estimate is stable enough to use for calibration diff --git a/msg/vehicle_imu.msg b/msg/vehicle_imu.msg index ef8402f2b0..0184039157 100644 --- a/msg/vehicle_imu.msg +++ b/msg/vehicle_imu.msg @@ -16,4 +16,5 @@ uint8 CLIPPING_Y = 2 uint8 CLIPPING_Z = 4 uint8 delta_velocity_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame -uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes. +uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. +uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index 020eb6d33d..98d9c99e2e 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -84,6 +84,12 @@ public: return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)}; } + // Compute sensor offset from bias (board frame) + matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const + { + return (_rotation.I() * bias).edivide(_scale) + _thermal_offset + _offset; + } + bool ParametersSave(); void ParametersUpdate(); diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index aaa8e96df7..8162a5909e 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -88,6 +88,12 @@ public: return (_rotation.I() * corrected_data) + _thermal_offset + _offset; } + // Compute sensor offset from bias (board frame) + matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const + { + return (_rotation.I() * bias) + _thermal_offset + _offset; + } + bool ParametersSave(); void ParametersUpdate(); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 7acf148846..d902ebf9e0 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -341,20 +341,45 @@ void EKF2::Run() if ((_device_id_accel == 0) || (_device_id_gyro == 0)) { _device_id_accel = imu.accel_device_id; _device_id_gyro = imu.gyro_device_id; - _imu_calibration_count = imu.calibration_count; + _accel_calibration_count = imu.accel_calibration_count; + _gyro_calibration_count = imu.gyro_calibration_count; - } else if ((imu.calibration_count > _imu_calibration_count) - || (imu.accel_device_id != _device_id_accel) - || (imu.gyro_device_id != _device_id_gyro)) { + } else { + bool reset_actioned = false; - PX4_DEBUG("%d - resetting IMU bias", _instance); - _device_id_accel = imu.accel_device_id; - _device_id_gyro = imu.gyro_device_id; + if ((imu.accel_calibration_count != _accel_calibration_count) + || (imu.accel_device_id != _device_id_accel)) { - _ekf.resetImuBias(); - _imu_calibration_count = imu.calibration_count; + PX4_DEBUG("%d - resetting accelerometer bias", _instance); + _device_id_accel = imu.accel_device_id; - SelectImuStatus(); + _ekf.resetAccelBias(); + _accel_calibration_count = imu.accel_calibration_count; + + // reset bias learning + _accel_cal = {}; + + reset_actioned = true; + } + + if ((imu.gyro_calibration_count != _gyro_calibration_count) + || (imu.gyro_device_id != _device_id_gyro)) { + + PX4_DEBUG("%d - resetting rate gyro bias", _instance); + _device_id_gyro = imu.gyro_device_id; + + _ekf.resetGyroBias(); + _gyro_calibration_count = imu.gyro_calibration_count; + + // reset bias learning + _gyro_cal = {}; + + reset_actioned = true; + } + + if (reset_actioned) { + SelectImuStatus(); + } } } else { @@ -385,14 +410,26 @@ void EKF2::Run() if (_sensor_selection_sub.copy(&sensor_selection)) { if (_device_id_accel != sensor_selection.accel_device_id) { - _ekf.resetAccelBias(); + _device_id_accel = sensor_selection.accel_device_id; + + _ekf.resetAccelBias(); + + // reset bias learning + _accel_cal = {}; + SelectImuStatus(); } if (_device_id_gyro != sensor_selection.gyro_device_id) { - _ekf.resetGyroBias(); + _device_id_gyro = sensor_selection.gyro_device_id; + + _ekf.resetGyroBias(); + + // reset bias learning + _gyro_cal = {}; + SelectImuStatus(); } } @@ -452,6 +489,7 @@ void EKF2::Run() vehicle_land_detected_s vehicle_land_detected; if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + const bool was_in_air = _ekf.control_status_flags().in_air; _ekf.set_in_air_status(!vehicle_land_detected.landed); if (_armed && (_param_ekf2_gnd_eff_dz.get() > 0.f)) { @@ -463,6 +501,13 @@ void EKF2::Run() } else { _ekf.set_gnd_effect_flag(false); } + + // reset learned sensor calibrations on takeoff + if (_ekf.control_status_flags().in_air && !was_in_air) { + _accel_cal = {}; + _gyro_cal = {}; + _mag_cal = {}; + } } } @@ -500,7 +545,6 @@ void EKF2::Run() PublishLocalPosition(now); PublishOdometry(now, imu_sample_new); PublishGlobalPosition(now); - PublishSensorBias(now); PublishWindEstimate(now); // publish status/logging messages @@ -515,7 +559,10 @@ void EKF2::Run() PublishInnovationVariances(now); PublishYawEstimatorStatus(now); + UpdateAccelCalibration(now); + UpdateGyroCalibration(now); UpdateMagCalibration(now); + PublishSensorBias(now); } else { // ekf no update @@ -1013,7 +1060,7 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) const Vector3f gyro_bias{_ekf.getGyroBias()}; const Vector3f accel_bias{_ekf.getAccelBias()}; - const Vector3f mag_bias{_mag_cal_last_bias}; + const Vector3f mag_bias{_ekf.getMagBias()}; // only publish on change if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f) @@ -1026,8 +1073,8 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) gyro_bias.copyTo(bias.gyro_bias); bias.gyro_bias_limit = math::radians(20.f); // 20 degrees/s see Ekf::constrainStates() _ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance); - bias.gyro_bias_valid = true; - + bias.gyro_bias_valid = true; // TODO + bias.gyro_bias_stable = _gyro_cal.cal_available; _last_gyro_bias_published = gyro_bias; } @@ -1036,8 +1083,8 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) accel_bias.copyTo(bias.accel_bias); bias.accel_bias_limit = _params->acc_bias_lim; _ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance); - bias.accel_bias_valid = !_ekf.fault_status_flags().bad_acc_bias; - + bias.accel_bias_valid = true; // TODO + bias.accel_bias_stable = _accel_cal.cal_available; _last_accel_bias_published = accel_bias; } @@ -1045,9 +1092,9 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) bias.mag_device_id = _device_id_mag; mag_bias.copyTo(bias.mag_bias); bias.mag_bias_limit = 0.5f; // 0.5 Gauss see Ekf::constrainStates() - _mag_cal_last_bias_variance.copyTo(bias.mag_bias_variance); - bias.mag_bias_valid = _mag_cal_available; - + _ekf.getMagBiasVariance().copyTo(bias.mag_bias_variance); + bias.mag_bias_valid = true; // TODO + bias.mag_bias_stable = _mag_cal.cal_available; _last_mag_bias_published = mag_bias; } @@ -1646,9 +1693,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) _mag_calibration_count = magnetometer.calibration_count; // reset magnetometer bias learning - _mag_cal_total_time_us = 0; - _mag_cal_last_us = 0; - _mag_cal_available = false; + _mag_cal = {}; } _ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}}); @@ -1725,34 +1770,99 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) } } +void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp) +{ + // Check if conditions are OK for learning of accelerometer bias values + // the EKF is operating in the correct mode and there are no filter faults + if (_ekf.control_status_flags().in_air && (_ekf.fault_status().value == 0) + && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) { + + if (_accel_cal.last_us != 0) { + _accel_cal.total_time_us += timestamp - _accel_cal.last_us; + + // Start checking accel bias estimates when we have accumulated sufficient calibration time + if (_accel_cal.total_time_us > 30_s) { + _accel_cal.last_bias = _ekf.getAccelBias(); + _accel_cal.last_bias_variance = _ekf.getAccelBiasVariance(); + _accel_cal.cal_available = true; + } + } + + _accel_cal.last_us = timestamp; + + } else { + // conditions are NOT OK for learning accelerometer bias, reset timestamp + // but keep the accumulated calibration time + _accel_cal.last_us = 0; + + if (_ekf.fault_status().value != 0) { + // if a filter fault has occurred, assume previous learning was invalid and do not + // count it towards total learning time. + _accel_cal.total_time_us = 0; + } + } +} + +void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp) +{ + // Check if conditions are OK for learning of gyro bias values + // the EKF is operating in the correct mode and there are no filter faults + if (_ekf.control_status_flags().in_air && (_ekf.fault_status().value == 0)) { + + if (_gyro_cal.last_us != 0) { + _gyro_cal.total_time_us += timestamp - _gyro_cal.last_us; + + // Start checking gyro bias estimates when we have accumulated sufficient calibration time + if (_gyro_cal.total_time_us > 30_s) { + _gyro_cal.last_bias = _ekf.getGyroBias(); + _gyro_cal.last_bias_variance = _ekf.getGyroBiasVariance(); + _gyro_cal.cal_available = true; + } + } + + _gyro_cal.last_us = timestamp; + + } else { + // conditions are NOT OK for learning gyro bias, reset timestamp + // but keep the accumulated calibration time + _gyro_cal.last_us = 0; + + if (_ekf.fault_status().value != 0) { + // if a filter fault has occurred, assume previous learning was invalid and do not + // count it towards total learning time. + _gyro_cal.total_time_us = 0; + } + } +} + void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) { // Check if conditions are OK for learning of magnetometer bias values // the EKF is operating in the correct mode and there are no filter faults if (_ekf.control_status_flags().in_air && _ekf.control_status_flags().mag_3D && (_ekf.fault_status().value == 0)) { - if (_mag_cal_last_us != 0) { - _mag_cal_total_time_us += timestamp - _mag_cal_last_us; + if (_mag_cal.last_us != 0) { + _mag_cal.total_time_us += timestamp - _mag_cal.last_us; // Start checking mag bias estimates when we have accumulated sufficient calibration time - if (_mag_cal_total_time_us > 30_s) { - _mag_cal_last_bias = _ekf.getMagBias(); - _mag_cal_last_bias_variance = _ekf.getMagBiasVariance(); - _mag_cal_available = true; + if (_mag_cal.total_time_us > 30_s) { + _mag_cal.last_bias = _ekf.getMagBias(); + _mag_cal.last_bias_variance = _ekf.getMagBiasVariance(); + _mag_cal.cal_available = true; } } - _mag_cal_last_us = timestamp; + _mag_cal.last_us = timestamp; } else { // conditions are NOT OK for learning magnetometer bias, reset timestamp // but keep the accumulated calibration time - _mag_cal_last_us = 0; + _mag_cal.last_us = 0; if (_ekf.fault_status().value != 0) { // if a filter fault has occurred, assume previous learning was invalid and do not // count it towards total learning time. - _mag_cal_total_time_us = 0; + _mag_cal.total_time_us = 0; } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index fd5dbf60fa..63ba984726 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -166,8 +166,11 @@ private: void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateImuStatus(); + void UpdateAccelCalibration(const hrt_abstime ×tamp); + void UpdateGyroCalibration(const hrt_abstime ×tamp); void UpdateMagCalibration(const hrt_abstime ×tamp); + /* * Calculate filtered WGS84 height from estimated AMSL height */ @@ -198,17 +201,22 @@ private: perf_counter_t _msg_missed_odometry_perf{nullptr}; perf_counter_t _msg_missed_optical_flow_perf{nullptr}; - // Used to check, save and use learned magnetometer biases - hrt_abstime _mag_cal_last_us{0}; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec) - hrt_abstime _mag_cal_total_time_us{0}; ///< accumulated calibration time since the last save - - Vector3f _mag_cal_last_bias{}; ///< last valid XYZ magnetometer bias estimates (Gauss) - Vector3f _mag_cal_last_bias_variance{}; ///< variances for the last valid magnetometer XYZ bias estimates (Gauss**2) - bool _mag_cal_available{false}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available - // Used to control saving of mag declination to be used on next startup bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved + // Used to check, save and use learned accel/gyro/mag biases + struct InFlightCalibration { + hrt_abstime last_us{0}; ///< last time the EKF was operating a mode that estimates accelerometer biases (uSec) + hrt_abstime total_time_us{0}; ///< accumulated calibration time since the last save + Vector3f last_bias{}; ///< last valid XYZ accelerometer bias estimates (Gauss) + Vector3f last_bias_variance{}; ///< variances for the last valid accelerometer XYZ bias estimates (m/s**2)**2 + bool cal_available{false}; ///< true when an unsaved valid calibration for the XYZ accelerometer bias is available + }; + + InFlightCalibration _accel_cal{}; + InFlightCalibration _gyro_cal{}; + InFlightCalibration _mag_cal{}; + bool _had_valid_terrain{false}; ///< true if at any time there was a valid terrain estimate uint64_t _gps_time_usec{0}; @@ -216,7 +224,8 @@ private: uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 - uint8_t _imu_calibration_count{0}; + uint8_t _accel_calibration_count{0}; + uint8_t _gyro_calibration_count{0}; uint8_t _mag_calibration_count{0}; uint32_t _device_id_accel{0}; @@ -227,6 +236,11 @@ private: Vector3f _last_accel_bias_published{}; Vector3f _last_gyro_bias_published{}; Vector3f _last_mag_bias_published{}; + + Vector3f _last_accel_calibration_published{}; + Vector3f _last_gyro_calibration_published{}; + Vector3f _last_mag_calibration_published{}; + float _last_baro_bias_published{}; float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 2cfcadec35..f9fc2fa012 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -146,6 +146,15 @@ void VehicleIMU::Run() ParametersUpdate(); + // check vehicle status for changes to armed state + if (_vehicle_control_mode_sub.updated()) { + vehicle_control_mode_s vehicle_control_mode; + + if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { + _armed = vehicle_control_mode.flag_armed; + } + } + if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) { return; } @@ -210,15 +219,20 @@ void VehicleIMU::Run() _gyro_update_latency_mean.update(Vector2f{time_run_s - _gyro_timestamp_sample_last * 1e-6f, time_run_s - _gyro_timestamp_last * 1e-6f}); - return; + break; } } // finish if there are no more updates, but didn't publish if (!updated) { - return; + break; } } + + if (hrt_elapsed_time(&_in_flight_calibration_check_timestamp_last) > 1_s) { + SensorCalibrationUpdate(); + _in_flight_calibration_check_timestamp_last = hrt_absolute_time(); + } } bool VehicleIMU::UpdateAccel() @@ -537,7 +551,8 @@ bool VehicleIMU::Publish() delta_angle_corrected.copyTo(imu.delta_angle); delta_velocity_corrected.copyTo(imu.delta_velocity); imu.delta_velocity_clipping = _delta_velocity_clipping; - imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count(); + imu.accel_calibration_count = _accel_calibration.calibration_count(); + imu.gyro_calibration_count = _gyro_calibration.calibration_count(); imu.timestamp = hrt_absolute_time(); _vehicle_imu_pub.publish(imu); @@ -650,4 +665,178 @@ void VehicleIMU::PrintStatus() _gyro_calibration.PrintStatus(); } +void VehicleIMU::SensorCalibrationUpdate() +{ + // State variance assumed for accelerometer bias storage. + // This is a reference variance used to calculate the fraction of learned accelerometer 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 max_var_allowed = 1e-3f; + static constexpr float max_var_ratio = 1e2f; + + if (_armed) { + 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)) { + // find corresponding accel bias + if (_accel_calibration.device_id() == estimator_sensor_bias.accel_device_id) { + const Vector3f bias{estimator_sensor_bias.accel_bias}; + const Vector3f bias_variance{estimator_sensor_bias.accel_bias_variance}; + + const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) && + (estimator_sensor_bias.accel_device_id != 0) && + estimator_sensor_bias.accel_bias_stable && + (bias_variance.max() < max_var_allowed) && + (bias_variance.max() < max_var_ratio * bias_variance.min()); + + if (valid) { + _accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias); + _accel_learned_calibration[i].bias_variance = bias_variance; + _accel_learned_calibration[i].valid = true; + _accel_cal_available = true; + + } else { + _accel_learned_calibration[i].valid = false; + } + } + + // find corresponding gyro calibration + if (_gyro_calibration.device_id() == estimator_sensor_bias.gyro_device_id) { + const Vector3f bias{estimator_sensor_bias.gyro_bias}; + const Vector3f bias_variance{estimator_sensor_bias.gyro_bias_variance}; + + const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) && + (estimator_sensor_bias.gyro_device_id != 0) && + estimator_sensor_bias.gyro_bias_valid && + estimator_sensor_bias.gyro_bias_stable && + (bias_variance.max() < max_var_allowed) && + (bias_variance.max() < max_var_ratio * bias_variance.min()); + + if (valid) { + _gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias); + _gyro_learned_calibration[i].bias_variance = bias_variance; + _gyro_learned_calibration[i].valid = true; + _gyro_cal_available = true; + + } else { + _gyro_learned_calibration[i].valid = false; + } + } + } + } + + } + + + // not armed and accel cal available + if (!_armed && _accel_cal_available) { + + bool initialised = false; + Vector3f bias_variance {}; + Vector3f bias_estimate {}; + + // 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; + bias_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; + bias_estimate(axis_index) = k2 * bias_estimate(axis_index) + k1 * _accel_learned_calibration[i].offset(axis_index); + bias_variance(axis_index) *= k2; + } + } + } + } + + if (initialised && bias_estimate.longerThan(0.05f)) { + const Vector3f accel_cal_orig{_accel_calibration.offset()}; + Vector3f accel_cal_offset{_accel_calibration.offset()}; + + for (int axis_index = 0; axis_index < 3; axis_index++) { + accel_cal_offset(axis_index) += bias_estimate(axis_index); + } + + if (_accel_calibration.set_offset(accel_cal_offset)) { + + PX4_INFO("accel %d (%" PRIu32 ") offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])", + _instance, _accel_calibration.device_id(), + (double)accel_cal_orig(0), (double)accel_cal_orig(1), (double)accel_cal_orig(2), + (double)accel_cal_offset(0), (double)accel_cal_offset(1), (double)accel_cal_offset(2), + (double)bias_estimate(0), (double)bias_estimate(1), (double)bias_estimate(2)); + + _accel_calibration.ParametersSave(); + } + } + + // reset + _accel_cal_available = false; + + for (auto &learned_cal : _accel_learned_calibration) { + learned_cal = {}; + } + } + + + // not armed and gyro cal available + if (!_armed && _gyro_cal_available) { + bool initialised = false; + Vector3f bias_variance {}; + Vector3f bias_estimate {}; + + // 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; + bias_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; + bias_estimate(axis_index) = k2 * bias_estimate(axis_index) + k1 * _gyro_learned_calibration[i].offset(axis_index); + bias_variance(axis_index) *= k2; + } + } + } + } + + if (initialised && bias_estimate.longerThan(0.05f)) { + const Vector3f gyro_cal_orig{_gyro_calibration.offset()}; + Vector3f gyro_cal_offset{_gyro_calibration.offset()}; + + for (int axis_index = 0; axis_index < 3; axis_index++) { + gyro_cal_offset(axis_index) += bias_estimate(axis_index); + } + + if (_gyro_calibration.set_offset(gyro_cal_offset)) { + + PX4_INFO("gyro %d (%" PRIu32 ") offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])", + _instance, _gyro_calibration.device_id(), + (double)gyro_cal_orig(0), (double)gyro_cal_orig(1), (double)gyro_cal_orig(2), + (double)gyro_cal_offset(0), (double)gyro_cal_offset(1), (double)gyro_cal_offset(2), + (double)bias_estimate(0), (double)bias_estimate(1), (double)bias_estimate(2)); + + _gyro_calibration.ParametersSave(); + } + } + + // reset + _gyro_cal_available = false; + + for (auto &learned_cal : _gyro_learned_calibration) { + learned_cal = {}; + } + } +} + } // namespace sensors diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 1580b79931..7b46c5d5c0 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -47,10 +47,13 @@ #include #include #include +#include #include +#include #include #include #include +#include #include #include @@ -85,14 +88,21 @@ private: void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity); void UpdateAccelSquaredErrorSum(const matrix::Vector3f &acceleration); + void SensorCalibrationUpdate(); + uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + // Used to check, save and use learned magnetometer biases + uORB::SubscriptionMultiArray _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias}; + uORB::Subscription _sensor_accel_sub; uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + calibration::Accelerometer _accel_calibration{}; calibration::Gyroscope _gyro_calibration{}; @@ -105,6 +115,8 @@ private: hrt_abstime _gyro_timestamp_sample_last{0}; hrt_abstime _gyro_timestamp_last{0}; + hrt_abstime _in_flight_calibration_check_timestamp_last{0}; + math::WelfordMean _accel_interval_mean{}; math::WelfordMean _gyro_interval_mean{}; @@ -147,6 +159,21 @@ private: const uint8_t _instance; + bool _armed{false}; + + bool _accel_cal_available{false}; + bool _gyro_cal_available{false}; + + 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] {}; + + 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 9b6500cd77..e76db0f226 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -160,8 +160,10 @@ void VehicleMagnetometer::MagCalibrationUpdate() 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 - && (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed); + && (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