From dc5d1d099b0bbca2e25e20c0fc8a327e80b68698 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 17 Jun 2020 15:35:07 +0900
Subject: [PATCH] AP_AHRS: add set_posvelyaw_source_set

---
 libraries/AP_AHRS/AP_AHRS.h          | 3 +++
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 8 ++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   | 3 +++
 3 files changed, 14 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 80ef9a71d3..fba2a1af63 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -186,6 +186,9 @@ public:
     // request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
     virtual void request_yaw_reset(void) {}
 
+    // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
+    virtual void set_posvelyaw_source_set(uint8_t source_set_idx) {}
+
     // Euler angles (radians)
     float roll;
     float pitch;
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 123408209d..ef16622ba3 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -2339,6 +2339,14 @@ void AP_AHRS_NavEKF::request_yaw_reset(void)
     }
 }
 
+// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
+void AP_AHRS_NavEKF::set_posvelyaw_source_set(uint8_t source_set_idx)
+{
+#if HAL_NAVEKF3_AVAILABLE
+    EKF3.setPosVelYawSourceSet(source_set_idx);
+#endif
+}
+
 void AP_AHRS_NavEKF::Log_Write()
 {
 #if HAL_NAVEKF2_AVAILABLE
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index ab3fc8edbb..305be3fb18 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -299,6 +299,9 @@ public:
     // request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
     void request_yaw_reset(void) override;
 
+    // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
+    void set_posvelyaw_source_set(uint8_t source_set_idx) override;
+
     void Log_Write();
 
     // check whether external navigation is providing yaw.  Allows compass pre-arm checks to be bypassed