From 41903531929df6f6b5cd6c7ea665d9ba73b36c8f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 7 Nov 2022 09:31:17 -0500 Subject: [PATCH] vehicle_magnetometer: fix standalone mag bias est factored into in flight mag cal - preflight mag bias estimate is in the vehicle body frame and removed from the raw mag data after it's rotated, calibrated, etc - in flight mag bias (from ekf2) is in the vehicle body frame, but stored as a sensor frame offset immediately --- .../sensors/vehicle_magnetometer/VehicleMagnetometer.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 494af2c2b6..7d87f705a7 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -280,9 +280,8 @@ void VehicleMagnetometer::UpdateMagCalibration() _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]; + // readd estimated bias that was removed before publishing vehicle_magnetometer (_calibration_estimator_bias) + _mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias + _calibration_estimator_bias[mag_index]); _mag_cal[i].variance = bias_variance;