From ae66085f89ea6197b2c469c63f9c6d5d70c62f3c Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Thu, 21 Jul 2016 18:48:28 +0200 Subject: [PATCH] MPU6050: Apply calibration after coordinate frame rotation --- .../df_mpu6050_wrapper/df_mpu6050_wrapper.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp index 4f9f3d4780..08fcb50217 100644 --- a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp +++ b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp @@ -432,25 +432,29 @@ int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data) math::Vector<3> vec_integrated_unused; uint64_t integral_dt_unused; - math::Vector<3> accel_val((data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale, - (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale, - (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale); + math::Vector<3> accel_val(data.accel_m_s2_x, data.accel_m_s2_y, data.accel_m_s2_z); // apply sensor rotation on the accel measurement accel_val = _rotation_matrix * accel_val; + accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale; + accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale; + accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale; + _accel_int.put_with_interval(data.fifo_sample_interval_us, accel_val, 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; + 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,