AP_NavEKF3: delay use of zero-sideslip for 10 seconds

this is a test patch to demonstrate in replay that delaying sideslip fusion for 10s helps with real log data
tridge · Oct 19, 2023 · 2f091c0 · 2f091c0
1 parent fbdd4d6
commit 2f091c0
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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<wheel_odm_elements> storedWheelOdm;    // body velocity data buffer