mirror of https://github.com/ArduPilot/ardupilot
AP_CANManager: support mavcan with CANFD_FRAME
This commit is contained in:
parent
b3b561f32b
commit
6320599404
|
@ -426,14 +426,29 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
|
|||
*/
|
||||
void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) const
|
||||
{
|
||||
const uint16_t timeout_us = 2000;
|
||||
switch (msg.msgid) {
|
||||
case MAVLINK_MSG_ID_CAN_FRAME: {
|
||||
mavlink_can_frame_t p;
|
||||
mavlink_msg_can_frame_decode(&msg, &p);
|
||||
if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
|
||||
return;
|
||||
}
|
||||
const uint16_t timeout_us = 2000;
|
||||
AP_HAL::CANFrame frame{p.id, p.data, p.len};
|
||||
hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN);
|
||||
break;
|
||||
}
|
||||
case MAVLINK_MSG_ID_CANFD_FRAME: {
|
||||
mavlink_canfd_frame_t p;
|
||||
mavlink_msg_canfd_frame_decode(&msg, &p);
|
||||
if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
|
||||
return;
|
||||
}
|
||||
AP_HAL::CANFrame frame{p.id, p.data, p.len, true};
|
||||
hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -527,7 +542,7 @@ void AP_CANManager::handle_can_filter_modify(const mavlink_message_t &msg)
|
|||
|
||||
/*
|
||||
handler for CAN frames from the registered callback, sending frames
|
||||
out as CAN_FRAME messages
|
||||
out as CAN_FRAME or CANFD_FRAME messages
|
||||
*/
|
||||
void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame)
|
||||
{
|
||||
|
@ -543,8 +558,7 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram
|
|||
can_forward.frame_counter = 0;
|
||||
}
|
||||
WITH_SEMAPHORE(comm_chan_lock(can_forward.chan));
|
||||
if (HAVE_PAYLOAD_SPACE(can_forward.chan, CAN_FRAME)) {
|
||||
if (can_forward.num_filter_ids != 0) {
|
||||
if (can_forward.filter_ids != nullptr) {
|
||||
// work out ID of this frame
|
||||
uint16_t id = 0;
|
||||
if ((frame.id&0xff) != 0) {
|
||||
|
@ -561,8 +575,17 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram
|
|||
return;
|
||||
}
|
||||
}
|
||||
const uint8_t data_len = AP_HAL::CANFrame::dlcToDataLength(frame.dlc);
|
||||
if (frame.isCanFDFrame()) {
|
||||
if (HAVE_PAYLOAD_SPACE(can_forward.chan, CANFD_FRAME)) {
|
||||
mavlink_msg_canfd_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id,
|
||||
bus, data_len, frame.id, const_cast<uint8_t*>(frame.data));
|
||||
}
|
||||
} else {
|
||||
if (HAVE_PAYLOAD_SPACE(can_forward.chan, CAN_FRAME)) {
|
||||
mavlink_msg_can_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id,
|
||||
bus, frame.dlc, frame.id, const_cast<uint8_t*>(frame.data));
|
||||
bus, data_len, frame.id, const_cast<uint8_t*>(frame.data));
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // HAL_GCS_ENABLED
|
||||
|
|
Loading…
Reference in New Issue