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.
This commit is contained in:
Daniel Agar
2020-04-26 13:24:51 -04:00
committed by GitHub
parent 37d5d1b4d2
commit 3832214145
2 changed files with 6 additions and 6 deletions
@@ -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;
+3 -3
View File
@@ -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;