Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: allow for RTCMv3 packets larger than 300 bytes 

with newer receivers, RTCMv3 packets can be larger than 300
This commit is contained in:
Andrew Tridgell 2024-02-25 10:50:30 +11:00 committed by Randy Mackay
parent 127ea4ab95
commit 82d5b2ae8e
1 changed files with 28 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
Tools/AP_Periph/gps.cpp
View File

@ -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<<gps_mb_can_port;
}
#endif
// get the data from the moving base and send as MovingBaselineData message
while (len > 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
}