diff --git a/libraries/AP_CANManager/AP_SLCANIface.cpp b/libraries/AP_CANManager/AP_SLCANIface.cpp index 785b7a7ff7..8060c31126 100644 --- a/libraries/AP_CANManager/AP_SLCANIface.cpp +++ b/libraries/AP_CANManager/AP_SLCANIface.cpp @@ -666,16 +666,19 @@ int16_t SLCAN::CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& rx_time, // flush bytes from port while (num_bytes--) { int16_t ret = _port->read_locked(_serial_lock_key); - if (ret <= 0) { + if (ret < 0) { break; } addByte(ret); + if (!rx_queue_.space()) { + break; + } } } if (rx_queue_.available()) { // if we already have something in buffer transmit it CanRxItem frm; - if (!rx_queue_.pop(frm)) { + if (!rx_queue_.peek(frm)) { return 0; } out_frame = frm.frame; @@ -685,7 +688,19 @@ int16_t SLCAN::CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& rx_time, // Also send this frame over can_iface when in passthrough mode, // We just push this frame without caring for priority etc if (_can_iface) { - _can_iface->send(out_frame, AP_HAL::native_micros64() + 1000, out_flags); + if (_can_iface->send(out_frame, AP_HAL::native_micros64() + 100000, out_flags) == 1) { + rx_queue_.pop(); + num_tries = 0; + } else if (num_tries > 8) { + rx_queue_.pop(); + num_tries = 0; + } else { + num_tries++; + } + } else { + // we just throw away frames if we don't + // have any can iface to pass through to + rx_queue_.pop(); } return 1; } diff --git a/libraries/AP_CANManager/AP_SLCANIface.h b/libraries/AP_CANManager/AP_SLCANIface.h index d10266b49b..ab828f8a4e 100644 --- a/libraries/AP_CANManager/AP_SLCANIface.h +++ b/libraries/AP_CANManager/AP_SLCANIface.h @@ -79,6 +79,7 @@ class CANIface: public AP_HAL::CANIface int8_t _prev_ser_port; int8_t _iface_num = -1; uint32_t _last_had_activity; + uint8_t num_tries; AP_HAL::CANIface* _can_iface; // Can interface to be used for interaction by SLCAN interface HAL_Semaphore port_sem; bool _set_by_sermgr;