CANManager: support CAN frame logging

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,13 +11,15 @@
 import threading
 from pymavlink import mavutil
 from dronecan.driver.common import CANFrame
+import struct
 
 
 # get command line arguments
 from argparse import ArgumentParser
 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("<QQQQQQQQ", m.D0, m.D1, m.D2, m.D3, m.D4, m.D5, m.D6, m.D7)
+        data = data[:dlc_to_datalength(m.DLC)]
+    else:
+        data = struct.pack("<BBBBBBBB", m.B0, m.B1, m.B2, m.B3, m.B4, m.B5, m.B6, m.B7)
+        data = data[:m.DLC]
+
     fid = m.Id
     is_extended = (fid & (1<<31)) != 0
-    driver.send(fid, data, extended=is_extended, canfd=False)
+    driver.send(fid, data, extended=is_extended, canfd=canfd)
     print(m)

Tools/scripts/build_options.py

@@ -434,7 +434,8 @@ def config_option(self):
     Feature('Networking', 'PPP', 'AP_NETWORKING_BACKEND_PPP', 'Enable PPP networking', 0, None),
     Feature('Networking', 'CAN MCAST', 'AP_NETWORKING_CAN_MCAST_ENABLED', 'Enable CAN multicast bridge', 0, None),
 
-    Feature('DroneCAN', 'DroneCAN', 'HAL_ENABLE_DRONECAN_DRIVERS', 'Enable DroneCAN support', 0, None),
+    Feature('CAN', 'DroneCAN', 'HAL_ENABLE_DRONECAN_DRIVERS', 'Enable DroneCAN support', 0, None),
+    Feature('CAN', 'CAN Logging', 'AP_CAN_LOGGING_ENABLED', 'Enable CAN logging support', 0, None),
 ]
 
 BUILD_OPTIONS.sort(key=lambda x: (x.category + x.label))

Tools/scripts/extract_features.py

@@ -285,6 +285,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('AP_SERIALMANAGER_REGISTER_ENABLED', r'AP_SerialManager::register_port'),
             ('AP_QUICKTUNE_ENABLED', r'AP_Quicktune::update'),
             ('AP_FILTER_ENABLED', r'AP_Filters::update'),
+            ('AP_CAN_LOGGING_ENABLED', r'AP_CANManager::can_logging_callback'),
         ]
 
     def progress(self, msg):

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
 };

libraries/AP_CANManager/AP_CANManager.cpp

@@ -41,6 +41,15 @@
 
 #include <AP_Common/ExpandingString.h>
 #include <AP_Common/sorting.h>
+#include <AP_Logger/AP_Logger.h>
+
+/*
+  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<uint8_t*>(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<uint8_t*>(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();

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

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
+

libraries/AP_CANManager/LogStructure.h

@@ -0,0 +1,79 @@
+#pragma once
+/*
+  log structures for AP_CANManager
+ */
+
+#include <AP_Logger/LogStructure.h>
+#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

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_;