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

AP_GPS: fixed injection of RTCM on 2 different CAN buses 

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];
_detected_modules[j] = _detected_modules[j-1];
_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)
{
// we only handle this if we are the first UAVCAN GPS, as we send
// the data as broadcast on all UAVCAN devive ports and we don't
// want to send duplicates
if (_detected_module == 0) {
_detected_modules[0].ap_uavcan->send_RTCMStream(data, len);
// we only handle this if we are the first UAVCAN GPS or we are
// using a different uavcan instance than the first GPS, as we
// send the data as broadcast on all UAVCAN devive ports and we
// don't want to send duplicates
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);
}
}