|
|
@ -71,16 +71,6 @@ int16_t AP_HAL::CANIface::send(const CANFrame& frame, uint64_t tx_deadline, CanI |
|
|
|
{ |
|
|
|
{ |
|
|
|
if (frame_callback) { |
|
|
|
if (frame_callback) { |
|
|
|
if ((flags & IsMAVCAN) == 0) { |
|
|
|
if ((flags & IsMAVCAN) == 0) { |
|
|
|
if (frame_counter++ == 100) { |
|
|
|
|
|
|
|
// check every 100 frames for disabling CAN_FRAME send
|
|
|
|
|
|
|
|
// we stop sending after 5s if the client stops
|
|
|
|
|
|
|
|
// sending MAV_CMD_CAN_FORWARD requests
|
|
|
|
|
|
|
|
if (AP_HAL::millis() - last_callback_enable_ms > 5000) { |
|
|
|
|
|
|
|
frame_callback = nullptr; |
|
|
|
|
|
|
|
return 1; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
frame_counter = 0; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
frame_callback(get_iface_num(), frame); |
|
|
|
frame_callback(get_iface_num(), frame); |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
CanRxItem rx_item; |
|
|
|
CanRxItem rx_item; |
|
|
@ -98,7 +88,6 @@ int16_t AP_HAL::CANIface::send(const CANFrame& frame, uint64_t tx_deadline, CanI |
|
|
|
*/ |
|
|
|
*/ |
|
|
|
bool AP_HAL::CANIface::register_frame_callback(FrameCb cb) |
|
|
|
bool AP_HAL::CANIface::register_frame_callback(FrameCb cb) |
|
|
|
{ |
|
|
|
{ |
|
|
|
last_callback_enable_ms = AP_HAL::millis(); |
|
|
|
|
|
|
|
frame_callback = cb; |
|
|
|
frame_callback = cb; |
|
|
|
return true; |
|
|
|
return true; |
|
|
|
} |
|
|
|
} |
|
|
|