dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add missing mag calibration biases

This commit is contained in:
kamilritz 2020-01-17 12:12:09 +01:00 committed by Roman Bapst
parent a3d2254e44
commit 04e09d772e
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/ekf2/ekf2_main.cpp
View File

@ -819,7 +819,12 @@ void Ekf2::Run()
PX4_INFO("Mag sensor ID changed to %i", _param_ekf2_magbias_id.get());
}
_ekf.setMagData(magnetometer.timestamp, magnetometer.magnetometer_ga);
float magnetometer_ga [3];
magnetometer_ga[0] = magnetometer.magnetometer_ga[0] - _param_ekf2_magbias_x.get();
magnetometer_ga[1] = magnetometer.magnetometer_ga[1] - _param_ekf2_magbias_y.get();
magnetometer_ga[2] = magnetometer.magnetometer_ga[2] - _param_ekf2_magbias_z.get();
_ekf.setMagData(magnetometer.timestamp, magnetometer_ga);
ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}