From c2864b5733daffa2fdaa38276255d02ccae7845d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2024 14:59:21 +1100 Subject: [PATCH] AP_CANManager: support CAN frame logging this supports logging of all bxCAN and CANFD frames, which helps with debugging tricky CAN support issues and for the development of new CAN driver lua scripts --- libraries/AP_CANManager/AP_CANIfaceParams.cpp | 8 ++ libraries/AP_CANManager/AP_CANManager.cpp | 75 +++++++++++++++++- libraries/AP_CANManager/AP_CANManager.h | 21 +++++ .../AP_CANManager/AP_CANManager_config.h | 5 ++ libraries/AP_CANManager/LogStructure.h | 79 +++++++++++++++++++ 5 files changed, 187 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_CANManager/LogStructure.h diff --git a/libraries/AP_CANManager/AP_CANIfaceParams.cpp b/libraries/AP_CANManager/AP_CANIfaceParams.cpp index b088f392c7..4a91755388 100644 --- a/libraries/AP_CANManager/AP_CANIfaceParams.cpp +++ b/libraries/AP_CANManager/AP_CANIfaceParams.cpp @@ -43,6 +43,14 @@ const AP_Param::GroupInfo AP_CANManager::CANIface_Params::var_info[] = { // @User: Advanced AP_GROUPINFO("FDBITRATE", 3, AP_CANManager::CANIface_Params, _fdbitrate, HAL_CANFD_SUPPORTED), #endif + + // @Param: OPTIONS + // @DisplayName: CAN per-interface options + // @Description: CAN per-interface options + // @Bitmask: 0:LogAllFrames + // @User: Advanced + AP_GROUPINFO("OPTIONS", 4, AP_CANManager::CANIface_Params, _options, 0), + // Index 3 occupied by Param: DEBUG AP_GROUPEND }; diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index ebf574df99..5a1caaf9dd 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -41,6 +41,15 @@ #include #include +#include + +/* + avoid a recursion issue with config defines + */ +#if AP_CAN_LOGGING_ENABLED && !HAL_LOGGING_ENABLED +#undef AP_CAN_LOGGING_ENABLED +#define AP_CAN_LOGGING_ENABLED 0 +#endif #define LOG_TAG "CANMGR" #define LOG_BUFFER_SIZE 1024 @@ -260,6 +269,10 @@ void AP_CANManager::init() _drivers[drv_num]->init(drv_num, enable_filter); } + +#if AP_CAN_LOGGING_ENABLED + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_CANManager::check_logging_enable, void)); +#endif } #else void AP_CANManager::init() @@ -509,6 +522,7 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) 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); @@ -523,6 +537,7 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) frame_buffer->push(frame); break; } +#endif } process_frame_buffer(); } @@ -684,12 +699,15 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram } } 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 { + } 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)); @@ -698,6 +716,61 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram } #endif // HAL_GCS_ENABLED +#if AP_CAN_LOGGING_ENABLED +/* + handler for CAN frames for frame logging + */ +void AP_CANManager::can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame) +{ +#if HAL_CANFD_SUPPORTED + if (frame.canfd) { + struct log_CAFD pkt { + LOG_PACKET_HEADER_INIT(LOG_CAFD_MSG), + time_us : AP_HAL::micros64(), + bus : bus, + id : frame.id, + dlc : frame.dlc + }; + memcpy(pkt.data, frame.data, frame.dlcToDataLength(frame.dlc)); + AP::logger().WriteBlock(&pkt, sizeof(pkt)); + return; + } +#endif + struct log_CANF pkt { + LOG_PACKET_HEADER_INIT(LOG_CANF_MSG), + time_us : AP_HAL::micros64(), + bus : bus, + id : frame.id, + dlc : frame.dlc + }; + memcpy(pkt.data, frame.data, frame.dlc); + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} + +/* + see if we need to enable/disable the CAN logging callback + */ +void AP_CANManager::check_logging_enable(void) +{ + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + const bool enabled = _interfaces[i].option_is_set(CANIface_Params::Options::LOG_ALL_FRAMES); + uint8_t &logging_id = _interfaces[i].logging_id; + auto *can = hal.can[i]; + if (can == nullptr) { + continue; + } + if (enabled && logging_id == 0) { + can->register_frame_callback( + FUNCTOR_BIND_MEMBER(&AP_CANManager::can_logging_callback, void, uint8_t, const AP_HAL::CANFrame &), + logging_id); + } else if (!enabled && logging_id != 0) { + can->unregister_frame_callback(logging_id); + } + } +} + +#endif // AP_CAN_LOGGING_ENABLED + AP_CANManager& AP::can() { return *AP_CANManager::get_singleton(); diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h index f820d83174..561c9c4680 100644 --- a/libraries/AP_CANManager/AP_CANManager.h +++ b/libraries/AP_CANManager/AP_CANManager.h @@ -126,10 +126,23 @@ private: static const struct AP_Param::GroupInfo var_info[]; + enum class Options : uint32_t { + LOG_ALL_FRAMES = (1U<<0), + }; + + bool option_is_set(Options option) const { + return (_options & uint32_t(option)) != 0; + } + private: AP_Int8 _driver_number; AP_Int32 _bitrate; AP_Int32 _fdbitrate; + AP_Int32 _options; + +#if AP_CAN_LOGGING_ENABLED && HAL_LOGGING_ENABLED + uint8_t logging_id; +#endif }; //Parameter Interface for CANDrivers @@ -198,6 +211,14 @@ private: void process_frame_buffer(void); #endif // HAL_GCS_ENABLED + +#if AP_CAN_LOGGING_ENABLED && HAL_LOGGING_ENABLED + /* + handler for CAN frames for logging + */ + void can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame); + void check_logging_enable(void); +#endif }; namespace AP diff --git a/libraries/AP_CANManager/AP_CANManager_config.h b/libraries/AP_CANManager/AP_CANManager_config.h index c8b9f8af91..383abe0041 100644 --- a/libraries/AP_CANManager/AP_CANManager_config.h +++ b/libraries/AP_CANManager/AP_CANManager_config.h @@ -5,3 +5,8 @@ #ifndef AP_CAN_SLCAN_ENABLED #define AP_CAN_SLCAN_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS #endif + +#ifndef AP_CAN_LOGGING_ENABLED +#define AP_CAN_LOGGING_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS +#endif + diff --git a/libraries/AP_CANManager/LogStructure.h b/libraries/AP_CANManager/LogStructure.h new file mode 100644 index 0000000000..3f2e2c33cc --- /dev/null +++ b/libraries/AP_CANManager/LogStructure.h @@ -0,0 +1,79 @@ +#pragma once +/* + log structures for AP_CANManager + */ + +#include +#include "AP_CANManager_config.h" + +#define LOG_IDS_FROM_CANMANAGER \ + LOG_CANF_MSG, \ + LOG_CAFD_MSG + +// @LoggerMessage: CANF +// @Description: CAN Frame +// @Field: TimeUS: Time since system startup +// @Field: Bus: bus number +// @Field: Id: frame identifier +// @Field: DLC: data length code +// @Field: B0: byte 0 +// @Field: B1: byte 1 +// @Field: B2: byte 2 +// @Field: B3: byte 3 +// @Field: B4: byte 4 +// @Field: B5: byte 5 +// @Field: B6: byte 6 +// @Field: B7: byte 7 +struct PACKED log_CANF { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t bus; + uint32_t id; + uint8_t dlc; + uint8_t data[8]; +}; + +// @LoggerMessage: CAFD +// @Description: CANFD Frame +// @Field: TimeUS: Time since system startup +// @Field: Bus: bus number +// @Field: Id: frame identifier +// @Field: DLC: data length code +// @Field: D0: data 0 +// @Field: D1: data 1 +// @Field: D2: data 2 +// @Field: D3: data 3 +// @Field: D4: data 4 +// @Field: D5: data 5 +// @Field: D6: data 6 +// @Field: D7: data 7 +struct PACKED log_CAFD { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t bus; + uint32_t id; + uint8_t dlc; + uint64_t data[8]; +}; + +#if !AP_CAN_LOGGING_ENABLED +#define LOG_STRUCTURE_FROM_CANMANAGER +#else +#define LOG_STRUCTURE_FROM_CANMANAGER \ + { LOG_CANF_MSG, sizeof(log_CANF), \ + "CANF", \ + "Q" "B" "I" "B" "B" "B" "B" "B" "B" "B" "B" "B", \ + "TimeUS," "Bus," "Id," "DLC," "B0," "B1," "B2," "B3," "B4," "B5," "B6," "B7", \ + "s" "#" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \ + "F" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \ + false \ + }, \ + { LOG_CAFD_MSG, sizeof(log_CAFD), \ + "CAFD", \ + "Q" "B" "I" "B" "Q" "Q" "Q" "Q" "Q" "Q" "Q" "Q", \ + "TimeUS," "Bus," "Id," "DLC," "D0," "D1," "D2," "D3," "D4," "D5," "D6," "D7", \ + "s" "#" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \ + "F" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \ + false \ + }, +#endif // AP_CAN_LOGGING_ENABLED