AP_AHRS: use enum class for takeoff/touchdown states

Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -428,6 +428,8 @@ const Vector3f &AP_AHRS_NavEKF::get_accel_ef_blended(void) const
     return _accel_ef_ekf_blended;
 }
 
+#include <stdio.h>
+
 void AP_AHRS_NavEKF::reset(bool recover_eulers)
 {
     // support locked access functions to AHRS data
@@ -1995,6 +1997,22 @@ bool AP_AHRS_NavEKF::get_hgt_ctrl_limit(float& limit) const
 // this is not related to terrain following
 void AP_AHRS_NavEKF::set_terrain_hgt_stable(bool stable)
 {
+    switch (terrainHgtStableState) {
+    case TriState::UNKNOWN:
+        break;
+    case TriState::True:
+        if (stable) {
+            return;
+        }
+        break;
+    case TriState::False:
+        if (!stable) {
+            return;
+        }
+        break;
+    }
+    terrainHgtStableState = (TriState)stable;
+
 #if HAL_NAVEKF2_AVAILABLE
     EKF2.setTerrainHgtStable(stable);
 #endif
@@ -2113,6 +2131,22 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar,
 
 void AP_AHRS_NavEKF::setTakeoffExpected(bool val)
 {
+    switch (takeoffExpectedState) {
+    case TriState::UNKNOWN:
+        break;
+    case TriState::True:
+        if (val) {
+            return;
+        }
+        break;
+    case TriState::False:
+        if (!val) {
+            return;
+        }
+        break;
+    }
+    takeoffExpectedState = (TriState)val;
+
 #if HAL_NAVEKF2_AVAILABLE
     EKF2.setTakeoffExpected(val);
 #endif
@@ -2123,6 +2157,22 @@ void AP_AHRS_NavEKF::setTakeoffExpected(bool val)
 
 void AP_AHRS_NavEKF::setTouchdownExpected(bool val)
 {
+    switch (touchdownExpectedState) {
+    case TriState::UNKNOWN:
+        break;
+    case TriState::True:
+        if (val) {
+            return;
+        }
+        break;
+    case TriState::False:
+        if (!val) {
+            return;
+        }
+        break;
+    }
+    touchdownExpectedState = (TriState)val;
+
 #if HAL_NAVEKF2_AVAILABLE
     EKF2.setTouchdownExpected(val);
 #endif

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -364,6 +364,16 @@ private:
     // get the index of the current primary IMU
     uint8_t get_primary_IMU_index(void) const;
 
+    // avoid setting current state repeatedly across all cores on all EKFs:
+    enum class TriState {
+        False = 0,
+        True = 1,
+        UNKNOWN = 3,
+    };
+    TriState touchdownExpectedState = TriState::UNKNOWN;
+    TriState takeoffExpectedState = TriState::UNKNOWN;
+    TriState terrainHgtStableState = TriState::UNKNOWN;
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     SITL::SITL *_sitl;
     uint32_t _last_body_odm_update_ms;