AP_GPS: UBLOX: Parse RELPOSNED messages

This does not yet:
  - validate the receiver configuration
  - manage timing out stale GPS heading info
  - relPosNormalized usage isn't clear, which may defeat the STRICT_LENGTH_CHECK

libraries/AP_GPS/AP_GPS.h

@@ -145,11 +145,13 @@ public:
         float speed_accuracy;               ///< 3D velocity RMS accuracy estimate in m/s
         float horizontal_accuracy;          ///< horizontal RMS accuracy estimate in m
         float vertical_accuracy;            ///< vertical RMS accuracy estimate in m
+        float gps_yaw_accuracy;           ///< heading accuracy of the GPS in degrees
         bool have_vertical_velocity;      ///< does GPS give vertical velocity? Set to true only once available.
         bool have_speed_accuracy;         ///< does GPS give speed accuracy? Set to true only once available.
         bool have_horizontal_accuracy;    ///< does GPS give horizontal position accuracy? Set to true only once available.
         bool have_vertical_accuracy;      ///< does GPS give vertical position accuracy? Set to true only once available.
         bool have_gps_yaw;                ///< does GPS give yaw? Set to true only once available.
+        bool have_gps_yaw_accuracy;       ///< does the GPS give a heading accuracy estimate? Set to true only once available
         uint32_t last_gps_time_ms;          ///< the system time we got the last GPS timestamp, milliseconds
         uint32_t uart_timestamp_ms;         ///< optional timestamp from set_uart_timestamp()
 
@@ -270,11 +272,12 @@ public:
             return false;
         }
         yaw_deg = state[instance].gps_yaw;
-        // None of the GPS backends can provide this yet, so we hard
-        // code a fixed value of 10 degrees, which seems like a
-        // reasonable guess. Once a backend can provide a proper
-        // estimate we can implement it
-        accuracy_deg = 10;
+        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 {

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -1032,8 +1032,50 @@ AP_GPS_UBLOX::_parse_gps(void)
         state.hdop = 130;
 #endif
         break;
+    case MSG_RELPOSNED:
+        {
+            const Vector3f &offset0 = gps._antenna_offset[0].get();
+            const Vector3f &offset1 = gps._antenna_offset[1].get();
+            const uint32_t valid_mask = static_cast<uint32_t>(RELPOSNED::relPosHeadingValid) |
+                                        static_cast<uint32_t>(RELPOSNED::relPosValid) |
+                                        static_cast<uint32_t>(RELPOSNED::gnssFixOK) |
+                                        static_cast<uint32_t>(RELPOSNED::isMoving) |
+                                        (2 << static_cast<uint32_t>(RELPOSNED::carrSoln)); // require fixed ambiguity
+            const uint32_t invalid_mask = static_cast<uint32_t>(RELPOSNED::refPosMiss) |
+                                          static_cast<uint32_t>(RELPOSNED::refObsMiss);
+
+            if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
+                ((_buffer.relposned.flags & invalid_mask) == 0)
+#if defined(STRICT_LENGTH_CHECK)
+                // force the estimated position to be within
+                && (fabsf(offset0.distance(offset1) - (_buffer.relposned.relPosLength * 1e-2)) < STRICT_LENGTH_CHECK)
+#endif // defined(STRICT_LENGTH_CHECK)
+                ) {
+                float rotation_offset_rad;
+                if (offset0.is_zero()) {
+                    rotation_offset_rad = Vector2f(offset1.x, offset1.y).angle();
+                } else if (offset1.is_zero()) {
+                    rotation_offset_rad = Vector2f(offset0.x, offset0.y).angle();
+                } else {
+                    const Vector3f diff = offset0 - offset1;
+                    rotation_offset_rad = Vector2f(diff.x, diff.y).angle();
+                }
+                if (state.instance != 0) {
+                    rotation_offset_rad += M_PI;
+                }
+                state.gps_yaw = wrap_360(_buffer.relposned.relPosHeading * 1e-5 + degrees(rotation_offset_rad));
+                state.have_gps_yaw = true;
+                state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
+                state.have_gps_yaw_accuracy = true;
+            } else {
+                state.have_gps_yaw = false;
+                state.have_gps_yaw_accuracy = false;
+            }
+        }
+        break;
     case MSG_PVT:
         Debug("MSG_PVT");
+
         havePvtMsg = true;
         // position
         _check_new_itow(_buffer.pvt.itow);

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -281,6 +281,30 @@ private:
         uint32_t headVeh;
         uint8_t reserved2[4]; 
     };
+    struct PACKED ubx_nav_relposned {
+        uint8_t version;
+        uint8_t reserved1;
+        uint16_t refStationId;
+        uint32_t iTOW;
+        int32_t relPosN;
+        int32_t relPosE;
+        int32_t relPosD;
+        int32_t relPosLength;
+        int32_t relPosHeading;
+        uint8_t reserved2[4];
+        int8_t relPosHPN;
+        int8_t relPosHPE;
+        int8_t relPosHPD;
+        int8_t relPosHPLength;
+        uint32_t accN;
+        uint32_t accE;
+        uint32_t accD;
+        uint32_t accLength;
+        uint32_t accHeading;
+        uint8_t reserved3[4];
+        uint32_t flags;
+    };
+
     struct PACKED ubx_nav_velned {
         uint32_t itow;                                  // GPS msToW
         int32_t ned_north;
@@ -444,6 +468,7 @@ private:
 #endif
         ubx_cfg_sbas sbas;
         ubx_nav_svinfo_header svinfo_header;
+        ubx_nav_relposned relposned;
 #if UBLOX_RXM_RAW_LOGGING
         ubx_rxm_raw rxm_raw;
         ubx_rxm_rawx rxm_rawx;
@@ -451,6 +476,18 @@ private:
         ubx_ack_ack ack;
     } _buffer;
 
+    enum class RELPOSNED {
+        gnssFixOK          = 1U << 0,
+        diffSoln           = 1U << 1,
+        relPosValid        = 1U << 2,
+        carrSoln           = 1U << 3,
+        isMoving           = 1U << 5,
+        refPosMiss         = 1U << 6,
+        refObsMiss         = 1U << 7,
+        relPosHeadingValid = 1U << 8,
+        relPosNormalized   = 1U << 9
+    };
+
     enum ubs_protocol_bytes {
         PREAMBLE1 = 0xb5,
         PREAMBLE2 = 0x62,
@@ -467,6 +504,7 @@ private:
         MSG_SOL = 0x6,
         MSG_PVT = 0x7,
         MSG_TIMEGPS = 0x20,
+        MSG_RELPOSNED = 0x3c,
         MSG_VELNED = 0x12,
         MSG_CFG_CFG = 0x09,
         MSG_CFG_RATE = 0x08,