From 61036599dbf1162d802573b677d7109d5a5a9fbe Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 6 Jul 2023 14:55:58 +0200 Subject: [PATCH] mag_cal: fix mag bias estimate to mag cal - since last_us is set to 0 every time the bias is not observable, the total time was also reset -> needed 30 consecutive seconds in mag 3D to be declared "stable" - after landing, the mag_aligned_in_flight flag is reset. Using this for bias validity makes it invalid before we have a chance to save it to the calibration. --- src/lib/sensor_calibration/Magnetometer.cpp | 2 +- src/modules/ekf2/EKF2.cpp | 7 ++++--- .../sensors/vehicle_magnetometer/VehicleMagnetometer.cpp | 2 +- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index efb2b8c1b2..a370bd2b41 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -70,7 +70,7 @@ void Magnetometer::set_device_id(uint32_t device_id) bool Magnetometer::set_offset(const Vector3f &offset) { - if (Vector3f(_offset - offset).longerThan(0.01f)) { + if (Vector3f(_offset - offset).longerThan(0.005f)) { if (offset.isAllFinite()) { _offset = offset; _calibration_count++; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index f5e2ae7034..c3e575a64b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2456,8 +2456,10 @@ void EKF2::UpdateCalibration(const hrt_abstime ×tamp, InFlightCalibration & // consider bias estimates stable when all checks pass consistently and bias hasn't changed more than 10% of the limit const float bias_change_limit = 0.1f * bias_limit; - if ((cal.last_us != 0) && !(cal.bias - bias).longerThan(bias_change_limit)) { - cal.total_time_us += timestamp - cal.last_us; + if (!(cal.bias - bias).longerThan(bias_change_limit)) { + if (cal.last_us != 0) { + cal.total_time_us += timestamp - cal.last_us; + } if (cal.total_time_us > 30_s) { cal.cal_available = true; @@ -2517,7 +2519,6 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp) void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) { const bool bias_valid = (_ekf.control_status_flags().mag_hdg || _ekf.control_status_flags().mag_3D) - && _ekf.control_status_flags().mag_aligned_in_flight && !_ekf.control_status_flags().mag_fault && !_ekf.control_status_flags().mag_field_disturbed; diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 8ee9380f30..7b82f82ebb 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -329,7 +329,7 @@ void VehicleMagnetometer::UpdateMagCalibration() 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])", + PX4_INFO("%d (%" PRIu32 ") EST:%d offset: [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] (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),