AP_GPS: change handling of moving baseline yaw

this changes yaw handling in a few ways:

 - GPS yaw now has a timestamp associated with the yaw separate from
   the timestamp associated with the GPS fix

 - we no longer force the primary to change to the UBLOX MB rover when
   it has a GPS yaw. This means we don't change GPS primary due to GPS
   loss, which keeps the GPS more stable. It also increases accuracy
   as the rover is always less accurate in position and velocity than
   the base

 - now we force the primary to be the MB base if the other GPS is a
   rover and the base has GPS lock

libraries/AP_GPS/AP_GPS.cpp

@@ -984,45 +984,26 @@ void AP_GPS::update_primary(void)
 
     uint32_t now = AP_HAL::millis();
 
-    // special handling of RTK moving baseline pair. If a rover has a
+    // special handling of RTK moving baseline pair. Always use the
-    // RTK fixed lock and yaw available then always select it as
+    // base as the rover position is derived from the base, which
-    // primary. This ensures that the yaw data and position/velocity
+    // means the rover always has worse position and velocity than the
-    // data is time aligned whenever we provide yaw to the EKF
+    // base. This overrides the normal logic which would select the
+    // rover as it typically is in fix type 6 (RTK) whereas base is
+    // usually fix type 3
     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) {
-            if (primary_instance != i) {
-                _last_instance_swap_ms = now;
-                primary_instance = i;
-            }
-            return;
-        }
-        /*
-          if this is a BASE and the other GPS is a MB rover, then we
-          should force the use of the BASE GPS if the rover doesn't
-          have a yaw lock. This is important as when the rover doesn't
-          have a lock it will often report a higher status than the
-          base (eg. status=4), but the velocity and position data can
-          be very bad. As the rover is getting it's base position from
-          the base GPS then the position and velocity are expected to
-          be worse than the base GPS unless it has a full moving
-          baseline lock
-         */
-        const uint8_t i2 = i^1; // the other GPS in the pair
         if (_type[i] == GPS_TYPE_UBLOX_RTK_BASE &&
-            state[i].status >= GPS_OK_FIX_3D &&
+            _type[i^1] == GPS_TYPE_UBLOX_RTK_ROVER &&
-            _type[i2] == GPS_TYPE_UBLOX_RTK_ROVER &&
+            ((state[i].status >= GPS_OK_FIX_3D) || (state[i].status >= state[i^1].status))) {
-            (state[i2].status != GPS_OK_FIX_3D_RTK_FIXED ||
-             !state[i2].have_gps_yaw)) {
             if (primary_instance != i) {
                 _last_instance_swap_ms = now;
                 primary_instance = i;
             }
+            // don't do any more switching logic. We don't want the
+            // RTK status of the rover to cause a switch
             return;
         }
     }
-    
+
     // handling switching away from blended GPS
     if (primary_instance == GPS_BLENDED_INSTANCE) {
         primary_instance = 0;
@@ -1195,7 +1176,8 @@ uint16_t AP_GPS::gps_yaw_cdeg(uint8_t instance) const
         return 0;
     }
     float yaw_deg, accuracy_deg;
-    if (!gps_yaw_deg(instance, yaw_deg, accuracy_deg)) {
+    uint32_t time_ms;
+    if (!gps_yaw_deg(instance, yaw_deg, accuracy_deg, time_ms)) {
         return 65535;
     }
     int yaw_cd = wrap_360_cd(yaw_deg * 100);
@@ -1944,7 +1926,8 @@ void AP_GPS::Write_GPS(uint8_t i)
     const struct Location &loc = location(i);
 
     float yaw_deg=0, yaw_accuracy_deg=0;
-    gps_yaw_deg(i, yaw_deg, yaw_accuracy_deg);
+    uint32_t yaw_time_ms;
+    gps_yaw_deg(i, yaw_deg, yaw_accuracy_deg, yaw_time_ms);
 
     const struct log_GPS pkt {
         LOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
@@ -1987,6 +1970,41 @@ void AP_GPS::Write_GPS(uint8_t i)
     AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
 }
 
+/*
+  get GPS based yaw
+ */
+bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const
+{
+#if GPS_MAX_RECEIVERS > 1
+    if (instance < GPS_MAX_RECEIVERS &&
+        _type[instance] == GPS_TYPE_UBLOX_RTK_BASE &&
+        _type[instance^1] == GPS_TYPE_UBLOX_RTK_ROVER) {
+        // return the yaw from the rover
+        instance ^= 1;
+    }
+#endif
+    if (!have_gps_yaw(instance)) {
+        return false;
+    }
+    yaw_deg = state[instance].gps_yaw;
+
+    // get lagged timestamp
+    time_ms = state[instance].gps_yaw_time_ms;
+    float lag_s;
+    if (get_lag(instance, lag_s)) {
+        uint32_t lag_ms = lag_s * 1000;
+        time_ms -= lag_ms;
+    }
+
+    if (state[instance].have_gps_yaw_accuracy) {
+        accuracy_deg = state[instance].gps_yaw_accuracy;
+    } else {
+        // fall back to 10 degrees as a generic default
+        accuracy_deg = 10;
+    }
+    return true;
+}
+
 namespace AP {
 
 AP_GPS &gps()

libraries/AP_GPS/AP_GPS.h

@@ -168,6 +168,7 @@ public:
         float ground_speed;                 ///< ground speed in m/sec
         float ground_course;                ///< ground course in degrees
         float gps_yaw;                      ///< GPS derived yaw information, if available (degrees)
+        uint32_t gps_yaw_time_ms;           ///< timestamp of last GPS yaw reading
         bool  gps_yaw_configured;           ///< GPS is configured to provide yaw
         uint16_t hdop;                      ///< horizontal dilution of precision in cm
         uint16_t vdop;                      ///< vertical dilution of precision in cm
@@ -328,21 +329,9 @@ public:
     }
 
     // yaw in degrees if available
-    bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg) const {
+    bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const;
-        if (!have_gps_yaw(instance)) {
+    bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const {
-            return false;
+        return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg, time_ms);
-        }
-        yaw_deg = state[instance].gps_yaw;
-        if (state[instance].have_gps_yaw_accuracy) {
-            accuracy_deg = state[instance].gps_yaw_accuracy;
-        } else {
-            // fall back to 10 degrees as a generic default
-            accuracy_deg = 10;
-        }
-        return true;
-    }
-    bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg) const {
-        return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg);
     }
 
     // number of locked satellites

