diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp index 540120564d807..5fa96606f5be4 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp +++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp @@ -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<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<