fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 13:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update EKF mag interface

Browse Source
This commit is contained in:
kamilritz
2020-01-21 14:38:31 +01:00
committed by Mathieu Bresciani
parent 009ba638f5
commit 439a20bfcb
1 changed files with 6 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+6 -5
View File
@@ -893,12 +893,13 @@ void Ekf2::Run()
PX4_INFO("Mag sensor ID changed to %i", _param_ekf2_magbias_id.get());
}
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();
magSample mag_sample;
mag_sample.mag(0) = magnetometer.magnetometer_ga[0] - _param_ekf2_magbias_x.get();
mag_sample.mag(1) = magnetometer.magnetometer_ga[1] - _param_ekf2_magbias_y.get();
mag_sample.mag(2) = magnetometer.magnetometer_ga[2] - _param_ekf2_magbias_z.get();
mag_sample.time_us = magnetometer.timestamp;
_ekf.setMagData(magnetometer.timestamp, magnetometer_ga);
_ekf.setMagData(mag_sample);
ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}