diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index c8366411ab..db8617ff10 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -34,7 +34,7 @@ AP_AHRS_NavEKF::AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gp AP_AHRS_DCM(ins, baro, gps), EKF1(_EKF1), EKF2(_EKF2), - _flags(flags) + _ekf_flags(flags) { _dcm_matrix.identity(); } @@ -713,9 +713,15 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const #endif } + /* + 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 + */ if (ret != EKF_TYPE_NONE && - (_vehicle_class == AHRS_VEHICLE_FIXED_WING || - _vehicle_class == AHRS_VEHICLE_GROUND)) { + (_vehicle_class == AHRS_VEHICLE_FIXED_WING || + _vehicle_class == AHRS_VEHICLE_GROUND) && + _flags.fly_forward) { nav_filter_status filt_state; if (ret == EKF_TYPE1) { EKF1.getFilterStatus(filt_state); diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 3740bf1c2f..be4e15d837 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -217,7 +217,7 @@ private: EKF_TYPE active_EKF_type(void) const; bool always_use_EKF() const { - return _flags & FLAG_ALWAYS_USE_EKF; + return _ekf_flags & FLAG_ALWAYS_USE_EKF; } NavEKF &EKF1; @@ -232,7 +232,7 @@ private: Vector3f _accel_ef_ekf_blended; const uint16_t startup_delay_ms = 1000; uint32_t start_time_ms = 0; - Flags _flags; + Flags _ekf_flags; uint8_t ekf_type(void) const; void update_DCM(void);