diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index aeafeec7b75..8f8bf0a19d7 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -424,291 +424,6 @@ void AP_CANManager::log_retrieve(ExpandingString &str) const str.append(_log_buf, _log_pos); } -#if HAL_GCS_ENABLED -/* - handle MAV_CMD_CAN_FORWARD mavlink long command - */ -bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg) -{ - WITH_SEMAPHORE(can_forward.sem); - const int8_t bus = int8_t(packet.param1)-1; - - if (bus == -1) { - /* - a request to stop forwarding - */ - if (can_forward.callback_id != 0) { - hal.can[can_forward.callback_bus]->unregister_frame_callback(can_forward.callback_id); - can_forward.callback_id = 0; - } - return true; - } - - if (bus >= HAL_NUM_CAN_IFACES || hal.can[bus] == nullptr) { - return false; - } - - if (can_forward.callback_id != 0 && can_forward.callback_bus != bus) { - /* - the client is changing which bus they are monitoring, unregister from the previous bus - */ - hal.can[can_forward.callback_bus]->unregister_frame_callback(can_forward.callback_id); - can_forward.callback_id = 0; - } - - if (can_forward.callback_id == 0 && - !hal.can[bus]->register_frame_callback( - FUNCTOR_BIND_MEMBER(&AP_CANManager::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &, AP_HAL::CANIface::CanIOFlags), can_forward.callback_id)) { - // failed to register the callback - return false; - } - - can_forward.callback_bus = bus; - can_forward.last_callback_enable_ms = AP_HAL::millis(); - can_forward.chan = chan; - can_forward.system_id = msg.sysid; - can_forward.component_id = msg.compid; - - return true; -} - -/* - handle a CAN_FRAME packet - */ -void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) -{ - if (frame_buffer == nullptr) { - // allocate frame buffer - WITH_SEMAPHORE(_sem); - // 20 is good for firmware upload - uint8_t buffer_size = 20; - while (frame_buffer == nullptr && buffer_size > 0) { - // we'd like 20 frames, but will live with less - frame_buffer = NEW_NOTHROW ObjectBuffer(buffer_size); - if (frame_buffer != nullptr && frame_buffer->get_size() != 0) { - // register a callback for when frames can't be sent immediately - hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_CANManager::process_frame_buffer, void)); - break; - } - delete frame_buffer; - frame_buffer = nullptr; - buffer_size /= 2; - } - if (frame_buffer == nullptr) { - // discard the frames - return; - } - } - - 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; - } - struct BufferFrame frame { - bus : p.bus, - frame : AP_HAL::CANFrame(p.id, p.data, p.len) - }; - WITH_SEMAPHORE(_sem); - frame_buffer->push(frame); - break; - } -#if HAL_CANFD_SUPPORTED - 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; - } - struct BufferFrame frame { - bus : p.bus, - frame : AP_HAL::CANFrame(p.id, p.data, p.len, true) - }; - WITH_SEMAPHORE(_sem); - frame_buffer->push(frame); - break; - } -#endif - } - process_frame_buffer(); -} - -/* - process the frame buffer - */ -void AP_CANManager::process_frame_buffer(void) -{ - while (frame_buffer) { - WITH_SEMAPHORE(_sem); - struct BufferFrame frame; - const uint16_t timeout_us = 2000; - if (!frame_buffer->peek(frame)) { - // no frames in the queue - break; - } - const int16_t retcode = hal.can[frame.bus]->send(frame.frame, - AP_HAL::micros64() + timeout_us, - AP_HAL::CANIface::IsForwardedFrame); - if (retcode == 0) { - // no space in the CAN output slots, try again later - break; - } - // retcode == 1 means sent, -1 means a frame that can't be - // sent. Either way we should remove from the queue - frame_buffer->pop(); - } -} - -/* - handle a CAN_FILTER_MODIFY packet - */ -void AP_CANManager::handle_can_filter_modify(const mavlink_message_t &msg) -{ - mavlink_can_filter_modify_t p; - mavlink_msg_can_filter_modify_decode(&msg, &p); - const int8_t bus = int8_t(p.bus)-1; - if (bus >= HAL_NUM_CAN_IFACES || hal.can[bus] == nullptr) { - return; - } - if (p.num_ids > ARRAY_SIZE(p.ids)) { - return; - } - uint16_t *new_ids = nullptr; - uint16_t num_new_ids = 0; - WITH_SEMAPHORE(can_forward.sem); - - // sort the list, so we can bisection search and the array - // operations below are efficient - insertion_sort_uint16(p.ids, p.num_ids); - - switch (p.operation) { - case CAN_FILTER_REPLACE: { - if (p.num_ids == 0) { - can_forward.num_filter_ids = 0; - delete[] can_forward.filter_ids; - can_forward.filter_ids = nullptr; - return; - } - if (p.num_ids == can_forward.num_filter_ids && - memcmp(p.ids, can_forward.filter_ids, p.num_ids*sizeof(uint16_t)) == 0) { - // common case of replacing with identical list - return; - } - new_ids = NEW_NOTHROW uint16_t[p.num_ids]; - if (new_ids != nullptr) { - num_new_ids = p.num_ids; - memcpy((void*)new_ids, (const void *)p.ids, p.num_ids*sizeof(uint16_t)); - } - break; - } - case CAN_FILTER_ADD: { - if (common_list_uint16(can_forward.filter_ids, can_forward.num_filter_ids, - p.ids, p.num_ids) == p.num_ids) { - // nothing changing - return; - } - new_ids = NEW_NOTHROW uint16_t[can_forward.num_filter_ids+p.num_ids]; - if (new_ids == nullptr) { - return; - } - if (can_forward.num_filter_ids != 0) { - memcpy(new_ids, can_forward.filter_ids, can_forward.num_filter_ids*sizeof(uint16_t)); - } - memcpy(&new_ids[can_forward.num_filter_ids], p.ids, p.num_ids*sizeof(uint16_t)); - insertion_sort_uint16(new_ids, can_forward.num_filter_ids+p.num_ids); - num_new_ids = remove_duplicates_uint16(new_ids, can_forward.num_filter_ids+p.num_ids); - break; - } - case CAN_FILTER_REMOVE: { - if (common_list_uint16(can_forward.filter_ids, can_forward.num_filter_ids, - p.ids, p.num_ids) == 0) { - // nothing changing - return; - } - can_forward.num_filter_ids = remove_list_uint16(can_forward.filter_ids, can_forward.num_filter_ids, - p.ids, p.num_ids); - if (can_forward.num_filter_ids == 0) { - delete[] can_forward.filter_ids; - can_forward.filter_ids = nullptr; - } - break; - } - } - if (new_ids != nullptr) { - // handle common case of no change - if (num_new_ids == can_forward.num_filter_ids && - memcmp(new_ids, can_forward.filter_ids, num_new_ids*sizeof(uint16_t)) == 0) { - delete[] new_ids; - } else { - // put the new list in place - delete[] can_forward.filter_ids; - can_forward.filter_ids = new_ids; - can_forward.num_filter_ids = num_new_ids; - } - } -} - -/* - handler for CAN frames from the registered callback, sending frames - out as CAN_FRAME or CANFD_FRAME messages - */ -void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags) -{ - WITH_SEMAPHORE(can_forward.sem); - if (bus != can_forward.callback_bus) { - // we are not registered for forwarding this bus, discard frame - return; - } - if (can_forward.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 (can_forward.callback_id != 0 && - AP_HAL::millis() - can_forward.last_callback_enable_ms > 5000) { - hal.can[bus]->unregister_frame_callback(can_forward.callback_id); - can_forward.callback_id = 0; - return; - } - can_forward.frame_counter = 0; - } - WITH_SEMAPHORE(comm_chan_lock(can_forward.chan)); - 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); - } - } - 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 HAL_CANFD_SUPPORTED - 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(frame.data)); - } - } else -#endif - { - 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(frame.data)); - } - } -} -#endif // HAL_GCS_ENABLED - #if AP_CAN_LOGGING_ENABLED /* handler for CAN frames for frame logging diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h index ed63e4ee72c..367e357e0e1 100644 --- a/libraries/AP_CANManager/AP_CANManager.h +++ b/libraries/AP_CANManager/AP_CANManager.h @@ -28,8 +28,7 @@ #include "AP_CANDriver.h" #include #if HAL_GCS_ENABLED -#include -#include +#include "AP_MAVLinkCAN.h" #endif #include "AP_CAN.h" @@ -106,9 +105,20 @@ public: static const struct AP_Param::GroupInfo var_info[]; #if HAL_GCS_ENABLED - bool handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg); - void handle_can_frame(const mavlink_message_t &msg); - void handle_can_filter_modify(const mavlink_message_t &msg); + inline bool handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg) + { + return mavlink_can.handle_can_forward(chan, packet, msg); + } + + inline void handle_can_frame(const mavlink_message_t &msg) + { + mavlink_can.handle_can_frame(msg); + } + + inline void handle_can_filter_modify(const mavlink_message_t &msg) + { + mavlink_can.handle_can_filter_modify(msg); + } #endif private: @@ -183,33 +193,8 @@ private: HAL_Semaphore _sem; #if HAL_GCS_ENABLED - /* - handler for CAN frames from the registered callback, sending frames - out as CAN_FRAME messages - */ - void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags); - - struct { - mavlink_channel_t chan; - uint8_t system_id; - uint8_t component_id; - uint8_t frame_counter; - uint32_t last_callback_enable_ms; - HAL_Semaphore sem; - uint16_t num_filter_ids; - uint16_t *filter_ids; - uint8_t callback_id; - uint8_t callback_bus; - } can_forward; - - // buffer for MAVCAN frames - struct BufferFrame { - uint8_t bus; - AP_HAL::CANFrame frame; - }; - ObjectBuffer *frame_buffer; - - void process_frame_buffer(void); + // Class for handling MAVLink CAN frames + AP_MAVLinkCAN mavlink_can; #endif // HAL_GCS_ENABLED #if AP_CAN_LOGGING_ENABLED && HAL_LOGGING_ENABLED diff --git a/libraries/AP_CANManager/AP_MAVLinkCAN.cpp b/libraries/AP_CANManager/AP_MAVLinkCAN.cpp new file mode 100644 index 00000000000..e4be2bc99b6 --- /dev/null +++ b/libraries/AP_CANManager/AP_MAVLinkCAN.cpp @@ -0,0 +1,307 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include "AP_MAVLinkCAN.h" +#include +#include + +#if HAL_CANMANAGER_ENABLED && HAL_GCS_ENABLED + +extern const AP_HAL::HAL& hal; + +/* + handle MAV_CMD_CAN_FORWARD mavlink long command + */ +bool AP_MAVLinkCAN::handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + WITH_SEMAPHORE(can_forward.sem); + const int8_t bus = int8_t(packet.param1)-1; + + if (bus == -1) { + /* + a request to stop forwarding + */ + if (can_forward.callback_id != 0) { + hal.can[can_forward.callback_bus]->unregister_frame_callback(can_forward.callback_id); + can_forward.callback_id = 0; + } + return true; + } + + if (bus >= HAL_NUM_CAN_IFACES || hal.can[bus] == nullptr) { + return false; + } + + if (can_forward.callback_id != 0 && can_forward.callback_bus != bus) { + /* + the client is changing which bus they are monitoring, unregister from the previous bus + */ + hal.can[can_forward.callback_bus]->unregister_frame_callback(can_forward.callback_id); + can_forward.callback_id = 0; + } + + if (can_forward.callback_id == 0 && + !hal.can[bus]->register_frame_callback( + FUNCTOR_BIND_MEMBER(&AP_MAVLinkCAN::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &, AP_HAL::CANIface::CanIOFlags), can_forward.callback_id)) { + // failed to register the callback + return false; + } + + can_forward.callback_bus = bus; + can_forward.last_callback_enable_ms = AP_HAL::millis(); + can_forward.chan = chan; + can_forward.system_id = msg.sysid; + can_forward.component_id = msg.compid; + + return true; +} + +/* + handle a CAN_FRAME packet + */ +void AP_MAVLinkCAN::handle_can_frame(const mavlink_message_t &msg) +{ + if (frame_buffer == nullptr) { + // allocate frame buffer + WITH_SEMAPHORE(can_forward.sem); + // 20 is good for firmware upload + uint8_t buffer_size = 20; + while (frame_buffer == nullptr && buffer_size > 0) { + // we'd like 20 frames, but will live with less + frame_buffer = NEW_NOTHROW ObjectBuffer(buffer_size); + if (frame_buffer != nullptr && frame_buffer->get_size() != 0) { + // register a callback for when frames can't be sent immediately + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_MAVLinkCAN::process_frame_buffer, void)); + break; + } + delete frame_buffer; + frame_buffer = nullptr; + buffer_size /= 2; + } + if (frame_buffer == nullptr) { + // discard the frames + return; + } + } + + 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; + } + struct BufferFrame frame { + bus : p.bus, + frame : AP_HAL::CANFrame(p.id, p.data, p.len) + }; + WITH_SEMAPHORE(can_forward.sem); + frame_buffer->push(frame); + break; + } +#if HAL_CANFD_SUPPORTED + 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; + } + struct BufferFrame frame { + bus : p.bus, + frame : AP_HAL::CANFrame(p.id, p.data, p.len, true) + }; + WITH_SEMAPHORE(can_forward.sem); + frame_buffer->push(frame); + break; + } +#endif + } + process_frame_buffer(); +} + +/* + process the frame buffer + */ +void AP_MAVLinkCAN::process_frame_buffer() +{ + while (frame_buffer) { + WITH_SEMAPHORE(can_forward.sem); + struct BufferFrame frame; + const uint16_t timeout_us = 2000; + if (!frame_buffer->peek(frame)) { + // no frames in the queue + break; + } + const int16_t retcode = hal.can[frame.bus]->send(frame.frame, + AP_HAL::micros64() + timeout_us, + AP_HAL::CANIface::IsForwardedFrame); + if (retcode == 0) { + // no space in the CAN output slots, try again later + break; + } + // retcode == 1 means sent, -1 means a frame that can't be + // sent. Either way we should remove from the queue + frame_buffer->pop(); + } +} + +/* + handle a CAN_FILTER_MODIFY packet + */ +void AP_MAVLinkCAN::handle_can_filter_modify(const mavlink_message_t &msg) +{ + mavlink_can_filter_modify_t p; + mavlink_msg_can_filter_modify_decode(&msg, &p); + const int8_t bus = int8_t(p.bus)-1; + if (bus >= HAL_NUM_CAN_IFACES || hal.can[bus] == nullptr) { + return; + } + if (p.num_ids > ARRAY_SIZE(p.ids)) { + return; + } + uint16_t *new_ids = nullptr; + uint16_t num_new_ids = 0; + WITH_SEMAPHORE(can_forward.sem); + + // sort the list, so we can bisection search and the array + // operations below are efficient + insertion_sort_uint16(p.ids, p.num_ids); + + switch (p.operation) { + case CAN_FILTER_REPLACE: { + if (p.num_ids == 0) { + can_forward.num_filter_ids = 0; + delete[] can_forward.filter_ids; + can_forward.filter_ids = nullptr; + return; + } + if (p.num_ids == can_forward.num_filter_ids && + memcmp(p.ids, can_forward.filter_ids, p.num_ids*sizeof(uint16_t)) == 0) { + // common case of replacing with identical list + return; + } + new_ids = NEW_NOTHROW uint16_t[p.num_ids]; + if (new_ids != nullptr) { + num_new_ids = p.num_ids; + memcpy((void*)new_ids, (const void *)p.ids, p.num_ids*sizeof(uint16_t)); + } + break; + } + case CAN_FILTER_ADD: { + if (common_list_uint16(can_forward.filter_ids, can_forward.num_filter_ids, + p.ids, p.num_ids) == p.num_ids) { + // nothing changing + return; + } + new_ids = NEW_NOTHROW uint16_t[can_forward.num_filter_ids+p.num_ids]; + if (new_ids == nullptr) { + return; + } + if (can_forward.num_filter_ids != 0) { + memcpy(new_ids, can_forward.filter_ids, can_forward.num_filter_ids*sizeof(uint16_t)); + } + memcpy(&new_ids[can_forward.num_filter_ids], p.ids, p.num_ids*sizeof(uint16_t)); + insertion_sort_uint16(new_ids, can_forward.num_filter_ids+p.num_ids); + num_new_ids = remove_duplicates_uint16(new_ids, can_forward.num_filter_ids+p.num_ids); + break; + } + case CAN_FILTER_REMOVE: { + if (common_list_uint16(can_forward.filter_ids, can_forward.num_filter_ids, + p.ids, p.num_ids) == 0) { + // nothing changing + return; + } + can_forward.num_filter_ids = remove_list_uint16(can_forward.filter_ids, can_forward.num_filter_ids, + p.ids, p.num_ids); + if (can_forward.num_filter_ids == 0) { + delete[] can_forward.filter_ids; + can_forward.filter_ids = nullptr; + } + break; + } + } + if (new_ids != nullptr) { + // handle common case of no change + if (num_new_ids == can_forward.num_filter_ids && + memcmp(new_ids, can_forward.filter_ids, num_new_ids*sizeof(uint16_t)) == 0) { + delete[] new_ids; + } else { + // put the new list in place + delete[] can_forward.filter_ids; + can_forward.filter_ids = new_ids; + can_forward.num_filter_ids = num_new_ids; + } + } +} + +/* + handler for CAN frames from the registered callback, sending frames + out as CAN_FRAME or CANFD_FRAME messages + */ +void AP_MAVLinkCAN::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags) +{ + WITH_SEMAPHORE(can_forward.sem); + if (bus != can_forward.callback_bus) { + // we are not registered for forwarding this bus, discard frame + return; + } + if (can_forward.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 (can_forward.callback_id != 0 && + AP_HAL::millis() - can_forward.last_callback_enable_ms > 5000) { + hal.can[bus]->unregister_frame_callback(can_forward.callback_id); + can_forward.callback_id = 0; + return; + } + can_forward.frame_counter = 0; + } + WITH_SEMAPHORE(comm_chan_lock(can_forward.chan)); + 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); + } + } + 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 HAL_CANFD_SUPPORTED + 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(frame.data)); + } + } else +#endif + { + 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(frame.data)); + } + } +} + +#endif // HAL_CANMANAGER_ENABLED && HAL_GCS_ENABLED diff --git a/libraries/AP_CANManager/AP_MAVLinkCAN.h b/libraries/AP_CANManager/AP_MAVLinkCAN.h new file mode 100644 index 00000000000..0132ad04bc3 --- /dev/null +++ b/libraries/AP_CANManager/AP_MAVLinkCAN.h @@ -0,0 +1,75 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once + +#include +#include +#include "AP_CANManager_config.h" + +#if HAL_CANMANAGER_ENABLED && HAL_GCS_ENABLED + +#include +#include +#include +#include + +class AP_MAVLinkCAN { +public: + AP_MAVLinkCAN() {} + + // Process CAN frame forwarding + void process_frame_buffer(); + + // Handle commands to forward CAN frames to GCS + bool handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg); + + // Handle received MAVLink CAN frames + void handle_can_frame(const mavlink_message_t &msg); + + // Handle CAN filter modification + void handle_can_filter_modify(const mavlink_message_t &msg); + +private: + // Callback for receiving CAN frames from CAN bus and sending to GCS + void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags); + + /* + * Structure to maintain forwarding state + */ + struct { + mavlink_channel_t chan; + uint8_t system_id; + uint8_t component_id; + uint8_t frame_counter; + uint32_t last_callback_enable_ms; + HAL_Semaphore sem; + uint16_t num_filter_ids; + uint16_t *filter_ids; + uint8_t callback_id; + uint8_t callback_bus; + } can_forward; + + // Buffer for storing CAN frames to be sent + struct BufferFrame { + uint8_t bus; + AP_HAL::CANFrame frame; + }; + + // Frame buffer for queuing frames + ObjectBuffer *frame_buffer; +}; + +#endif // HAL_CANMANAGER_ENABLED && HAL_GCS_ENABLED