AP_AHRS: add ad use active_estimates pointer

ArduPilot · Dec 23, 2024 · be1faf1 · be1faf1
1 parent bac6648
commit be1faf1
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 13 deletions.
diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -247,8 +247,6 @@ void AP_AHRS::init()
     }
 #endif
 
-    last_active_ekf_type = (EKFType)_ekf_type.get();
-
     // init backends
 #if AP_AHRS_DCM_ENABLED
     dcm.init();
@@ -452,16 +450,19 @@ void AP_AHRS::update(bool skip_ins_update)
 #if AP_AHRS_DCM_ENABLED
         case EKFType::DCM:
             shortname = "DCM";
+            active_estimates = &dcm_estimates;
             break;
 #endif
 #if AP_AHRS_SIM_ENABLED
         case EKFType::SIM:
             shortname = "SIM";
+            active_estimates = &sim_estimates;
             break;
 #endif
 #if AP_AHRS_EXTERNAL_ENABLED
         case EKFType::EXTERNAL:
             shortname = "External";
+            active_estimates = &external_estimates;
             break;
 #endif
 #if HAL_NAVEKF3_AVAILABLE
@@ -786,8 +787,14 @@ bool AP_AHRS::_get_location(Location &loc) const
     switch (active_EKF_type()) {
 #if AP_AHRS_DCM_ENABLED
     case EKFType::DCM:
-        return dcm_estimates.get_location(loc);
 #endif
+#if AP_AHRS_SIM_ENABLED
+    case EKFType::SIM:
+#endif
+#if AP_AHRS_EXTERNAL_ENABLED
+    case EKFType::EXTERNAL:
+#endif
+        return active_estimates->get_location(loc);
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO:
         if (EKF2.getLLH(loc)) {
@@ -803,16 +810,6 @@ bool AP_AHRS::_get_location(Location &loc) const
         }
         break;
 #endif
-
-#if AP_AHRS_SIM_ENABLED
-    case EKFType::SIM:
-        return sim_estimates.get_location(loc);
-#endif
-
-#if AP_AHRS_EXTERNAL_ENABLED
-    case EKFType::EXTERNAL:
-        return external_estimates.get_location(loc);
-#endif
     }
 
 #if AP_AHRS_DCM_ENABLED

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
@@ -725,6 +725,7 @@ class AP_AHRS {
     AP_Int8 _gps_minsats;
 
     EKFType active_EKF_type(void) const { return state.active_EKF; }
+    AP_AHRS_Backend::Estimates *active_estimates;
 
     bool always_use_EKF() const {
         return _ekf_flags & FLAG_ALWAYS_USE_EKF;