AP_AHRS: add using_extnav_for_yaw

Icatme · Aug 30, 2021 · 1c08866 · 1c08866
1 parent 63e579d
commit 1c08866
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 1 deletion.
diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -3047,6 +3047,31 @@ bool AP_AHRS::using_noncompass_for_yaw(void) const
     return false;
 }
 
+// check if external nav is providing yaw
+bool AP_AHRS::using_extnav_for_yaw(void) const
+{
+    switch (active_EKF_type()) {
+#if HAL_NAVEKF2_AVAILABLE
+    case EKFType::TWO:
+        return EKF2.isExtNavUsedForYaw();
+#endif
+    case EKFType::NONE:
+#if HAL_NAVEKF3_AVAILABLE
+    case EKFType::THREE:
+        return EKF3.using_extnav_for_yaw();
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKFType::SIM:
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+#endif
+        return false;
+    }
+    // since there is no default case above, this is unreachable
+    return false;
+}
+
 // set and save the alt noise parameter value
 void AP_AHRS::set_alt_measurement_noise(float noise)
 {

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
@@ -329,6 +329,9 @@ class AP_AHRS : public AP_AHRS_DCM {
     // check if non-compass sensor is providing yaw.  Allows compass pre-arm checks to be bypassed
     bool using_noncompass_for_yaw(void) const override;
 
+    // check if external nav is providing yaw
+    bool using_extnav_for_yaw(void) const override;
+
     // set and save the ALT_M_NSE parameter value
     void set_alt_measurement_noise(float noise) override;
 

diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h
@@ -92,7 +92,10 @@ class AP_AHRS_Backend
 
     // check if non-compass sensor is providing yaw.  Allows compass pre-arm checks to be bypassed
     virtual bool using_noncompass_for_yaw(void) const { return false; }
-
+
+    // check if external nav is providing yaw
+    virtual bool using_extnav_for_yaw(void) const { return false; }
+
     // request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
     virtual void request_yaw_reset(void) {}