From 0fb168d6ab6b40db47ba9ae3129fb9f23c6276e1 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 22 Dec 2020 16:15:21 +0900
Subject: [PATCH] AP_NavEKF: source supports GSF yaw

---
 libraries/AP_NavEKF/AP_NavEKF_Source.cpp | 12 ++++++++----
 libraries/AP_NavEKF/AP_NavEKF_Source.h   |  3 ++-
 2 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
index 84b267923e..066a7c496a 100644
--- a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
@@ -52,7 +52,7 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
     // @Param: 1_YAW
     // @DisplayName: Yaw Source
     // @Description: Yaw Source
-    // @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback, 6:ExternalNav
+    // @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback, 6:ExternalNav, 8:GSF
     // @User: Advanced
     AP_GROUPINFO("1_YAW", 5, AP_NavEKF_Source, _source_set[0].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::COMPASS),
 
@@ -88,7 +88,7 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
     // @Param: 2_YAW
     // @DisplayName: Yaw Source (Secondary)
     // @Description: Yaw Source (Secondary)
-    // @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback, 6:ExternalNav
+    // @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback, 6:ExternalNav, 8:GSF
     // @User: Advanced
     AP_GROUPINFO("2_YAW", 10, AP_NavEKF_Source, _source_set[1].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::NONE),
 #endif
@@ -125,7 +125,7 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
     // @Param: 3_YAW
     // @DisplayName: Yaw Source (Tertiary)
     // @Description: Yaw Source (Tertiary)
-    // @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback, 6:ExternalNav
+    // @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback, 6:ExternalNav, 8:GSF
     // @User: Advanced
     AP_GROUPINFO("3_YAW", 15, AP_NavEKF_Source, _source_set[2].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::NONE),
 #endif
@@ -274,7 +274,8 @@ bool AP_NavEKF_Source::usingGPS() const
     return getPosXYSource() == SourceXY::GPS ||
            getPosZSource() == SourceZ::GPS ||
            getVelXYSource() == SourceXY::GPS ||
-           getVelZSource() == SourceZ::GPS;
+           getVelZSource() == SourceZ::GPS ||
+           getYawSource() == SourceYaw::GSF;
 }
 
 // true if some parameters have been configured (used during parameter conversion)
@@ -415,6 +416,9 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
         case SourceYaw::EXTNAV:
             visualodom_required = true;
             break;
+        case SourceYaw::GSF:
+            gps_required = true;
+            break;
         default:
             // invalid yaw value
             hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_YAW", (int)i+1);
diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.h b/libraries/AP_NavEKF/AP_NavEKF_Source.h
index 31087c8f95..2c394c8a57 100644
--- a/libraries/AP_NavEKF/AP_NavEKF_Source.h
+++ b/libraries/AP_NavEKF/AP_NavEKF_Source.h
@@ -42,7 +42,8 @@ public:
         COMPASS = 1,
         EXTERNAL = 2,
         EXTERNAL_COMPASS_FALLBACK = 3,
-        EXTNAV = 6
+        EXTNAV = 6,
+        GSF = 8
     };
 
     // enum for OPTIONS parameter