AP_GPS: support GPS_RTCM_DATA fragmented MAVLink message

support re-assembly of RTCM data packets

libraries/AP_GPS/AP_GPS.cpp

@@ -493,8 +493,13 @@ AP_GPS::update(void)
   pass along a mavlink message (for MAV type)
  */
 void
-AP_GPS::handle_msg(mavlink_message_t *msg)
+AP_GPS::handle_msg(const mavlink_message_t *msg)
 {
+    if (msg->msgid == MAVLINK_MSG_ID_GPS_RTCM_DATA) {
+        // pass data to de-fragmenter
+        handle_gps_rtcm_data(msg);
+        return;
+    }
     uint8_t i;
     for (i=0; i<num_instances; i++) {
         if ((drivers[i] != NULL) && (_type[i] != GPS_TYPE_NONE)) {
@@ -709,3 +714,88 @@ AP_GPS::_broadcast_gps_type(const char *type, uint8_t instance, int8_t baud_inde
     }
     GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, buffer);
 }
+
+
+/* 
+   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 > MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) {
+        // invalid packet
+        return;
+    }
+    
+    if ((packet.flags & 1) == 0) {
+        // it is not fragmented, pass direct
+        inject_data_all(packet.data, packet.len);
+        return;
+    }
+
+    // see if we need to allocate re-assembly buffer
+    if (rtcm_buffer == nullptr) {
+        rtcm_buffer = (struct rtcm_buffer *)calloc(1, sizeof(*rtcm_buffer));
+        if (rtcm_buffer == nullptr) {
+            // nothing to do but discard the data
+            return;
+        }
+    }
+
+    uint8_t fragment = (packet.flags >> 1U) & 0x03;
+    uint8_t sequence = (packet.flags >> 3U) & 0x1F;
+
+    // see if this fragment is consistent with existing fragments
+    if (rtcm_buffer->fragments_received &&
+        (rtcm_buffer->sequence != sequence ||
+         rtcm_buffer->fragments_received & (1U<<fragment))) {
+        // we have one or more partial fragments already received
+        // which conflict with the new fragment, discard previous fragments
+        memset(rtcm_buffer, 0, sizeof(*rtcm_buffer));
+    }
+
+    // add this fragment
+    rtcm_buffer->sequence = sequence;
+    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);
+
+    // 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) {
+        rtcm_buffer->fragment_count = fragment+1;
+        rtcm_buffer->total_length = (MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*fragment) + packet.len;
+    } else if (rtcm_buffer->fragments_received == 0x0F) {
+        // special case of 4 full fragments
+        rtcm_buffer->fragment_count = 4;
+        rtcm_buffer->total_length = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*4;
+    }
+
+
+    // see if we have all fragments
+    if (rtcm_buffer->fragment_count != 0 &&
+        rtcm_buffer->fragments_received == (1U << rtcm_buffer->fragment_count) - 1) {
+        // we have them all, inject
+        inject_data_all(rtcm_buffer->buffer, rtcm_buffer->total_length);
+        memset(rtcm_buffer, 0, sizeof(*rtcm_buffer));
+    }
+}
+
+/*
+  inject data into all backends
+*/
+void AP_GPS::inject_data_all(const uint8_t *data, uint16_t len)
+{
+    uint8_t i;
+    for (i=0; i<num_instances; i++) {
+        if ((drivers[i] != NULL) && (_type[i] != GPS_TYPE_NONE)) {
+            drivers[i]->inject_data(data, len);
+        }
+    }
+    
+}

libraries/AP_GPS/AP_GPS.h

@@ -130,7 +130,7 @@ public:
     };
 
     // Pass mavlink data to message handlers (for MAV type)
-    void handle_msg(mavlink_message_t *msg);
+    void handle_msg(const mavlink_message_t *msg);
 
     // Accessor functions
 
@@ -412,6 +412,32 @@ private:
     void detect_instance(uint8_t instance);
     void update_instance(uint8_t instance);
     void _broadcast_gps_type(const char *type, uint8_t instance, int8_t baud_index);
+
+    /*
+      buffer for re-assembling RTCM data for GPS injection. 
+      The 8 bit flags field in GPS_RTCM_DATA is interpreted as:
+              1 bit for "is fragmented"
+              2 bits for fragment number
+              5 bits for sequence number
+
+      The rtcm_buffer is allocated on first use. Once a block of data
+      is successfully reassembled it is injected into all active GPS
+      backends. This assumes we don't want more than 4*180=720 bytes
+      in a RTCM data block
+     */
+    struct rtcm_buffer {
+        uint8_t fragments_received:4;
+        uint8_t sequence:5;
+        uint8_t fragment_count;
+        uint16_t total_length;
+        uint8_t buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*4];
+    } *rtcm_buffer;
+
+    // re-assemble GPS_RTCM_DATA message
+    void handle_gps_rtcm_data(const mavlink_message_t *msg);
+
+    // ibject data into all backends
+    void inject_data_all(const uint8_t *data, uint16_t len);
 };
 
 #define GPS_BAUD_TIME_MS 1200

libraries/AP_GPS/AP_GPS_ERB.h

@@ -134,7 +134,7 @@ private:
     // Buffer parse & GPS state update
     bool _parse_gps();
 
-    void inject_data(uint8_t *data, uint8_t len);
+    void inject_data(const uint8_t *data, uint16_t len) override;
 
     // used to update fix between status and position packets
     AP_GPS::GPS_Status next_fix;

libraries/AP_GPS/GPS_Backend.h

@@ -42,7 +42,7 @@ public:
 
     virtual bool is_configured(void) { return true; }
 
-    virtual void inject_data(uint8_t *data, uint8_t len) { return; }
+    virtual void inject_data(const uint8_t *data, uint16_t len) { return; }
 
     //MAVLink methods
     virtual void send_mavlink_gps_rtk(mavlink_channel_t chan) { return ; }
@@ -51,7 +51,7 @@ public:
 
     virtual void broadcast_configuration_failure_reason(void) const { return ; }
 
-    virtual void handle_msg(mavlink_message_t *msg) { return ; }
+    virtual void handle_msg(const mavlink_message_t *msg) { return ; }
 
 protected:
     AP_HAL::UARTDriver *port;           ///< UART we are attached to