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 }