From 3832214145705db3661816e8b4e323f81a9e1883 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 26 Apr 2020 13:24:51 -0400 Subject: [PATCH] PX4Accelerometer/PX4Gyroscope: fix calibration offset for integrated FIFO case This is a quick follow up fix to to a bug introduced by #14752. In the case of FIFO data (new IMU drivers) the calibration offset wasn't being applied correctly to the result of integrating the FIFO samples. This slipped through basic sanity testing (simple bench testing, the test rack, and SITL CI) due to the calibration offsets being zeroed. --- src/lib/drivers/accelerometer/PX4Accelerometer.cpp | 6 +++--- src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index b75b841445f4..1dab7a09a89c 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -268,11 +268,11 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample) // Apply rotation (before scaling) rotate_3f(_rotation, _integration_raw(0), _integration_raw(1), _integration_raw(2)); - // integrated in microseconds, convert to seconds - const Vector3f delta_velocity_uncalibrated{_integration_raw * 1e-6f * dt * _scale}; + // scale calibration offset to number of samples + const Vector3f offset{_calibration_offset * _integrator_fifo_samples}; // Apply calibration and scale to seconds - const Vector3f delta_velocity{(delta_velocity_uncalibrated - _calibration_offset).emult(_calibration_scale)}; + const Vector3f delta_velocity{((_integration_raw * _scale) - offset).emult(_calibration_scale) * 1e-6f * dt}; // fill sensor_accel_integrated and publish sensor_accel_integrated_s report; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index f47a0c5bef6d..b969c7ecdbf3 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -270,11 +270,11 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample) // Apply rotation (before scaling) rotate_3f(_rotation, _integration_raw(0), _integration_raw(1), _integration_raw(2)); - // integrated in microseconds, convert to seconds - const Vector3f delta_angle_uncalibrated{_integration_raw * 1e-6f * dt * _scale}; + // scale calibration offset to number of samples + const Vector3f offset{_calibration_offset * _integrator_fifo_samples}; // Apply calibration and scale to seconds - const Vector3f delta_angle{delta_angle_uncalibrated - _calibration_offset}; + const Vector3f delta_angle{((_integration_raw * _scale) - offset) * 1e-6f * dt}; // fill sensor_gyro_integrated and publish sensor_gyro_integrated_s report;