AP_GPS: add support for dual GPS heading using Periph GPSes

libraries/AP_GPS/AP_GPS.cpp

@@ -81,7 +81,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: 1st GPS type
     // @Description: GPS type of 1st GPS
-    // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS
+    // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:UAVCAN-MovingBaseline-Base,23:UAVCAN-MovingBaseline-Rover
     // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("_TYPE",    0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT),
@@ -90,7 +90,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Param: _TYPE2
     // @DisplayName: 2nd GPS type
     // @Description: GPS type of 2nd GPS
-    // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS
+    // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:UAVCAN-MovingBaseline-Base,23:UAVCAN-MovingBaseline-Rover
     // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("_TYPE2",   1, AP_GPS, _type[1], 0),
@@ -401,6 +401,8 @@ bool AP_GPS::needs_uart(GPS_Type type) const
     case GPS_TYPE_NONE:
     case GPS_TYPE_HIL:
     case GPS_TYPE_UAVCAN:
+    case GPS_TYPE_UAVCAN_RTK_BASE:
+    case GPS_TYPE_UAVCAN_RTK_ROVER:
     case GPS_TYPE_MAV:
     case GPS_TYPE_MSP:
     case GPS_TYPE_EXTERNAL_AHRS:
@@ -573,6 +575,8 @@ void AP_GPS::detect_instance(uint8_t instance)
 
     // user has to explicitly set the UAVCAN type, do not use AUTO
     case GPS_TYPE_UAVCAN:
+    case GPS_TYPE_UAVCAN_RTK_BASE:
+    case GPS_TYPE_UAVCAN_RTK_ROVER:
 #if HAL_ENABLE_LIBUAVCAN_DRIVERS
         dstate->auto_detected_baud = false; // specified, not detected
         new_gps = AP_GPS_UAVCAN::probe(*this, state[instance]);
@@ -824,7 +828,10 @@ void AP_GPS::update_instance(uint8_t instance)
             timing[instance].last_message_time_ms = tnow;
             timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
             // do not try to detect again if type is MAV or UAVCAN
-            if (_type[instance] == GPS_TYPE_MAV || _type[instance] == GPS_TYPE_UAVCAN) {
+            if (_type[instance] == GPS_TYPE_MAV ||
+                _type[instance] == GPS_TYPE_UAVCAN ||
+                _type[instance] == GPS_TYPE_UAVCAN_RTK_BASE ||
+                _type[instance] == GPS_TYPE_UAVCAN_RTK_ROVER) {
                 state[instance].status = NO_FIX;
             } else {
                 // free the driver before we run the next detection, so we
@@ -911,6 +918,55 @@ void AP_GPS::update_instance(uint8_t instance)
 #endif
 }
 
+
+#if GPS_MOVING_BASELINE
+void AP_GPS::get_RelPosHeading(uint32_t &timestamp, float &relPosHeading, float &relPosLength, float &relPosD, float &accHeading)
+{
+    for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
+        if (drivers[i] && _type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
+           relPosHeading = state[i].relPosHeading;
+           relPosLength = state[i].relPosLength;
+           relPosD = state[i].relPosD;
+           accHeading = state[i].accHeading;
+           timestamp = state[i].relposheading_ts;
+        }
+    }
+}
+
+bool AP_GPS::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
+{
+    for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
+        if (drivers[i] && _type[i] == GPS_TYPE_UBLOX_RTK_BASE) {
+            return drivers[i]->get_RTCMV3(bytes, len);
+        }
+    }
+    return false;
+}
+
+void AP_GPS::clear_RTCMV3()
+{
+    for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
+        if (drivers[i] && _type[i] == GPS_TYPE_UBLOX_RTK_BASE) {
+            drivers[i]->clear_RTCMV3();
+        }
+    }
+}
+
+/*
+    inject Moving Baseline Data messages.
+*/
+void AP_GPS::inject_MBL_data(uint8_t* data, uint16_t length)
+{
+    for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
+        if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
+            // pass the data to the rover
+            inject_data(i, data, length);
+            break;
+        }
+    }
+}
+#endif //#if GPS_MOVING_BASELINE
+
 /*
   update all GPS instances
  */
@@ -1003,8 +1059,8 @@ void AP_GPS::update_primary(void)
     // 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_BASE &&
