diff --git a/Tools/scripts/CAN/CAN_playback.py b/Tools/scripts/CAN/CAN_playback.py index bdd4825f4fa221..b3b8cc0ecc66f8 100755 --- a/Tools/scripts/CAN/CAN_playback.py +++ b/Tools/scripts/CAN/CAN_playback.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 ''' -playback a set of CAN frames from libraries/AP_Scripting/examples/CAN_logger.lua onto a CAN bus + playback a set of CAN frames + capture frames either using libraries/AP_Scripting/examples/CAN_logger.lua + or the CAN_Pn_OPTIONS bit to enable CAN logging ''' import dronecan @@ -9,6 +11,7 @@ import threading from pymavlink import mavutil from dronecan.driver.common import CANFrame +import struct # get command line arguments @@ -16,6 +19,7 @@ parser = ArgumentParser(description='CAN playback') parser.add_argument("logfile", default=None, type=str, help="logfile") parser.add_argument("canport", default=None, type=str, help="CAN port") +parser.add_argument("--bus", default=0, type=int, help="CAN bus") args = parser.parse_args() @@ -28,8 +32,27 @@ tstart = time.time() first_tstamp = None +def dlc_to_datalength(dlc): + # Data Length Code 9 10 11 12 13 14 15 + # Number of data bytes 12 16 20 24 32 48 64 + if (dlc <= 8): + return dlc + elif (dlc == 9): + return 12 + elif (dlc == 10): + return 16 + elif (dlc == 11): + return 20 + elif (dlc == 12): + return 24 + elif (dlc == 13): + return 32 + elif (dlc == 14): + return 48 + return 64 + while True: - m = mlog.recv_match(type='CANF') + m = mlog.recv_match(type=['CANF','CAFD']) if m is None: print("Rewinding") @@ -38,15 +61,25 @@ first_tstamp = None continue + if getattr(m,'bus',0) != args.bus: + continue + if first_tstamp is None: first_tstamp = m.TimeUS dt = time.time() - tstart dt2 = (m.TimeUS - first_tstamp)*1.0e-6 if dt2 > dt: time.sleep(dt2 - dt) - data = [m.B0, m.B1, m.B2, m.B3, m.B4, m.B5, m.B6, m.B7] - data = data[:m.DLC] + + canfd = m.get_type() == 'CAFD' + if canfd: + data = struct.pack(" #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 f820d8317482de..561c9c468054a7 100644 --- a/libraries/AP_CANManager/AP_CANManager.h +++ b/libraries/AP_CANManager/AP_CANManager.h @@ -126,10 +126,23 @@ class AP_CANManager 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 @@ class AP_CANManager 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 c8b9f8af91cef7..383abe0041dc9f 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 00000000000000..3f2e2c33ccbeff --- /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 diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h index b92274a6a5d516..629432bd1fac11 100644 --- a/libraries/AP_HAL/CANIface.h +++ b/libraries/AP_HAL/CANIface.h @@ -273,8 +273,8 @@ class AP_HAL::CANIface #ifndef HAL_BOOTLOADER_BUILD HAL_Semaphore sem; #endif - // allow up to 2 callbacks per interface - FrameCb cb[2]; + // allow up to 3 callbacks per interface + FrameCb cb[3]; } callbacks; uint32_t bitrate_; diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 418b8de1c84920..3d388f9795f50f 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -135,6 +135,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include #include #include #include @@ -1159,6 +1160,7 @@ LOG_STRUCTURE_FROM_GPS \ { LOG_RSSI_MSG, sizeof(log_RSSI), \ "RSSI", "Qff", "TimeUS,RXRSSI,RXLQ", "s-%", "F--", true }, \ LOG_STRUCTURE_FROM_BARO \ +LOG_STRUCTURE_FROM_CANMANAGER \ LOG_STRUCTURE_FROM_PRECLAND \ { LOG_POWR_MSG, sizeof(log_POWR), \ "POWR","QffHHB","TimeUS,Vcc,VServo,Flags,AccFlags,Safety", "svv---", "F00---", true }, \ @@ -1266,6 +1268,7 @@ enum LogMessages : uint8_t { LOG_RCOUT_MSG, LOG_RSSI_MSG, LOG_IDS_FROM_BARO, + LOG_IDS_FROM_CANMANAGER, LOG_POWR_MSG, LOG_MCU_MSG, LOG_IDS_FROM_AHRS,