libraries/AP_GPS/AP_GPS_MAV.cpp

@@ -104,8 +104,11 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
                 state.have_vertical_accuracy = true;
             }
 
+            const uint32_t now_ms = AP_HAL::millis();
+
             if (have_yaw) {
                 state.gps_yaw = wrap_360(packet.yaw*0.01);
+                state.gps_yaw_time_ms = now_ms;
                 state.have_gps_yaw = true;
                 state.gps_yaw_configured = true;
             }
@@ -120,12 +123,15 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
                     first_week = packet.time_week;
                 }
                 uint32_t timestamp_ms = (packet.time_week - first_week) * AP_MSEC_PER_WEEK + packet.time_week_ms;
-                uint32_t corrected_ms = jitter.correct_offboard_timestamp_msec(timestamp_ms, AP_HAL::millis());
+                uint32_t corrected_ms = jitter.correct_offboard_timestamp_msec(timestamp_ms, now_ms);
                 state.uart_timestamp_ms = corrected_ms;
+                if (have_yaw) {
+                    state.gps_yaw_time_ms = corrected_ms;
+                }
             }
 
             state.num_sats = packet.satellites_visible;
-            state.last_gps_time_ms = AP_HAL::millis();
+            state.last_gps_time_ms = now_ms;
             _new_data = true;
             break;
             }

libraries/AP_GPS/AP_GPS_MSP.cpp

@@ -77,13 +77,14 @@ void AP_GPS_MSP::handle_msp(const MSP::msp_gps_data_message_t &pkt)
     state.vertical_accuracy = pkt.vertical_pos_accuracy * 0.01;
     state.speed_accuracy = pkt.horizontal_vel_accuracy * 0.01;
 
+    state.last_gps_time_ms = AP_HAL::millis();
+
     if (pkt.true_yaw != 65535) {
         state.gps_yaw = wrap_360(pkt.true_yaw*0.01);
         state.have_gps_yaw = true;
+        state.gps_yaw_time_ms = state.last_gps_time_ms;
     }
 
-    state.last_gps_time_ms = AP_HAL::millis();
-
     new_data = pkt.fix_type>0;
 }
 

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -339,6 +339,7 @@ bool AP_GPS_NMEA::_term_complete()
                     _last_HDT_THS_ms = now;
                     state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
                     state.have_gps_yaw = true;
+                    state.gps_yaw_time_ms = AP_HAL::millis();
                     // remember that we are setup to provide yaw. With
                     // a NMEA GPS we can only tell if the GPS is
                     // configured to provide yaw when it first sends a

libraries/AP_GPS/AP_GPS_UAVCAN.cpp

@@ -525,6 +525,9 @@ void AP_GPS_UAVCAN::handle_heading_msg(const HeadingCb &cb)
 
     interim_state.have_gps_yaw = cb.msg->heading_valid;
     interim_state.gps_yaw = degrees(cb.msg->heading_rad);
+    if (interim_state.have_gps_yaw) {
+        interim_state.gps_yaw_time_ms = AP_HAL::millis();
+    }
 
     interim_state.have_gps_yaw_accuracy = cb.msg->heading_accuracy_valid;
     interim_state.gps_yaw_accuracy = degrees(cb.msg->heading_accuracy_rad);

libraries/AP_GPS/GPS_Backend.cpp

@@ -384,6 +384,7 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(const float reported_heading_deg,
             const float rotation_offset_rad = Vector2f(-offset.x, -offset.y).angle();
             state.gps_yaw = wrap_360(reported_heading_deg - degrees(rotation_offset_rad));
             state.have_gps_yaw = true;
+            state.gps_yaw_time_ms = AP_HAL::millis();
         }
     }