diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 38508e44a9a15..fbabee9b36b70 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -198,6 +198,20 @@ void NavEKF3_core::FuseAirspeed() // select fusion of true airspeed measurements void NavEKF3_core::SelectTasFusion() { + // we don't assume zero sideslip for ground vehicles as EKF could + // be quite sensitive to a rapid spin of the ground vehicle if + // traction is lost + const bool dal_ff = dal.get_fly_forward() && dal.get_vehicle_class() != AP_DAL::VehicleClass::GROUND; + const uint32_t now_ms = imuSampleTime_ms; + if (dal_ff) { + if (fly_forward_start_ms == 0) { + fly_forward_start_ms = now_ms; + } + } else { + fly_forward_start_ms = 0; + } + fly_forward_active = fly_forward_start_ms != 0 && now_ms - fly_forward_start_ms > 10000U; + // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz // If so, don't fuse measurements on this time step to reduce frame over-runs // Only allow one time slip to prevent high rate magnetometer data locking out fusion of other measurements diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index d2179f348cbed..bc575b6c243b9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -627,10 +627,7 @@ bool NavEKF3_core::using_extnav_for_yaw() const */ bool NavEKF3_core::assume_zero_sideslip(void) const { - // we don't assume zero sideslip for ground vehicles as EKF could - // be quite sensitive to a rapid spin of the ground vehicle if - // traction is lost - return dal.get_fly_forward() && dal.get_vehicle_class() != AP_DAL::VehicleClass::GROUND; + return fly_forward_active; } // sets the local NED origin using a LLH location (latitude, longitude, height) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 1d2316f966219..9ec307fdc59c8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -449,6 +449,9 @@ void NavEKF3_core::InitialiseVariables() EKFGSF_yaw_valid_count = 0; effectiveMagCal = effective_magCal(); + + fly_forward_start_ms = 0; + fly_forward_active = false; } // Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary. diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 8ba964b7e88fd..c4fe3920e8b65 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1306,6 +1306,10 @@ class NavEKF3_core : public NavEKF_core_common bool bodyVelFusionDelayed; // true when body frame velocity fusion has been delayed bool bodyVelFusionActive; // true when body frame velocity fusion is active + // time when fly_forward became true + uint32_t fly_forward_start_ms; + bool fly_forward_active; + #if EK3_FEATURE_BODY_ODOM // wheel sensor fusion EKF_obs_buffer_t storedWheelOdm; // body velocity data buffer