diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 474c069ae0..0efc3ec17b 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1101,7 +1101,8 @@ void AP_Periph_FW::processTx(void) #endif // push message with 1s timeout bool sent = true; - const uint64_t deadline = AP_HAL::micros64() + 1000000; + const uint64_t now_us = AP_HAL::micros64(); + const uint64_t deadline = now_us + 1000000U; // try sending to all interfaces for (auto &_ins : instances) { if (_ins.iface == NULL) { @@ -1118,7 +1119,17 @@ void AP_Periph_FW::processTx(void) } #endif if (_ins.iface->send(txmsg, deadline, 0) <= 0) { - sent = false; + /* + 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 2 seconds + */ + const auto *stats = _ins.iface->get_statistics(); + if (stats == nullptr || now_us - stats->last_transmit_us < 2000000UL) { + sent = false; + } } } if (sent) {