diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index e294c2c7c5..8bba20b859 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -738,10 +738,10 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = { // @Param: OPTIONS // @DisplayName: Optional EKF behaviour - // @Description: This controls optional EKF behaviour. Setting JammingExpected will change the EKF behaviour such that if dead reckoning navigation is possible it will require the preflight alignment GPS quality checks controlled by EK3_GPS_CHECK and EK3_CHECK_SCALE to pass before resuming GPS use if GPS lock is lost for more than 2 seconds to prevent bad GPS measurements from getting fused. If PlaneRequiresGPS is true, EKF3 will not initialize unless the preferred GPS has at least GPS_OK_FIX_3D. - // @Bitmask: 0:JammingExpected,1:PlaneRequiresGPS + // @Description: This controls optional EKF behaviour. Setting JammingExpected will change the EKF behaviour such that if dead reckoning navigation is possible it will require the preflight alignment GPS quality checks controlled by EK3_GPS_CHECK and EK3_CHECK_SCALE to pass before resuming GPS use if GPS lock is lost for more than 2 seconds to prevent bad GPS measurements from getting fused. If SuppressGPSFixCheck is true, EKF3 will initialize despite the preferred GPS not having at least GPS_OK_FIX_3D (this check only applies to forward-flying non-ground vehicles - i.e. Plane in non-VTOL mode). + // @Bitmask: 0:JammingExpected,1:SuppressGPSFixCheck // @User: Advanced - AP_GROUPINFO("OPTIONS", 11, NavEKF3, _options, 2), + AP_GROUPINFO("OPTIONS", 11, NavEKF3, _options, 0), AP_GROUPEND }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index c0d04f09d3..8d380621c9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -455,7 +455,7 @@ private: // enum for processing options enum class Options { JammingExpected = (1<<0), - PlaneRequiresGPS = (1<<1), + SuppressGPSFixCheck = (1<<1), }; // Possible values for _flowUse diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 62868d9797..77b791bb4d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -472,8 +472,9 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void) // update sensor selection (for affinity) update_sensor_selection(); - // If PlaneRequiresGPS bit in EK3_OPTIONS is set, then don't initialise if we are a plane and don't have GPS lock - if ((frontend->_options & (int32_t)NavEKF3::Options::PlaneRequiresGPS) && + // Unless SuppressGPSFixCheck bit in EK3_OPTIONS is set, don't initialise if we are a + // non-sideslip vehicle (most likely a non-VTOL plane and don't have GPS lock. + if (!(frontend->_options & (int32_t)NavEKF3::Options::SuppressGPSFixCheck) && assume_zero_sideslip() && dal.gps().status(preferred_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) { dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string),