Browse Source

AP_AHRS: fixed EKF GPS arming check for fixed wing

when in a quadplane mode we set fly_forward to zero. We need to ensure
the GPS checks for EKF health are applied when doing arming checks, so
we use the checks always when disarmed
master
Andrew Tridgell 9 years ago
parent
commit
bf1e0e1536
  1. 7
      libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

7
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@ -865,12 +865,15 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const @@ -865,12 +865,15 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
/*
fixed wing and rover when in fly_forward mode will fall back to
DCM if the EKF doesn't have GPS. This is the safest option as
DCM is very robust
DCM is very robust. Note that we also check the filter status
when fly_forward is false and we are disarmed. This is to ensure
that the arming checks do wait for good GPS position on fixed
wing and rover
*/
if (ret != EKF_TYPE_NONE &&
(_vehicle_class == AHRS_VEHICLE_FIXED_WING ||
_vehicle_class == AHRS_VEHICLE_GROUND) &&
_flags.fly_forward) {
(_flags.fly_forward || !hal.util->get_soft_armed())) {
nav_filter_status filt_state;
if (ret == EKF_TYPE2) {
EKF2.getFilterStatus(-1,filt_state);

Loading…
Cancel
Save