From 969940e98a60da5193bc44f1eebefbd71b359b68 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 31 May 2016 17:31:24 +0100 Subject: [PATCH] df_mpu9250_wrapper: add rotation to internal mag --- .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 13 ++++++++++--- 1 file changed, 10 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 251ce72f2a..fa68568f3b 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 @@ -687,9 +687,16 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) accel_report.z = accel_val_filt(2); if (_mag_enabled) { - mag_report.x = (data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale; - mag_report.y = (data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale; - mag_report.z = (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale; + + math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale, + (data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale, + (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale); + + mag_val = _rotation_matrix * mag_val; + + mag_report.x = mag_val(0); + mag_report.y = mag_val(1); + mag_report.z = mag_val(2); } gyro_report.x_integral = gyro_val_integ(0);