diff --git a/Tools/AP_Periph/gps.cpp b/Tools/AP_Periph/gps.cpp index 029676f26d1b8..f185f709a0720 100644 --- a/Tools/AP_Periph/gps.cpp +++ b/Tools/AP_Periph/gps.cpp @@ -249,14 +249,11 @@ void AP_Periph_FW::send_moving_baseline_msg() if (len == 0 || data == nullptr) { return; } - // send the packet from Moving Base to be used RelPosHeading calc by GPS module - ardupilot_gnss_MovingBaselineData mbldata {}; - // get the data from the moving base - static_assert(sizeof(ardupilot_gnss_MovingBaselineData::data.data) == RTCM3_MAX_PACKET_LEN, "Size of Moving Base data is wrong"); - mbldata.data.len = len; - memcpy(mbldata.data.data, data, len); - uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !canfdout()); + + /* + send the packet from Moving Base to be used RelPosHeading calc by GPS module + for long RTCMv3 packets we may need to split it up + */ uint8_t iface_mask = 0; #if HAL_NUM_CAN_IFACES >= 2 && CANARD_MULTI_IFACE @@ -264,12 +261,29 @@ void AP_Periph_FW::send_moving_baseline_msg() iface_mask = 1U< 0) { + ardupilot_gnss_MovingBaselineData mbldata {}; + + const uint16_t n = MIN(len, sizeof(mbldata.data.data)); + + mbldata.data.len = n; + memcpy(mbldata.data.data, data, n); + + uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {}; + const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !canfdout()); + + canard_broadcast(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, + ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size, + iface_mask); + len -= n; + data += n; + } + gps.clear_RTCMV3(); #endif // GPS_MOVING_BASELINE } diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index b67fea52eb88b..4e2ccc5c840b9 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -55,6 +55,10 @@ #include #include "AP_GPS_FixType.h" +#if AP_GPS_RTCM_DECODE_ENABLED +#include "RTCM3_Parser.h" +#endif + #define GPS_RTK_INJECT_TO_ALL 127 #ifndef GPS_MAX_RATE_MS #define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms @@ -341,7 +345,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: _DRV_OPTIONS // @DisplayName: driver options // @Description: Additional backend specific options - // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health + // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health, 6:Enable RTCM full parse even for a single channel, 7:Disable automatic full RTCM parsing when RTCM seen on more than one channel // @User: Advanced AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0), @@ -1338,12 +1342,12 @@ void AP_GPS::handle_gps_inject(const mavlink_message_t &msg) /* pass along a mavlink message (for MAV type) */ -void AP_GPS::handle_msg(const mavlink_message_t &msg) +void AP_GPS::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg) { switch (msg.msgid) { case MAVLINK_MSG_ID_GPS_RTCM_DATA: // pass data to de-fragmenter - handle_gps_rtcm_data(msg); + handle_gps_rtcm_data(chan, msg); break; case MAVLINK_MSG_ID_GPS_INJECT_DATA: handle_gps_inject(msg); @@ -1676,7 +1680,7 @@ void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_ /* re-assemble GPS_RTCM_DATA message */ -void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg) +void AP_GPS::handle_gps_rtcm_data(mavlink_channel_t chan, const mavlink_message_t &msg) { mavlink_gps_rtcm_data_t packet; mavlink_msg_gps_rtcm_data_decode(&msg, &packet); @@ -1686,9 +1690,89 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg) return; } +#if AP_GPS_RTCM_DECODE_ENABLED + if (!option_set(DriverOptions::DisableRTCMDecode)) { + const uint16_t mask = (1U << unsigned(chan)); + rtcm.seen_mav_channels |= mask; + if (option_set(DriverOptions::AlwaysRTCMDecode) || + (rtcm.seen_mav_channels & ~mask) != 0) { + /* + we are seeing RTCM on multiple mavlink channels. We will run + the data through a full per-channel RTCM decoder + */ + if (parse_rtcm_injection(chan, packet)) { + return; + } + } + } +#endif + handle_gps_rtcm_fragment(packet.flags, packet.data, packet.len); } +#if AP_GPS_RTCM_DECODE_ENABLED +/* + fully parse RTCM data coming in from a MAVLink channel, when we have + a full message inject it to the GPS. This approach allows for 2 or + more MAVLink channels to be used for the same RTCM data, allowing + for redundent transports for maximum reliability at the cost of some + extra CPU and a bit of re-assembly lag + */ +bool AP_GPS::parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm_data_t &pkt) +{ + if (chan >= MAVLINK_COMM_NUM_BUFFERS) { + return false; + } + if (rtcm.parsers[chan] == nullptr) { + rtcm.parsers[chan] = new RTCM3_Parser(); + if (rtcm.parsers[chan] == nullptr) { + return false; + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS: RTCM parsing for chan %u", unsigned(chan)); + } + for (uint16_t i=0; iread(pkt.data[i])) { + // we have a full message, inject it + const uint8_t *buf = nullptr; + uint16_t len = rtcm.parsers[chan]->get_len(buf); + + // see if we have already sent it. This prevents + // duplicates from multiple sources + const uint32_t crc = crc_crc32(0, buf, len); + +#if HAL_LOGGING_ENABLED + AP::logger().WriteStreaming("RTCM", "TimeUS,Chan,RTCMId,Len,CRC", "s#---", "F----", "QBHHI", + AP_HAL::micros64(), + uint8_t(chan), + rtcm.parsers[chan]->get_id(), + len, + crc); +#endif + + bool already_seen = false; + for (uint8_t c=0; c 0) { + inject_data(buf, len); + } + rtcm.parsers[chan]->reset(); + } + } + return true; +} +#endif // AP_GPS_RTCM_DECODE_ENABLED + void AP_GPS::Write_AP_Logger_Log_Startup_messages() { for (uint8_t instance=0; instance 1024 +#endif diff --git a/libraries/AP_GPS/RTCM3_Parser.h b/libraries/AP_GPS/RTCM3_Parser.h index 4c514abb45396..0b15e5b701b92 100644 --- a/libraries/AP_GPS/RTCM3_Parser.h +++ b/libraries/AP_GPS/RTCM3_Parser.h @@ -19,7 +19,10 @@ #pragma once #include -#define RTCM3_MAX_PACKET_LEN 300 +// maximum packet length with MAVLink GPS_RTCM_DATA is 4*180 as we +// handle up to 4 fragments +#define RTCM3_MAX_PACKET_LEN 720 + class RTCM3_Parser { public: // process one byte, return true if packet found @@ -40,7 +43,7 @@ class RTCM3_Parser { private: const uint8_t RTCMv3_PREAMBLE = 0xD3; - // raw packet, we shouldn't need over 300 bytes for the MB configs we use + // raw packet, we shouldn't need over 600 bytes for the MB configs we use uint8_t pkt[RTCM3_MAX_PACKET_LEN]; // number of bytes in pkt[] diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index b04f34bd751f8..f58e3e45c4023 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4107,7 +4107,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_GPS_INPUT: case MAVLINK_MSG_ID_HIL_GPS: case MAVLINK_MSG_ID_GPS_INJECT_DATA: - AP::gps().handle_msg(msg); + AP::gps().handle_msg(chan, msg); break; #endif