fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 07:28:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: prevent false mag fault detection

Browse Source 
A false positive could be triggered if velocity fusion started,
then stopped after takeoff and only position fusion started again
(because velocity fusion timed out and had a timestamp > time_last_on_ground).
We now also check if the fusion timeout is due to the innovation being
rejected (and not just a temporary check failure or data interruption).
This commit is contained in:
bresch
2025-12-15 14:06:04 +01:00
committed by Mathieu Bresciani
parent 14186cf74f
commit d62f112017
1 changed files with 10 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
+10 -5
View File
@@ -104,12 +104,17 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
bool do_vel_pos_reset = false;
if (!_control_status.flags.gnss_fault && (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos)) {
if (!_control_status.flags.gnss_fault && _control_status.flags.in_air && isYawFailure()) {
const bool velocity_fusion_failure = _aid_src_gnss_vel.innovation_rejected
&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us);
if (_control_status.flags.in_air
&& isYawFailure()
&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
const bool position_fusion_failure = _aid_src_gnss_pos.innovation_rejected
&& isTimedOut(_time_last_hor_pos_fuse, _params.EKFGSF_reset_delay)
&& (_time_last_hor_pos_fuse > _time_last_on_ground_us);
if ((_control_status.flags.gnss_vel && velocity_fusion_failure)
|| (_control_status.flags.gnss_pos && position_fusion_failure)) {
do_vel_pos_reset = tryYawEmergencyReset();
}
}