fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 21:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

df_hmc5883_wrapper: use calibration after rotation

Browse Source 
The calibration values need to be applied after the rotation, otherwise
the offsets and scale can be applied to the wrong axes.
This commit is contained in:
Julian Oes 2016-07-22 09:35:38 +02:00 committed by Lorenz Meier
parent 2a15578f8d
commit 1dd2c94949
1 changed files with 7 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;