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

@@ -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 @@ 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

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