From 1dd2c94949034cf41b42dcbf54fe65a5ef0d67fb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Jul 2016 09:35:38 +0200 Subject: [PATCH] df_hmc5883_wrapper: use calibration after rotation The calibration values need to be applied after the rotation, otherwise the offsets and scale can be applied to the wrong axes. --- .../df_hmc5883_wrapper/df_hmc5883_wrapper.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp index 978977048a..ba755d134d 100644 --- a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp +++ b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp @@ -281,18 +281,17 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data) mag_report.y_raw = NAN; mag_report.z_raw = NAN; - - math::Vector<3> mag_val((data.field_x_ga - _mag_calibration.x_offset) * _mag_calibration.x_scale, - (data.field_y_ga - _mag_calibration.y_offset) * _mag_calibration.y_scale, - (data.field_z_ga - _mag_calibration.z_offset) * _mag_calibration.z_scale); + math::Vector<3> mag_val(data.field_x_ga, + data.field_y_ga, + data.field_z_ga); // apply sensor rotation on the accel measurement mag_val = _rotation_matrix * mag_val; - mag_report.x = mag_val(0); - mag_report.y = mag_val(1); - mag_report.z = mag_val(2); - + // Apply calibration after rotation. + mag_report.x = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale; + mag_report.y = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale; + mag_report.z = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale; // TODO: get these right //mag_report.scaling = -1.0f;