diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index d90a137641..b0a0616e3a 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/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 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<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; iinject_data(data, len); + } + } + +} diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 51daf191fd..27db42b16a 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/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 diff --git a/libraries/AP_GPS/AP_GPS_ERB.h b/libraries/AP_GPS/AP_GPS_ERB.h index 537c6699df..0209ceea9a 100644 --- a/libraries/AP_GPS/AP_GPS_ERB.h +++ b/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; diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 185f37676b..deda226b8a 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/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