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

ekf2: push mag cal reset to delayed time horizon

Browse Source
This commit is contained in:
Daniel Agar
2022-12-13 10:24:02 -05:00
parent 57b82af3a0
commit 805ffa9d0b
5 changed files with 32 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+2 -3
View File
@@ -1972,7 +1972,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
// check if magnetometer has changed
if (magnetometer.device_id != _device_id_mag) {
if (_device_id_mag != 0) {
PX4_WARN("%d - mag sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_mag, magnetometer.device_id);
PX4_DEBUG("%d - mag sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_mag, magnetometer.device_id);
}
reset = true;
@@ -1984,7 +1984,6 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
}
if (reset) {
_ekf.resetMagBiasAndYaw();
_device_id_mag = magnetometer.device_id;
_mag_calibration_count = magnetometer.calibration_count;
@@ -1992,7 +1991,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
_mag_cal = {};
}
_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}});
_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}, reset});
ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);