From 3b26b7adaf5cdaad83e3282d5724f736816fa381 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 26 Jan 2021 15:00:50 -0500 Subject: [PATCH] sensors: magnetometer preserve original mag cal offset for status message --- .../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 0331dc0399..c718fd3bc0 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -190,6 +190,7 @@ void VehicleMagnetometer::MagCalibrationUpdate() 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()}; Vector3f mag_cal_offset{_calibration[mag_index].offset()}; // calculate weighting using ratio of variances and update stored bias values @@ -208,9 +209,7 @@ void VehicleMagnetometer::MagCalibrationUpdate() PX4_INFO("%d (%d) EST:%d offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])", mag_index, _calibration[mag_index].device_id(), i, - (double)_calibration[mag_index].offset()(0), - (double)_calibration[mag_index].offset()(1), - (double)_calibration[mag_index].offset()(2), + (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].mag_offset(0), (double)_mag_cal[i].mag_offset(1), (double)_mag_cal[i].mag_offset(2));