From f5a06e6bd36fbe9882c211089d28d6da3c51aef6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell 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 6a04ae2cb1..c26a8c452a 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -844,9 +844,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); } /* @@ -942,7 +946,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) { @@ -954,7 +958,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); @@ -1071,21 +1075,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; } @@ -1098,8 +1094,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 && @@ -1115,15 +1111,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; @@ -1140,6 +1136,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; instancesend_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 23c44c4373..2eb8fd9ada 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.h +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.h @@ -43,6 +43,8 @@ public: static void handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FixCb &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_aux_msg(const AuxCb &cb);