diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp index 540120564d8075..94f0f96eb92e10 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp +++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp @@ -214,6 +214,18 @@ void CanardInterface::processTx(bool raw_commands_only = false) { if (txq == nullptr) { return; } + const auto *stats = ifaces[iface]->get_statistics(); + bool iface_down = true; + if (stats == nullptr || (AP_HAL::micros64() - stats->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 a successful transmit in the + last 500 milliseconds + */ + iface_down = false; + } // scan through list of pending transfers while (true) { auto txf = &txq->frame; @@ -240,15 +252,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<deadline_usec)) { + if (!iface_down) { + break; + } else { + txf->iface_mask &= ~(1U<iface_mask & (1U<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_mask &= ~(1U<