Browse Source

AP_AHRS: check EKF status for having horizontal position estimate

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
c415-sdk
Andrew Tridgell 5 years ago
parent
commit
e5e092d077
  1. 14
      libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

14
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@ -1249,11 +1249,15 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const @@ -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) {

Loading…
Cancel
Save