+        if (((_type[i] == GPS_TYPE_UBLOX_RTK_BASE) || (_type[i] == GPS_TYPE_UAVCAN_RTK_BASE)) &&
-            _type[i^1] == GPS_TYPE_UBLOX_RTK_ROVER &&
+            ((_type[i^1] == GPS_TYPE_UBLOX_RTK_ROVER) || (_type[i^1] == GPS_TYPE_UAVCAN_RTK_ROVER)) &&
             ((state[i].status >= GPS_OK_FIX_3D) || (state[i].status >= state[i^1].status))) {
             if (primary_instance != i) {
                 _last_instance_swap_ms = now;
@@ -1158,7 +1214,7 @@ void AP_GPS::inject_data(const uint8_t *data, uint16_t len)
     //Support broadcasting to all GPSes.
     if (_inject_to == GPS_RTK_INJECT_TO_ALL) {
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
+            if ((_type[i] == GPS_TYPE_UBLOX_RTK_ROVER) || (_type[i] == GPS_TYPE_UAVCAN_RTK_ROVER)) {
                 // we don't externally inject to moving baseline rover
                 continue;
             }
@@ -1859,7 +1915,7 @@ bool AP_GPS::is_healthy(uint8_t instance) const
       happens with the RTCMv3 data
      */
     const uint8_t delay_threshold = 2;
-    const float delay_avg_max = _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER?245:215;
+    const float delay_avg_max = _type[instance] == (GPS_TYPE_UBLOX_RTK_ROVER || GPS_TYPE_UAVCAN_RTK_ROVER)?245:215;
     const GPS_timing &t = timing[instance];
     bool delay_ok = (t.delayed_count < delay_threshold) &&
         t.average_delta_ms < delay_avg_max &&
@@ -1888,13 +1944,16 @@ bool AP_GPS::prepare_for_arming(void) {
 bool AP_GPS::backends_healthy(char failure_msg[], uint16_t failure_msg_len) {
     for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
 #if HAL_ENABLE_LIBUAVCAN_DRIVERS
-        if (_type[i] == GPS_TYPE_UAVCAN) {
+        if (_type[i] == GPS_TYPE_UAVCAN ||
+            _type[i] == GPS_TYPE_UAVCAN_RTK_BASE ||
+            _type[i] == GPS_TYPE_UAVCAN_RTK_ROVER) {
             if (!AP_GPS_UAVCAN::backends_healthy(failure_msg, failure_msg_len)) {
                 return false;
             }
         }
 #endif
-        if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
+        if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER ||
+            _type[i] == GPS_TYPE_UAVCAN_RTK_ROVER) {
             if (AP_HAL::millis() - state[i].gps_yaw_time_ms > 15000) {
                 hal.util->snprintf(failure_msg, failure_msg_len, "GPS[%u] yaw not available", unsigned(i+1));
                 return false;
@@ -1995,8 +2054,8 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 {
 #if GPS_MAX_RECEIVERS > 1
     if (instance < GPS_MAX_RECEIVERS &&
-        _type[instance] == GPS_TYPE_UBLOX_RTK_BASE &&
+        ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE) || (_type[instance] == GPS_TYPE_UAVCAN_RTK_BASE)) &&
-        _type[instance^1] == GPS_TYPE_UBLOX_RTK_ROVER) {
+        ((_type[instance^1] == GPS_TYPE_UBLOX_RTK_ROVER) || (_type[instance^1] == GPS_TYPE_UAVCAN_RTK_ROVER))) {
         // return the yaw from the rover
         instance ^= 1;
     }

libraries/AP_GPS/AP_GPS.h

@@ -119,6 +119,8 @@ public:
         GPS_TYPE_MSP = 19,
         GPS_TYPE_ALLYSTAR = 20, // AllyStar NMEA
         GPS_TYPE_EXTERNAL_AHRS = 21,
+        GPS_TYPE_UAVCAN_RTK_BASE = 22,
+        GPS_TYPE_UAVCAN_RTK_ROVER = 23,
     };
 
     /// GPS status codes
@@ -199,6 +201,13 @@ public:
         int32_t  rtk_baseline_z_mm;        ///< Current baseline in ECEF z or NED down component in mm
         uint32_t rtk_accuracy;             ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999)
         int32_t  rtk_iar_num_hypotheses;   ///< Current number of integer ambiguity hypotheses
+        
+        // UBX Relative Position and Heading message information
+        float relPosHeading;               ///< Reported Heading in degrees
+        float relPosLength;                ///< Reported Position horizontal distance in meters
+        float relPosD;                     ///< Reported Vertical distance in meters
+        float accHeading;                  ///< Reported Heading Accuracy in degrees
+        uint32_t relposheading_ts;        ///< True if new data has been received since last time it was false
     };
 
     /// Startup initialisation.
@@ -520,6 +529,14 @@ public:
         DoNotChange = 2,
     };
 
+#if GPS_MOVING_BASELINE
+    // methods used by UAVCAN GPS driver and AP_Periph for moving baseline
+    void inject_MBL_data(uint8_t* data, uint16_t length);
+    void get_RelPosHeading(uint32_t &timestamp, float &relPosHeading, float &relPosLength, float &relPosD, float &accHeading);
+    bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len);
+    void clear_RTCMV3();
+#endif // GPS_MOVING_BASELINE
+
 protected:
 
     // configuration parameters

libraries/AP_GPS/AP_GPS_UAVCAN.cpp

@@ -32,6 +32,10 @@
 #include <uavcan/equipment/gnss/Auxiliary.hpp>
 #include <ardupilot/gnss/Heading.hpp>
 #include <ardupilot/gnss/Status.hpp>
+#if GPS_MOVING_BASELINE
+#include <ardupilot/gnss/MovingBaselineData.hpp>
+#include <ardupilot/gnss/RelPosHeading.hpp>
+#endif
 
 extern const AP_HAL::HAL& hal;
 
@@ -42,13 +46,18 @@ UC_REGISTRY_BINDER(Fix2Cb, uavcan::equipment::gnss::Fix2);
 UC_REGISTRY_BINDER(AuxCb, uavcan::equipment::gnss::Auxiliary);
 UC_REGISTRY_BINDER(HeadingCb, ardupilot::gnss::Heading);
 UC_REGISTRY_BINDER(StatusCb, ardupilot::gnss::Status);
+#if GPS_MOVING_BASELINE
+UC_REGISTRY_BINDER(MovingBaselineDataCb, ardupilot::gnss::MovingBaselineData);
+UC_REGISTRY_BINDER(RelPosHeadingCb, ardupilot::gnss::RelPosHeading);
+#endif
 
 AP_GPS_UAVCAN::DetectedModules AP_GPS_UAVCAN::_detected_modules[] = {0};
 HAL_Semaphore AP_GPS_UAVCAN::_sem_registry;
 
 // Member Methods
-AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state) :
+AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role _role) :
-    AP_GPS_Backend(_gps, _state, nullptr)
+    AP_GPS_Backend(_gps, _state, nullptr),
+    role(_role)
 {}
 
 AP_GPS_UAVCAN::~AP_GPS_UAVCAN()
