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

ekf2: stop mag fusion when there is no data anymore

Browse Source
This commit is contained in:
bresch 2023-02-17 11:18:08 +01:00 committed by Daniel Agar
parent deabe9a38d
commit 8ebf47edb1
2 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf2/EKF/common.h
View File

@ -70,7 +70,8 @@ static constexpr uint64_t BARO_MAX_INTERVAL = 200e3; ///< Maximum allowable
static constexpr uint64_t EV_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between external vision system measurements (uSec)
static constexpr uint64_t GNSS_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between GNSS measurements (uSec)
static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
static constexpr uint64_t RNG_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between range finder measurements (uSec)
static constexpr uint64_t RNG_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between range finder measurements (uSec)
static constexpr uint64_t MAG_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between magnetic field measurements (uSec)
// bad accelerometer detection and mitigation
static constexpr uint64_t BADACC_PROBATION = 10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)

4
src/modules/ekf2/EKF/mag_control.cpp
View File

@ -107,6 +107,10 @@ void Ekf::controlMagFusion()
_aid_src_mag.timestamp_sample = mag_sample.time_us;
mag_observation.copyTo(_aid_src_mag.observation);
mag_innov.copyTo(_aid_src_mag.innovation);
} else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) {
// No data anymore. Stop until it comes back.
stopMagFusion();
}
}