fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 17:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

df_mpu9250_wrapper: add rotation to internal mag

Browse Source
This commit is contained in:
Julian Oes
2016-06-01 08:39:10 +02:00
committed by Lorenz Meier
parent a7ae7907f7
commit 969940e98a
1 changed files with 10 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+10 -3
View File
@@ -687,9 +687,16 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
accel_report.z = accel_val_filt(2); accel_report.z = accel_val_filt(2);
if (_mag_enabled) { 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; math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale,
mag_report.z = (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_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); gyro_report.x_integral = gyro_val_integ(0);