AP_NavEKF3: Allow adjustment of on ground not moving test sensitivity

Also re-tune default sensitivity based on user supplied logs with noisy gyro data.

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -696,6 +696,14 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = {
     // @User: Advanced
     AP_GROUPINFO("DRAG_MCOEF", 5, NavEKF3, _momentumDragCoef, 0.0f),
 
+    // @Param: OGNM_TEST_SF
+    // @DisplayName: On ground not moving test scale factor
+    // @Description: This parameter is adjust the sensitivity of the on ground not moving test which is used to assist with learning the yaw gyro bias and stopping yaw drift before flight when operating without a yaw sensor. Bigger values allow the detection of a not moving condition with noiser IMU data. Check the XKFM data logged when the vehicle is on ground not moving and adjust the value of OGNM_TEST_SF to be slightly higher than the maximum value of the XKFM.ADR, XKFM.ALR, XKFM.GDR and XKFM.GLR test levels.
+    // @Range: 1.0 10.0
+    // @Increment: 0.5
+    // @User: Advanced
+    AP_GROUPINFO("OGNM_TEST_SF", 6, NavEKF3, _ognmTestScaleFactor, 2.0f),
+
     AP_GROUPEND
 };
 

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -447,6 +447,7 @@ private:
     AP_Float _ballisticCoef_y;      // ballistic coefficient measured for flow in Y body frame directions
     AP_Float _momentumDragCoef;     // lift rotor momentum drag coefficient
     AP_Int8 _betaMask;              // Bitmask controlling when sideslip angle fusion is used to estimate non wind states
+    AP_Float _ognmTestScaleFactor;  // Scale factor applied to the thresholds used by the on ground not moving test
 
 // Possible values for _flowUse
 #define FLOW_USE_NONE    0

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -1319,7 +1319,7 @@ void NavEKF3_core::updateMovementCheck(void)
     }
 
     const float gyro_limit = radians(3.0f);
-    const float gyro_diff_limit = 0.1;
+    const float gyro_diff_limit = 0.2f;
     const float accel_limit = 1.0f;
     const float accel_diff_limit = 5.0f;
     const float hysteresis_ratio = 0.7f;
@@ -1355,18 +1355,18 @@ void NavEKF3_core::updateMovementCheck(void)
     const float accel_diff_ratio = accel_diff / accel_diff_limit;
     bool logStatusChange = false;
     if (onGroundNotMoving) {
-        if (gyro_length_ratio > 1.0f ||
-            fabsf(accel_length_ratio) > 1.0f ||
-            gyro_diff_ratio > 1.0f ||
-            accel_diff_ratio > 1.0f)
+        if (gyro_length_ratio > frontend->_ognmTestScaleFactor ||
+            fabsf(accel_length_ratio) > frontend->_ognmTestScaleFactor ||
+            gyro_diff_ratio > frontend->_ognmTestScaleFactor ||
+            accel_diff_ratio > frontend->_ognmTestScaleFactor)
         {
             onGroundNotMoving = false;
             logStatusChange = true;
         }
-    } else if (gyro_length_ratio < hysteresis_ratio &&
-            fabsf(accel_length_ratio) < hysteresis_ratio &&
-            gyro_diff_ratio < hysteresis_ratio &&
-            accel_diff_ratio < hysteresis_ratio)
+    } else if (gyro_length_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
+            fabsf(accel_length_ratio) < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
+            gyro_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
+            accel_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio)
     {
         onGroundNotMoving = true;
         logStatusChange = true;