AP_GPS: Send RTK baseline data from SBF

libraries/AP_GPS/AP_GPS_ERB.h

@@ -35,7 +35,7 @@ public:
 
     AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }
 
-    bool supports_mavlink_gps_rtk_message() override { return true; }
+    bool supports_mavlink_gps_rtk_message() const override { return true; }
 
     static bool _detect(struct ERB_detect_state &state, uint8_t data);
 

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -62,6 +62,9 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
 
     port->write((const uint8_t*)_port_enable, strlen(_port_enable));
     _config_last_ack_time = AP_HAL::millis();
+
+    // if we ever parse RTK observations it will always be of type NED, so set it once
+    state.rtk_baseline_coords_type = RTK_BASELINE_COORDINATE_SYSTEM_NED;
 }
 
 AP_GPS_SBF::~AP_GPS_SBF (void) {
@@ -411,6 +414,37 @@ AP_GPS_SBF::process_message(void)
         }
         break;
     }
+    case BaseVectorGeod:
+    {
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as it's false positive when testing for DNU values
+        const msg4028 &temp = sbf_msg.data.msg4028u;
+
+        // just breakout any consts we need for Do Not Use (DNU) reasons
+        constexpr double doubleDNU = -2e-10;
+
+        check_new_itow(temp.TOW, sbf_msg.length);
+
+        if (temp.N == 0) { // no sub blocks so just bail, we can't do anything useful here
+            state.rtk_num_sats = 0;
+            state.rtk_age_ms = 0;
+            state.rtk_baseline_y_mm = 0;
+            state.rtk_baseline_x_mm = 0;
+            state.rtk_baseline_z_mm = 0;
+            break;
+        }
+
+        state.rtk_num_sats = temp.info.NrSV;
+
+        state.rtk_age_ms = (temp.info.CorrAge != 65535) ? ((uint32_t)temp.info.CorrAge) * 10 : 0;
+
+        // copy the position as long as the data isn't DNU
+        state.rtk_baseline_y_mm = (temp.info.DeltaEast != doubleDNU) ?  temp.info.DeltaEast * 1e3 : 0;
+        state.rtk_baseline_x_mm = (temp.info.DeltaNorth != doubleDNU) ? temp.info.DeltaNorth * 1e3 : 0;
+        state.rtk_baseline_z_mm = (temp.info.DeltaUp != doubleDNU) ? temp.info.DeltaUp * -1e3 : 0;
+#pragma GCC diagnostic pop
+        break;
+    }
     }
 
     return false;

libraries/AP_GPS/AP_GPS_SBF.h

@@ -43,6 +43,8 @@ public:
 
     void broadcast_configuration_failure_reason(void) const override;
 
+    bool supports_mavlink_gps_rtk_message(void) const override { return true; };
+
     // get the velocity lag, returns true if the driver is confident in the returned value
     bool get_lag(float &lag_sec) const override { lag_sec = 0.08f; return true; } ;
 
@@ -65,7 +67,7 @@ private:
     uint8_t _init_blob_index;
     uint32_t _init_blob_time;
     char *_initial_sso;
-    const char* _sso_normal = ", PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic, msec100\n";
+    const char* _sso_normal = ", PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod, msec100\n";
     const char* _initialisation_blob[4] = {
     "srd, Moderate, UAV\n",
     "sem, PVT, 5\n",
@@ -87,6 +89,7 @@ private:
         DOP = 4001,
         PVTGeodetic = 4007,
         ReceiverStatus = 4014,
+        BaseVectorGeod = 4028,
         VelCovGeodetic = 5908
     };
 
@@ -150,6 +153,33 @@ private:
          // remaining data is AGCData, which we don't have a use for, don't extract the data
     };
 
+    struct PACKED VectorInfoGeod {
+        uint8_t NrSV;
+        uint8_t Error;
+        uint8_t Mode;
+        uint8_t Misc;
+        double DeltaEast;
+        double DeltaNorth;
+        double DeltaUp;
+        float DeltaVe;
+        float DeltaVn;
+        float DeltaVu;
+        uint16_t Azimuth;
+        int16_t Elevation;
+        uint8_t ReferenceID;
+        uint16_t CorrAge;
+        uint32_t SignalInfo;
+    };
+
+    struct PACKED msg4028 // BaseVectorGeod
+    {
+        uint32_t TOW;
+        uint16_t WNc;
+        uint8_t N; // number of baselines
+        uint8_t SBLength;
+        VectorInfoGeod info; // there can be multiple baselines here, but we will only consume the first one, so don't worry about anything after
+    };
+
     struct PACKED msg5908 // VelCovGeodetic
     {
         uint32_t TOW;
@@ -172,6 +202,7 @@ private:
         msg4007 msg4007u;
         msg4001 msg4001u;
         msg4014 msg4014u;
+        msg4028 msg4028u;
         msg5908 msg5908u;
         uint8_t bytes[256];
     };

libraries/AP_GPS/AP_GPS_SBP.h

@@ -31,7 +31,7 @@ public:
 
     AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }
 
-    bool supports_mavlink_gps_rtk_message() override { return true; }
+    bool supports_mavlink_gps_rtk_message() const override { return true; }
 
     // Methods
     bool read() override;

libraries/AP_GPS/GPS_Backend.h

@@ -45,7 +45,7 @@ public:
     virtual void inject_data(const uint8_t *data, uint16_t len);
 
     //MAVLink methods
-    virtual bool supports_mavlink_gps_rtk_message() { return false; }
+    virtual bool supports_mavlink_gps_rtk_message() const { return false; }
     virtual void send_mavlink_gps_rtk(mavlink_channel_t chan);
 
     virtual void broadcast_configuration_failure_reason(void) const { return ; }