From 661eb2adb4fb59f0da96052ebce20165f825e76e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 6 Feb 2023 10:55:17 -0500 Subject: [PATCH] lib/sensor_calibration: BiasCorrectedSensorOffset() don't incorporate thermal offsets - the thermal offsets are an optional correction applied to the raw data, so when updating an existing calibration offset with new learned bias we don't want this incorporated --- src/lib/sensor_calibration/Accelerometer.hpp | 3 ++- src/lib/sensor_calibration/Gyroscope.hpp | 3 ++- src/lib/sensor_calibration/Magnetometer.hpp | 3 ++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index e22b58ab09..ef631a00f1 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -87,7 +87,8 @@ public: // Compute sensor offset from bias (board frame) matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const { - return (_rotation.I() * bias).edivide(_scale) + _thermal_offset + _offset; + // updated calibration offset = existing offset + bias rotated to sensor frame and unscaled + return _offset + (_rotation.I() * bias).edivide(_scale); } bool ParametersLoad(); diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index 70aa88cd47..6d160a1217 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -91,7 +91,8 @@ public: // Compute sensor offset from bias (board frame) matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const { - return (_rotation.I() * bias) + _thermal_offset + _offset; + // updated calibration offset = existing offset + bias rotated to sensor frame + return _offset + (_rotation.I() * bias); } bool ParametersLoad(); diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 24b9fa0ee0..961155e12f 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -89,7 +89,8 @@ public: // Compute sensor offset from bias (board frame) matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const { - return _scale.I() * _rotation.I() * bias + _offset; + // updated calibration offset = existing offset + bias rotated to sensor frame and unscaled + return _offset + (_scale.I() * _rotation.I() * bias); } bool ParametersLoad();