From d0ced1b7b25ceb30b36adfb428975d857ba9b12f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 21 Oct 2019 22:11:26 +1100
Subject: [PATCH] AP_GPS: support RTCMStream for RTCM injection on UAVCAN

---
 libraries/AP_GPS/AP_GPS.cpp        | 54 ++++++++++++++++++------------
 libraries/AP_GPS/AP_GPS.h          |  8 +++--
 libraries/AP_GPS/AP_GPS_UAVCAN.cpp | 13 +++++++
 libraries/AP_GPS/AP_GPS_UAVCAN.h   |  2 ++
 4 files changed, 53 insertions(+), 24 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 4bc96256fe..bbb58285f0 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -841,9 +841,13 @@ void AP_GPS::handle_gps_inject(const mavlink_message_t &msg)
 {
     mavlink_gps_inject_data_t packet;
     mavlink_msg_gps_inject_data_decode(&msg, &packet);
-    //TODO: check target
 
-    inject_data(packet.data, packet.len);
+    if (packet.len > sizeof(packet.data)) {
+        // invalid packet
+        return;
+    }
+
+    handle_gps_rtcm_fragment(0, packet.data, packet.len);
 }
 
 /*
@@ -939,7 +943,7 @@ void AP_GPS::lock_port(uint8_t instance, bool lock)
 }
 
 // Inject a packet of raw binary to a GPS
-void AP_GPS::inject_data(uint8_t *data, uint16_t len)
+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) {
@@ -951,7 +955,7 @@ void AP_GPS::inject_data(uint8_t *data, uint16_t len)
     }
 }
 
-void AP_GPS::inject_data(uint8_t instance, uint8_t *data, uint16_t len)
+void AP_GPS::inject_data(uint8_t instance, const uint8_t *data, uint16_t len)
 {
     if (instance < GPS_MAX_RECEIVERS && drivers[instance] != nullptr) {
         drivers[instance]->inject_data(data, len);
@@ -1088,21 +1092,13 @@ bool AP_GPS::blend_health_check() const
 }
 
 /*
-   re-assemble GPS_RTCM_DATA message
+   re-assemble fragmented RTCM data
  */
-void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
+void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len)
 {
-    mavlink_gps_rtcm_data_t packet;
-    mavlink_msg_gps_rtcm_data_decode(&msg, &packet);
-
-    if (packet.len > MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) {
-        // invalid packet
-        return;
-    }
-
-    if ((packet.flags & 1) == 0) {
+    if ((flags & 1) == 0) {
         // it is not fragmented, pass direct
-        inject_data(packet.data, packet.len);
+        inject_data(data, len);
         return;
     }
 
@@ -1115,8 +1111,8 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
         }
     }
 
-    uint8_t fragment = (packet.flags >> 1U) & 0x03;
-    uint8_t sequence = (packet.flags >> 3U) & 0x1F;
+    uint8_t fragment = (flags >> 1U) & 0x03;
+    uint8_t sequence = (flags >> 3U) & 0x1F;
 
     // see if this fragment is consistent with existing fragments
     if (rtcm_buffer->fragments_received &&
@@ -1132,15 +1128,15 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
     rtcm_buffer->fragments_received |= (1U << fragment);
 
     // copy the data
-    memcpy(&rtcm_buffer->buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*(uint16_t)fragment], packet.data, packet.len);
+    memcpy(&rtcm_buffer->buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*(uint16_t)fragment], data, len);
 
     // when we get a fragment of less than max size then we know the
     // number of fragments. Note that this means if you want to send a
     // block of RTCM data of an exact multiple of the buffer size you
     // need to send a final packet of zero length
-    if (packet.len < MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) {
+    if (len < MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) {
         rtcm_buffer->fragment_count = fragment+1;
-        rtcm_buffer->total_length = (MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*fragment) + packet.len;
+        rtcm_buffer->total_length = (MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*fragment) + len;
     } else if (rtcm_buffer->fragments_received == 0x0F) {
         // special case of 4 full fragments
         rtcm_buffer->fragment_count = 4;
@@ -1157,6 +1153,22 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
     }
 }
 
+/*
+   re-assemble GPS_RTCM_DATA message
+ */
+void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
+{
+    mavlink_gps_rtcm_data_t packet;
+    mavlink_msg_gps_rtcm_data_decode(&msg, &packet);
+
+    if (packet.len > sizeof(packet.data)) {
+        // invalid packet
+        return;
+    }
+
+    handle_gps_rtcm_fragment(packet.flags, packet.data, packet.len);
+}
+
 void AP_GPS::Write_AP_Logger_Log_Startup_messages()
 {
     for (uint8_t instance=0; instance<num_instances; instance++) {
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index 8cc3fef6d1..50481ef234 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -436,6 +436,9 @@ public:
         _force_disable_gps = disable;
     }
 
+    // handle possibly fragmented RTCM injection data
+    void handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len);
+
 protected:
 
     // configuration parameters
@@ -546,9 +549,8 @@ private:
     void handle_gps_inject(const mavlink_message_t &msg);
 
     //Inject a packet of raw binary to a GPS
-    void inject_data(uint8_t *data, uint16_t len);
-    void inject_data(uint8_t instance, uint8_t *data, uint16_t len);
-
+    void inject_data(const uint8_t *data, uint16_t len);
+    void inject_data(uint8_t instance, const uint8_t *data, uint16_t len);
 
     // GPS blending and switching
     Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS]; // Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)
diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp
index fcdb740f81..8245f5a4a5 100644
--- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp
+++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp
@@ -435,4 +435,17 @@ bool AP_GPS_UAVCAN::read(void)
     return false;
 }
 
+/*
+  handle RTCM data from MAVLink GPS_RTCM_DATA, forwarding it over MAVLink
+ */
+void AP_GPS_UAVCAN::inject_data(const uint8_t *data, uint16_t len)
+{
+    // we only handle this if we are the first UAVCAN GPS, as we send
+    // the data as broadcast on all UAVCAN devive ports and we don't
+    // want to send duplicates
+    if (_detected_module == 0) {
+        _detected_modules[0].ap_uavcan->send_RTCMStream(data, len);
+    }
+}
+
 #endif // HAL_WITH_UAVCAN
diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.h b/libraries/AP_GPS/AP_GPS_UAVCAN.h
index 9ee9db198a..a064254815 100644
--- a/libraries/AP_GPS/AP_GPS_UAVCAN.h
+++ b/libraries/AP_GPS/AP_GPS_UAVCAN.h
@@ -45,6 +45,8 @@ public:
     static void handle_fix2_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Fix2Cb &cb);
     static void handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb);
 
+    void inject_data(const uint8_t *data, uint16_t len) override;
+
 private:
     void handle_fix_msg(const FixCb &cb);
     void handle_fix2_msg(const Fix2Cb &cb);