Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_CANManager: support mavcan with CANFD_FRAME

This commit is contained in:
Andrew Tridgell 2022-02-19 09:35:51 +11:00
parent b3b561f32b
commit 6320599404
1 changed files with 49 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_CANManager/AP_CANManager.cpp
View File

@ -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 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_can_frame_t p;
mavlink_msg_can_frame_decode(&msg, &p); mavlink_msg_can_frame_decode(&msg, &p);
if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) { if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
return; return;
} }
const uint16_t timeout_us = 2000;
AP_HAL::CANFrame frame{p.id, p.data, p.len}; 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); 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 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) 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; can_forward.frame_counter = 0;
} }
WITH_SEMAPHORE(comm_chan_lock(can_forward.chan)); WITH_SEMAPHORE(comm_chan_lock(can_forward.chan));
if (HAVE_PAYLOAD_SPACE(can_forward.chan, CAN_FRAME)) { if (can_forward.filter_ids != nullptr) {
if (can_forward.num_filter_ids != 0) {
// work out ID of this frame // work out ID of this frame
uint16_t id = 0; uint16_t id = 0;
if ((frame.id&0xff) != 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; 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, 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 #endif // HAL_GCS_ENABLED