AP_NavEKF3: added in a check for high velocity innovation

if using GPS and our horizontal velocity innovation is high then mark
the filter as unhealthy, which causes an arming failure

this helps with ArduPilot#29384

Author: tridge

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -1157,6 +1157,10 @@ bool NavEKF3::pre_arm_check(bool requires_position, char *failure_msg, uint8_t f
             }
             return false;
         }
+        // run per-core pre-arm checks
+        if (!core[i].pre_arm_check(requires_position, failure_msg, failure_msg_len)) {
+            return false;
+        }
     }
     return true;
 }

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -32,6 +32,31 @@ bool NavEKF3_core::healthy(void) const
     return true;
 }
 
+/*
+  per-core pre-arm checks. returns false if we fail arming checks, in
+  which case the buffer will be populated with a failure message
+  requires_position should be true if horizontal position configuration should be checked
+*/
+bool NavEKF3_core::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
+{
+    if (requires_position) {
+        // additional checks when position is required, used by pre-arm checks
+        const float max_vel_innovation = 2.0;
+        const float hvel_innovation = sqrtf(sq(innovVelPos[0])+sq(innovVelPos[1]));
+        if (onGround && PV_AidingMode == AID_ABSOLUTE &&
+            frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS) &&
+            hvel_innovation > max_vel_innovation) {
+            // more than 2 m/s horizontal velocity innovation on the ground
+            dal.snprintf(failure_msg, failure_msg_len,
+                         "EKF3[%u] vel error %.1f", unsigned(core_index)+1, hvel_innovation);
+            return false;
+        }
+    }
+
+    // all OK
+    return true;
+}
+
 // Return a consolidated error score where higher numbers represent larger errors
 // Intended to be used by the front-end to determine which is the primary EKF
 float NavEKF3_core::errorScore() const

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -464,6 +464,12 @@ class NavEKF3_core : public NavEKF_core_common
     // get a yaw estimator instance
     const EKFGSF_yaw *get_yawEstimator(void) const { return yawEstimator; }
 
+    // per-core pre-arm checks. returns false if we fail arming
+    // checks, in which case the buffer will be populated with a
+    // failure message
+    // requires_position should be true if horizontal position configuration should be checked
+    bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const;
+    
 private:
     EKFGSF_yaw *yawEstimator;
     AP_DAL &dal;