@@ -56,6 +65,10 @@ AP_GPS_UAVCAN::~AP_GPS_UAVCAN()
     WITH_SEMAPHORE(_sem_registry);
 
     _detected_modules[_detected_module].driver = nullptr;
+
+#if GPS_MOVING_BASELINE
+    delete rtcm3_parser;
+#endif
 }
 
 void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
@@ -105,6 +118,24 @@ void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
         AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r");
         return;
     }
+
+#if GPS_MOVING_BASELINE
+    uavcan::Subscriber<ardupilot::gnss::MovingBaselineData, MovingBaselineDataCb> *gnss_moving_baseline;
+    gnss_moving_baseline = new uavcan::Subscriber<ardupilot::gnss::MovingBaselineData, MovingBaselineDataCb>(*node);
+    const int gnss_moving_baseline_start_res = gnss_moving_baseline->start(MovingBaselineDataCb(ap_uavcan, &handle_moving_baseline_msg_trampoline));
+    if (gnss_moving_baseline_start_res < 0) {
+        AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r");
+        return;
+    }
+
+    uavcan::Subscriber<ardupilot::gnss::RelPosHeading, RelPosHeadingCb> *gnss_relposheading;
+    gnss_relposheading = new uavcan::Subscriber<ardupilot::gnss::RelPosHeading, RelPosHeadingCb>(*node);
+    const int gnss_relposheading_start_res = gnss_relposheading->start(RelPosHeadingCb(ap_uavcan, &handle_relposheading_msg_trampoline));
+    if (gnss_relposheading_start_res < 0) {
+        AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r");
+        return;
+    }
+#endif
 }
 
 AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
@@ -153,7 +184,22 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
     if (found_match == -1) {
         return NULL;
     }
