Browse Source

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.
sbg
Daniel Agar 5 years ago committed by GitHub
parent
commit
3832214145
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 6
      src/lib/drivers/accelerometer/PX4Accelerometer.cpp
  2. 6
      src/lib/drivers/gyroscope/PX4Gyroscope.cpp

6
src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@ -268,11 +268,11 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample) @@ -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;

6
src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@ -270,11 +270,11 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample) @@ -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;

Loading…
Cancel
Save