Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: check EKF status for having horizontal position estimate

Browse Source 
this will allow a fixed wing to fall back to DCM if the EKF stops
providing an absolute position while we have 3D GPS lock. The
using_gps flag is not enough, as lagged GPS data can lead to the EKF
stopping fusing when the data is behind the fusion time horizon. In
that case EKF3 gives using_gps=1 but sets horiz_pos_abs=0
This commit is contained in:
Andrew Tridgell 2020-08-21 15:08:51 +10:00
parent 87c7781be2
commit e5e092d077
1 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1249,11 +1249,15 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const
get_filter_status(filt_state);
}
#endif
if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// if the EKF is not fusing GPS and we have a 3D lock, then
// plane and rover would prefer to use the GPS position from
// DCM. This is a safety net while some issues with the EKF
// get sorted out
if (hal.util->get_soft_armed() &&
(!filt_state.flags.using_gps ||
!filt_state.flags.horiz_pos_abs) &&
AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// if the EKF is not fusing GPS or doesn't have a 2D fix
// and we have a 3D lock, then plane and rover would
// prefer to use the GPS position from DCM. This is a
// safety net while some issues with the EKF get sorted
// out
return EKFType::NONE;
}
if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) {