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

@@ -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;

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;