Browse Source

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.
sbg
Julian Oes 9 years ago committed by Lorenz Meier
parent
commit
1dd2c94949
  1. 15
      src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp

15
src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp

@ -281,18 +281,17 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data) @@ -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;

Loading…
Cancel
Save