From 1ea4fe9b7b05ac4ddcd26c78d18ee20533e69ff3 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 27 Sep 2022 16:14:38 +0200 Subject: [PATCH] Mag: fix estimated bias save to calibration Clear the calibration at the end only otherwise everything gets erased at the end of the first iteration of the outer loop --- .../vehicle_magnetometer/VehicleMagnetometer.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 48c50365c4..37db3b7a21 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -312,16 +312,12 @@ void VehicleMagnetometer::UpdateMagCalibration() _calibration[mag_index].ParametersSave(); + _calibration_estimator_bias[mag_index].zero(); + calibration_param_save_needed = true; } - } - - // clear - _mag_cal[i] = {}; } - - _calibration_estimator_bias[mag_index].zero(); } if (calibration_param_save_needed) { @@ -329,6 +325,11 @@ void VehicleMagnetometer::UpdateMagCalibration() _last_calibration_update = hrt_absolute_time(); } + // clear all + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + _mag_cal[i] = {}; + } + _in_flight_mag_cal_available = false; } }