From b86c43aa662ae69630fed868d0a75ba4f85d0957 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 17 Nov 2019 21:11:19 +1100
Subject: [PATCH] AP_GPS: prefer RTK MB rover when it has yaw available

this ensures that yaw data is time aligned with position and velocity
data
---
 libraries/AP_GPS/AP_GPS.cpp | 18 ++++++++++++++++--
 1 file changed, 16 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 15ec110174..ab90d7ce47 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -841,9 +841,23 @@ void AP_GPS::update_primary(void)
         primary_instance = 1;
         return;
     }
-    
-    // use switch logic to find best GPS
+
     uint32_t now = AP_HAL::millis();
+
+    // special handling of RTK moving baseline pair. If a rover has a
+    // RTK fixed lock and yaw available then always select it as
+    // primary. This ensures that the yaw data and position/velocity
+    // data is time aligned whenever we provide yaw to the EKF
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER &&
+            state[i].status == GPS_OK_FIX_3D_RTK_FIXED &&
+            state[i].have_gps_yaw) {
+            primary_instance = i;
+            _last_instance_swap_ms = now;
+            return;
+        }
+    }
+    
     // handling switching away from blended GPS
     if (primary_instance == GPS_BLENDED_INSTANCE) {
         primary_instance = 0;