-    backend = new AP_GPS_UAVCAN(_gps, _state);
+    // initialise the backend based on the UAVCAN Moving baseline selection
+    switch (_gps.get_type(found_match)) {
+        case AP_GPS::GPS_TYPE_UAVCAN:
+            backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_NORMAL);
+            break;
+#if GPS_MOVING_BASELINE
+        case AP_GPS::GPS_TYPE_UAVCAN_RTK_BASE:
+            backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_MB_BASE);
+            break;
+        case AP_GPS::GPS_TYPE_UAVCAN_RTK_ROVER:
+            backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_MB_ROVER);
+            break;
+#endif
+        default:
+            return NULL;
+    }
     if (backend == nullptr) {
         AP::can().log_text(AP_CANManager::LOG_ERROR,
                             LOG_TAG,
@@ -183,7 +229,16 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
                 AP::gps()._node_id[i].set_and_notify(tmp);
             }
         }
+#if GPS_MOVING_BASELINE
+        if (_detected_modules[found_match].driver->role == AP_GPS::GPS_ROLE_MB_BASE) {
+            _detected_modules[found_match].driver->rtcm3_parser = new RTCM3_Parser;
+            if (_detected_modules[found_match].driver->rtcm3_parser == nullptr) {
+                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UAVCAN%u-%u: failed RTCMv3 parser allocation", _detected_modules[found_match].ap_uavcan->get_driver_index()+1, _detected_modules[found_match].node_id);
+            }
+        }
+#endif // GPS_MOVING_BASELINE
     }
+
     return backend;
 }
 
@@ -550,6 +605,67 @@ void AP_GPS_UAVCAN::handle_status_msg(const StatusCb &cb)
     }
 }
 
+#if GPS_MOVING_BASELINE
+/*
+  handle moving baseline data.
+  */
+void AP_GPS_UAVCAN::handle_moving_baseline_msg(const MovingBaselineDataCb &cb)
+{
+    WITH_SEMAPHORE(sem);
+    if ((rtcm3_parser == nullptr) || (role != AP_GPS::GPS_ROLE_MB_BASE)) {
+        return;
+    }
+    for (const auto &c : cb.msg->data) {
+        rtcm3_parser->read(c);
+    }
+}
+
+/*
+    handle relposheading message
+*/
+void AP_GPS_UAVCAN::handle_relposheading_msg(const RelPosHeadingCb &cb)
+{
+    if (role != AP_GPS::GPS_ROLE_MB_ROVER) {
+        return;
+    }
+ 
+    WITH_SEMAPHORE(sem);
+
+    interim_state.gps_yaw_configured = true;
+    // push raw heading data to calculate moving baseline heading states
+    if (calculate_moving_base_yaw(interim_state,
+                                cb.msg->reported_heading_deg,
+                                cb.msg->relative_distance_m,
+                                cb.msg->relative_down_pos_m)) {
+        if (cb.msg->reported_heading_acc_available) {
+            interim_state.gps_yaw_accuracy = cb.msg->reported_heading_acc_deg;
+        }
+        interim_state.have_gps_yaw_accuracy = cb.msg->reported_heading_acc_available;
+    }
+}
+
+// support for retrieving RTCMv3 data from a moving baseline base
+bool AP_GPS_UAVCAN::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
+{
+    WITH_SEMAPHORE(sem);
+    if (rtcm3_parser != nullptr) {
+        len = rtcm3_parser->get_len(bytes);
+        return len > 0;
+    }
+    return false;
+}
+
+// clear previous RTCM3 packet
+void AP_GPS_UAVCAN::clear_RTCMV3(void)
+{
+    WITH_SEMAPHORE(sem);
+    if (rtcm3_parser != nullptr) {
+        rtcm3_parser->clear_packet();
+    }
+}
+
+#endif // GPS_MOVING_BASELINE
+
 void AP_GPS_UAVCAN::handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FixCb &cb)
 {
     WITH_SEMAPHORE(_sem_registry);
@@ -600,6 +716,28 @@ void AP_GPS_UAVCAN::handle_status_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t n
     }
 }
 
