diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index cc4001940a..4d57effbc9 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -379,6 +379,17 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) { stats.tx_overflow++; + if (stats.tx_success == 0) { + /* + if we have never successfully transmitted a frame + then we may be operating with just MAVCAN or UDP + MCAST. Consider the frame sent if the send + succeeds. This allows for UDP MCAST and MAVCAN to + operate fully when the CAN bus has no cable plugged + in + */ + return AP_HAL::CANIface::send(frame, tx_deadline, flags); + } return 0; //we don't have free space } index = ((can_->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos); diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index cd38315008..172e412b0e 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -325,6 +325,19 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, txmailbox = 2; } else { PERF_STATS(stats.tx_overflow); +#if !defined(HAL_BOOTLOADER_BUILD) + if (stats.tx_success == 0) { + /* + if we have never successfully transmitted a frame + then we may be operating with just MAVCAN or UDP + MCAST. Consider the frame sent if the send + succeeds. This allows for UDP MCAST and MAVCAN to + operate fully when the CAN bus has no cable plugged + in + */ + return AP_HAL::CANIface::send(frame, tx_deadline, flags); + } +#endif return 0; // No transmission for you. }