From 68986604e9ade0271a34288e461616e02f8a49e9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Jul 2016 09:36:38 +0200 Subject: [PATCH] df_mpu9250_wrapper: calibration after rotation Same as for the df_hmc5883_wrapper. --- .../drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp index 368e120031..427633470a 100644 --- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp +++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp @@ -588,13 +588,18 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) vec_integrated_unused, integral_dt_unused); - math::Vector<3> gyro_val((data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale, - (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale, - (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale); + math::Vector<3> gyro_val(data.gyro_rad_s_x, + data.gyro_rad_s_y, + data.gyro_rad_s_z); // apply sensor rotation on the gyro measurement gyro_val = _rotation_matrix * gyro_val; + // Apply calibration after rotation. + gyro_val(0) = (gyro_val(0) - _gyro_calibration.x_offset) * _gyro_calibration.x_scale; + gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale; + gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale; + _gyro_int.put_with_interval(data.fifo_sample_interval_us, gyro_val, vec_integrated_unused,