+#if GPS_MOVING_BASELINE
+// Moving Baseline msg trampoline
+void AP_GPS_UAVCAN::handle_moving_baseline_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MovingBaselineDataCb &cb)
+{
+    WITH_SEMAPHORE(_sem_registry);
+    AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id);
+    if (driver != nullptr) {
+        driver->handle_moving_baseline_msg(cb);
+    }
+}
+
+// RelPosHeading msg trampoline
+void AP_GPS_UAVCAN::handle_relposheading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const RelPosHeadingCb &cb)
+{
+    WITH_SEMAPHORE(_sem_registry);
+    AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id);
+    if (driver != nullptr) {
+        driver->handle_relposheading_msg(cb);
+    }
+}
+#endif
+
 // Consume new data and mark it received
 bool AP_GPS_UAVCAN::read(void)
 {

libraries/AP_GPS/AP_GPS_UAVCAN.h

@@ -22,7 +22,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include "AP_GPS.h"
 #include "GPS_Backend.h"
-
+#include "RTCM3_Parser.h"
 #include <AP_UAVCAN/AP_UAVCAN.h>
 
 class FixCb;
@@ -30,10 +30,14 @@ class Fix2Cb;
 class AuxCb;
 class HeadingCb;
 class StatusCb;
+#if GPS_MOVING_BASELINE
+class MovingBaselineDataCb;
+class RelPosHeadingCb;
+#endif
 
 class AP_GPS_UAVCAN : public AP_GPS_Backend {
 public:
-    AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state);
+    AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role role);
     ~AP_GPS_UAVCAN();
 
     bool read() override;
@@ -54,12 +58,19 @@ public:
     static void handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb);
     static void handle_heading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HeadingCb &cb);
     static void handle_status_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const StatusCb &cb);
-
+#if GPS_MOVING_BASELINE
+    static void handle_moving_baseline_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MovingBaselineDataCb &cb);
+    static void handle_relposheading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const RelPosHeadingCb &cb);
+#endif
     static bool backends_healthy(char failure_msg[], uint16_t failure_msg_len);
     void inject_data(const uint8_t *data, uint16_t len) override;
 
     bool get_error_codes(uint32_t &error_codes) const override { error_codes = error_code; return seen_status; };
 
+#if GPS_MOVING_BASELINE
+    bool get_RTCMV3(const uint8_t *&data, uint16_t &len) override;
+    void clear_RTCMV3() override;
+#endif
 private:
     void handle_fix_msg(const FixCb &cb);
     void handle_fix2_msg(const Fix2Cb &cb);
@@ -67,6 +78,11 @@ private:
     void handle_heading_msg(const HeadingCb &cb);
     void handle_status_msg(const StatusCb &cb);
 
+#if GPS_MOVING_BASELINE
+    void handle_moving_baseline_msg(const MovingBaselineDataCb &cb);
+    void handle_relposheading_msg(const RelPosHeadingCb &cb);
+#endif
+
     static bool take_registry();
     static void give_registry();
     static AP_GPS_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id);
@@ -96,4 +112,12 @@ private:
     } _detected_modules[GPS_MAX_RECEIVERS];
 
     static HAL_Semaphore _sem_registry;
+
+#if GPS_MOVING_BASELINE
+    // RTCM3 parser for when in moving baseline base mode
+    RTCM3_Parser *rtcm3_parser;
+#endif
+    // the role set from GPS_TYPE
+    AP_GPS::GPS_Role role;
+
 };

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -50,13 +50,27 @@
 extern const AP_HAL::HAL& hal;
 
 #if UBLOX_DEBUGGING
+#if defined(HAL_BUILD_AP_PERIPH)
+ extern "C" {
+   void can_printf(const char *fmt, ...);
+ }
+ # define Debug(fmt, args ...)  do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
+#else
  # define Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
+#endif
 #else
  # define Debug(fmt, args ...)
 #endif
 
 #if UBLOX_MB_DEBUGGING
+#if defined(HAL_BUILD_AP_PERIPH)
+ extern "C" {
+   void can_printf(const char *fmt, ...);
+ }
+ # define MB_Debug(fmt, args ...)  do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
+#else
  # define MB_Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
+#endif
 #else
  # define MB_Debug(fmt, args ...)
 #endif
@@ -1353,15 +1367,21 @@ AP_GPS_UBLOX::_parse_gps(void)
                 MB_Debug("RELPOSNED ITOW %u %u\n", unsigned(_buffer.relposned.iTOW), unsigned(_last_relposned_itow));
             }
             _last_relposned_itow = _buffer.relposned.iTOW;
-
+            MB_Debug("RELPOSNED flags: %lx valid: %lx invalid: %lx\n", _buffer.relposned.flags, valid_mask, invalid_mask);
             if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
