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

75
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
{
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);
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;
}
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,26 +558,34 @@ 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) {
// work out ID of this frame
uint16_t id = 0;
if ((frame.id&0xff) != 0) {
// not anonymous
if (frame.id & 0x80) {
// service message
id = uint8_t(frame.id>>16);
} else {
// message frame
id = uint16_t(frame.id>>8);
}
}
if (!bisect_search_uint16(can_forward.filter_ids, can_forward.num_filter_ids, id)) {
return;
if (can_forward.filter_ids != nullptr) {
// work out ID of this frame
uint16_t id = 0;
if ((frame.id&0xff) != 0) {
// not anonymous
if (frame.id & 0x80) {
// service message
id = uint8_t(frame.id>>16);
} else {
// message frame
id = uint16_t(frame.id>>8);
}
}
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));
if (!bisect_search_uint16(can_forward.filter_ids, can_forward.num_filter_ids, id)) {
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, data_len, frame.id, const_cast<uint8_t*>(frame.data));
}
}
}
#endif // HAL_GCS_ENABLED