AP_UAVCAN_DNA: add CANH health logging message

Co-authored-by: Lokesh-Carbonix <[email protected]>

Author: 2 people

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -122,7 +122,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
     // @Param: OPTION
     // @DisplayName: UAVCAN options
     // @Description: Option flags
-    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd
+    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:LogNodeStatus
     // @User: Advanced
     AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0),
 

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -200,6 +200,7 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
         DNA_CLEAR_DATABASE        = (1U<<0),
         DNA_IGNORE_DUPLICATE_NODE = (1U<<1),
         CANFD_ENABLED             = (1U<<2),
+        LOG_NODE_STATUS           = (1U<<3),
     };
 
     // check if a option is set

libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp

@@ -484,6 +484,11 @@ void AP_UAVCAN_DNA_Server::handleNodeStatus(uint8_t node_id, const NodeStatusCb
         return;
     }
     WITH_SEMAPHORE(sem);
+
+    if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::LOG_NODE_STATUS)) {
+        log_NodeStatus(node_id, cb.msg->uptime_sec, cb.msg->health, cb.msg->mode);
+    }
+
     if (!isNodeIDVerified(node_id)) {
         //immediately begin verification of the node_id
         for (uint8_t i = 0; i < HAL_MAX_CAN_PROTOCOL_DRIVERS; i++) {
@@ -580,6 +585,29 @@ void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[],
     }
 }
 
+/*
+  optionally log NodeStatus packets
+ */
+void AP_UAVCAN_DNA_Server::log_NodeStatus(uint8_t node_id, uint32_t uptime_sec, uint8_t healthy, uint8_t mode)
+{
+    if (node_id > MAX_NODE_ID) {
+        return;
+    }
+
+    // @LoggerMessage: CANH
+    // @Description: CAN Health Status
+    // @Field: TimeUS: Time since system startup
+    // @Field: NodeID: Node ID
+    // @Field: Healthy: zero when node healthy
+    // @Field: UpTime: time since boot in seconds
+    AP::logger().WriteStreaming("CANH",
+                                "TimeUS," "NodeID," "Healthy," "UpTime", // labels
+                                "s"            "#"        "-"       "-", // units
+                                "F"            "-"        "-"       "-", // multipliers
+                                "Q"            "B"        "B"       "I", // types
+                                AP_HAL::micros64(), node_id, healthy, uptime_sec);
+}
+
 //Trampoline call for handleNodeInfo member call
 void trampoline_handleNodeInfo(const uavcan::ServiceCallResult<uavcan::protocol::GetNodeInfo>& resp)
 {

libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h

@@ -124,6 +124,9 @@ class AP_UAVCAN_DNA_Server
 
     //Run through the list of seen node ids for verification
     void verify_nodes(AP_UAVCAN *ap_uavcan);
+
+    // Log NodeInfo
+    void log_NodeStatus(uint8_t node_id, uint32_t uptime_sec, uint8_t healthy, uint8_t mode);
 };
 
 namespace AP