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

AP_DroneCAN: add support for detecting downed link

This commit is contained in:
bugobliterator 2023-10-17 16:33:02 +11:00 committed by Peter Barker
parent 6528394797
commit fb26bbfc4c
1 changed files with 25 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_DroneCAN/AP_Canard_iface.cpp
View File

@ -214,6 +214,20 @@ void CanardInterface::processTx(bool raw_commands_only = false) {
if (txq == nullptr) {
return;
}
// volatile as the value can change at any time during can interrupt
// we need to ensure that this is not optimized
volatile const auto *stats = ifaces[iface]->get_statistics();
uint64_t last_transmit_us = stats->last_transmit_us;
bool iface_down = true;
if (stats == nullptr || (AP_HAL::micros64() - last_transmit_us) < 200000UL) {
/*
We were not able to queue the frame for
sending. Only mark the send as failing if the
interface is active. We consider an interface as
active if it has had successful transmits for some time.
*/
iface_down = false;
}
// scan through list of pending transfers
while (true) {
auto txf = &txq->frame;
@ -240,15 +254,22 @@ void CanardInterface::processTx(bool raw_commands_only = false) {
if (!write) {
// if there is no space then we need to start from the
// top of the queue, so wait for the next loop
break;
}
if ((txf->iface_mask & (1U<<iface)) && (AP_HAL::micros64() < txf->deadline_usec)) {
if (!iface_down) {
break;
} else {
txf->iface_mask &= ~(1U<<iface);
}
} else if ((txf->iface_mask & (1U<<iface)) && (AP_HAL::micros64() < txf->deadline_usec)) {
// try sending to interfaces, clearing the mask if we succeed
if (ifaces[iface]->send(txmsg, txf->deadline_usec, 0) > 0) {
txf->iface_mask &= ~(1U<<iface);
} else {
// if we fail to send then we try sending on next interface
break;
if (!iface_down) {
break;
} else {
txf->iface_mask &= ~(1U<<iface);
}
}
}
// look at next transfer