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