diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 8d23e04f549444..e2776eb7a9b100 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -178,6 +178,13 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = { // index 17 + // @Param: OPTIONS + // @DisplayName: Optional AHRS behaviour + // @Description: This conrols optional AHRS behaviour. Setting DisableDCMFallback will change the AHRS behaviour for fixed wing aircraft to not fall back to DCM when the EKF stops fusing GPS measurements while there is 3D GPS lock + // @Bitmask: 0:DisableDCMFallback + // @User: Advanced + AP_GROUPINFO("OPTIONS", 18, AP_AHRS, _options, 0), + AP_GROUPEND }; @@ -1791,6 +1798,7 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const wait for good GPS position on fixed wing and rover */ if (ret != EKFType::NONE && + !option_set(Options::DISABLE_DCM_FALLBACK) && (_vehicle_class == VehicleClass::FIXED_WING || _vehicle_class == VehicleClass::GROUND)) { if (!dcm.yaw_source_available() && !fly_forward) { @@ -1826,14 +1834,13 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const if (hal.util->get_soft_armed() && (!filt_state.flags.using_gps || !filt_state.flags.horiz_pos_abs) && - !filt_state.flags.horiz_pos_rel && should_use_gps && AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) { // if the EKF is not fusing GPS or doesn't have a 2D fix - // and is not doing ground or wind relative dead reckoning // and we have a 3D lock, then plane and rover would - // prefer to use the GPS position from DCM. - // Note: This is a last resort fallback and makes the navigation highly vulnerable to GPS noise. + // 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) { diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index c75b268f0338ad..c1f46859e7da56 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -952,6 +952,15 @@ class AP_AHRS { Vector3f velocity_NED; bool velocity_NED_ok; } state; + + enum class Options : uint16_t { + DISABLE_DCM_FALLBACK=(1U<<0), + }; + AP_Int16 _options; + + bool option_set(Options option) const { + return (_options & uint16_t(option)) != 0; + } }; namespace AP {