-                ((_buffer.relposned.flags & invalid_mask) == 0) &&
+                ((_buffer.relposned.flags & invalid_mask) == 0)) {
-                calculate_moving_base_yaw(_buffer.relposned.relPosHeading * 1e-5,
+                if (calculate_moving_base_yaw(_buffer.relposned.relPosHeading * 1e-5,
                                           _buffer.relposned.relPosLength * 0.01,
                                           _buffer.relposned.relPosD*0.01)) {
-                state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
+                    state.have_gps_yaw_accuracy = true;
-                state.have_gps_yaw_accuracy = true;
+                    state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
-                _last_relposned_ms = AP_HAL::millis();
+                    _last_relposned_ms = AP_HAL::millis();
+                }
+                state.relPosHeading = _buffer.relposned.relPosHeading * 1e-5;
+                state.relPosLength  = _buffer.relposned.relPosLength * 0.01;
+                state.relPosD       = _buffer.relposned.relPosD * 0.01;
+                state.accHeading    = _buffer.relposned.accHeading * 1e-5;
+                state.relposheading_ts = AP_HAL::millis();
             } else {
                 state.have_gps_yaw_accuracy = false;
             }

libraries/AP_GPS/GPS_Backend.cpp

@@ -302,20 +302,23 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
 }
 
 #if GPS_MOVING_BASELINE
-bool AP_GPS_Backend::calculate_moving_base_yaw(const float reported_heading_deg, const float reported_distance, const float reported_D)
+bool AP_GPS_Backend::calculate_moving_base_yaw(float reported_heading_deg, const float reported_distance, const float reported_D) {
-{
+    return calculate_moving_base_yaw(state, reported_heading_deg, reported_distance, reported_D);
+}
+
+bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, const float reported_heading_deg, const float reported_distance, const float reported_D) {
     constexpr float minimum_antenna_seperation = 0.05; // meters
     constexpr float permitted_error_length_pct = 0.2;  // percentage
 
     bool selectedOffset = false;
     Vector3f offset;
-    switch (MovingBase::Type(gps.mb_params[state.instance].type.get())) {
+    switch (MovingBase::Type(gps.mb_params[interim_state.instance].type.get())) {
         case MovingBase::Type::RelativeToAlternateInstance:
-            offset = gps._antenna_offset[state.instance^1].get() - gps._antenna_offset[state.instance].get();
+            offset = gps._antenna_offset[interim_state.instance^1].get() - gps._antenna_offset[interim_state.instance].get();
             selectedOffset = true;
             break;
         case MovingBase::Type::RelativeToCustomBase:
-            offset = gps.mb_params[state.instance].base_offset.get();
+            offset = gps.mb_params[interim_state.instance].base_offset.get();
             selectedOffset = true;
             break;
     }
@@ -385,13 +388,16 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(const float reported_heading_deg,
             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();
+            interim_state.gps_yaw = state.gps_yaw;
+            interim_state.have_gps_yaw = state.have_gps_yaw;
+            interim_state.gps_yaw_time_ms = AP_HAL::millis();
         }
     }
 
     return true;
 
 bad_yaw:
-    state.have_gps_yaw = false;
+    interim_state.have_gps_yaw = false;
     return false;
 }
 #endif // GPS_MOVING_BASELINE

libraries/AP_GPS/GPS_Backend.h

@@ -131,6 +131,7 @@ protected:
 
 #if GPS_MOVING_BASELINE
     bool calculate_moving_base_yaw(const float reported_heading_deg, const float reported_distance, const float reported_D);
+    bool calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, const float reported_heading_deg, const float reported_distance, const float reported_D);
 #endif //GPS_MOVING_BASELINE
 
     // get GPS type, for subtype config

libraries/AP_GPS/RTCM3_Parser.h

@@ -16,9 +16,10 @@
  RTCMv3 parser, used to support moving baseline RTK mode between two
  GPS modules
 */
-
+#pragma once
 #include <stdint.h>
 
+#define RTCM3_MAX_PACKET_LEN 300
 class RTCM3_Parser {
 public:
     // process one byte, return true if packet found
@@ -40,7 +41,7 @@ private:
     const uint8_t RTCMv3_PREAMBLE = 0xD3;
 
     // raw packet, we shouldn't need over 300 bytes for the MB configs we use
-    uint8_t pkt[300];
+    uint8_t pkt[RTCM3_MAX_PACKET_LEN];
 
     // number of bytes in pkt[]
     uint16_t pkt_bytes;