Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: fixed injection of RTCM on 2 different CAN buses

Browse Source 
if we have two CAN GPS on difference can drivers we need to inject to
both
This commit is contained in:
Andrew Tridgell 2022-11-07 18:27:15 +11:00 committed by Randy Mackay
parent b7d8285dfd
commit 18fa90e765
1 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

@ -367,6 +367,14 @@ AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t n
tempslot = _detected_modules[j]; tempslot = _detected_modules[j];
_detected_modules[j] = _detected_modules[j-1]; _detected_modules[j] = _detected_modules[j-1];
_detected_modules[j-1] = tempslot; _detected_modules[j-1] = tempslot;
// also fix the _detected_module in the driver so that RTCM injection
// can determine if it has the bus to itself
if (_detected_modules[j].driver) {
_detected_modules[j].driver->_detected_module = j;
}
if (_detected_modules[j-1].driver) {
_detected_modules[j-1].driver->_detected_module = j-1;
}
} }
} }
} }
@ -935,11 +943,13 @@ bool AP_GPS_UAVCAN::is_configured(void) const
*/ */
void AP_GPS_UAVCAN::inject_data(const uint8_t *data, uint16_t len) void AP_GPS_UAVCAN::inject_data(const uint8_t *data, uint16_t len)
{ {
// we only handle this if we are the first UAVCAN GPS, as we send // we only handle this if we are the first UAVCAN GPS or we are
// the data as broadcast on all UAVCAN devive ports and we don't // using a different uavcan instance than the first GPS, as we
// want to send duplicates // send the data as broadcast on all UAVCAN devive ports and we
if (_detected_module == 0) { // don't want to send duplicates
_detected_modules[0].ap_uavcan->send_RTCMStream(data, len); if (_detected_module == 0 ||
_detected_modules[_detected_module].ap_uavcan != _detected_modules[0].ap_uavcan) {
_detected_modules[_detected_module].ap_uavcan->send_RTCMStream(data, len);
} }
} }