AP_Arming: Allow memoizing ICE state

ArduPilot · Jan 12, 2024 · c66972c · c66972c
1 parent 0cf6160
commit c66972c
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 4 deletions.
diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
@@ -56,6 +56,7 @@
 #include <AP_Scheduler/AP_Scheduler.h>
 #include <AP_KDECAN/AP_KDECAN.h>
 #include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_ICEngine/AP_ICEngine.h>
 
 #if HAL_MAX_CAN_PROTOCOL_DRIVERS
   #include <AP_CANManager/AP_CANManager.h>
@@ -142,7 +143,7 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: Arming options
     // @Description: Options that can be applied to change arming behaviour
-    // @Values: 0:None,1:Disable prearm display,2:Do not send status text on state change
+    // @Bitmask: 0:None,1:Disable prearm display,2:Do not send status text on state change,3:Use the last passing value for INS consistency check if ICE is running
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 9,   AP_Arming, _arming_options, 0),
 
@@ -357,8 +358,31 @@ bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
         return true;
     }
 
-    const Vector3f &prime_accel_vec = ins.get_accel();
+    const uint32_t minimum_time = 10000;
     const uint32_t now = AP_HAL::millis();
+
+    bool passed_before_ice_start = false;
+
+#if AP_ICENGINE_ENABLED
+    if (option_enabled (Option::FREEZE_INS_CONSISTENT_WITH_ICE_RUN)) {
+        const auto *ice = AP::ice();
+        if (ice != nullptr) {
+            if (ice->get_state() >= AP_ICEngine::ICE_START_HEIGHT_DELAY) {
+                // the engine can create a lot of vibrations, if we were valid before it started
+                // we can keep that state and propegate it forward. If the system is highly unstable
+                // with temperature this may cause us to miss a bad IMU, but it's going to be masked
+                // by the engine vibration anyways
+                if (last_accel_pass_ms == 0 || now - last_accel_pass_ms < minimum_time) {
+                    last_accel_pass_ms = 0;
+                } else {
+                    passed_before_ice_start = true;
+                }
+            }
+        }
+    }
+#endif
+
+    const Vector3f &prime_accel_vec = ins.get_accel();
     for(uint8_t i=0; i<accel_count; i++) {
         if (!ins.use_accel(i)) {
             continue;
@@ -380,7 +404,7 @@ bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
         // EKF is less sensitive to Z-axis error
         vec_diff.z *= 0.5f;
 
-        if (vec_diff.length() > threshold) {
+        if (vec_diff.length() > threshold && !passed_before_ice_start) {
             // this sensor disagrees with the primary sensor, so
             // accels are inconsistent:
             last_accel_pass_ms = 0;
@@ -395,7 +419,7 @@ bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
     }
 
     // must pass for at least 10 seconds before we're considered consistent:
-    if (now - last_accel_pass_ms < 10000) {
+    if (now - last_accel_pass_ms < minimum_time) {
         return false;
     }
 

diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h
@@ -141,6 +141,7 @@ class AP_Arming {
     enum class Option : int32_t {
         DISABLE_PREARM_DISPLAY             = (1U << 0),
         DISABLE_STATUSTEXT_ON_STATE_CHANGE = (1U << 1),
+        FREEZE_INS_CONSISTENT_WITH_ICE_RUN = (1U << 2),
     };
     bool option_enabled(Option option) const {
         return (_arming_options & uint32_t(option)) != 0;