DroneCAN: added FlexDebug support

libraries/AP_DroneCAN/AP_DroneCAN.cpp

@@ -128,7 +128,7 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = {
     // @Param: OPTION
     // @DisplayName: DroneCAN options
     // @Description: Option flags
-    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,8:EnableStats
+    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,8:EnableStats,9:EnableFlexDebug
     // @User: Advanced
     AP_GROUPINFO("OPTION", 5, AP_DroneCAN, _options, 0),
 
@@ -1503,6 +1503,62 @@ bool AP_DroneCAN::is_esc_data_index_valid(const uint8_t index) {
     return true;
 }
 
+#if AP_SCRIPTING_ENABLED
+/*
+  handle FlexDebug message, holding a copy locally for a lua script to access
+ */
+void AP_DroneCAN::handle_FlexDebug(const CanardRxTransfer& transfer, const dronecan_protocol_FlexDebug &msg)
+{
+    if (!option_is_set(Options::ENABLE_FLEX_DEBUG)) {
+        return;
+    }
+
+    // find an existing element in the list
+    const uint8_t source_node = transfer.source_node_id;
+    for (auto *p = flexDebug_list; p != nullptr; p = p->next) {
+        if (p->node_id == source_node && p->msg.id == msg.id) {
+            p->msg = msg;
+            p->timestamp_us = uint32_t(transfer.timestamp_usec);
+            return;
+        }
+    }
+
+    // new message ID, add to the list. Note that this gets called
+    // only from one thread, so no lock needed
+    auto *p = NEW_NOTHROW FlexDebug;
+    if (p == nullptr) {
+        return;
+    }
+    p->node_id = source_node;
+    p->msg = msg;
+    p->timestamp_us = uint32_t(transfer.timestamp_usec);
+    p->next = flexDebug_list;
+
+    // link into the list
+    flexDebug_list = p;
+}
+
+/*
+  get the last FlexDebug message from a node
+ */
+bool AP_DroneCAN::get_FlexDebug(uint8_t node_id, uint16_t msg_id, uint32_t &timestamp_us, dronecan_protocol_FlexDebug &msg) const
+{
+    for (const auto *p = flexDebug_list; p != nullptr; p = p->next) {
+        if (p->node_id == node_id && p->msg.id == msg_id) {
+            if (timestamp_us == p->timestamp_us) {
+                // stale message
+                return false;
+            }
+            timestamp_us = p->timestamp_us;
+            msg = p->msg;
+            return true;
+        }
+    }
+    return false;
+}
+
+#endif // AP_SCRIPTING_ENABLED
+
 /*
   handle LogMessage debug
  */

libraries/AP_DroneCAN/AP_DroneCAN.h

@@ -148,6 +148,7 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
         USE_HIMARK_SERVO          = (1U<<6),
         USE_HOBBYWING_ESC         = (1U<<7),
         ENABLE_STATS              = (1U<<8),
+        ENABLE_FLEX_DEBUG         = (1U<<9),
     };
 
     // check if a option is set
@@ -176,6 +177,10 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     Canard::Publisher<uavcan_equipment_hardpoint_Command> relay_hardpoint{canard_iface};
 #endif
 
+#if AP_SCRIPTING_ENABLED
+    bool get_FlexDebug(uint8_t node_id, uint16_t msg_id, uint32_t &timestamp_us, dronecan_protocol_FlexDebug &msg) const;
+#endif
+
 private:
     void loop(void);
 
@@ -363,6 +368,11 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     Canard::Server<uavcan_protocol_GetNodeInfoRequest> node_info_server{canard_iface, node_info_req_cb};
     uavcan_protocol_GetNodeInfoResponse node_info_rsp;
 
+#if AP_SCRIPTING_ENABLED
+    Canard::ObjCallback<AP_DroneCAN, dronecan_protocol_FlexDebug> FlexDebug_cb{this, &AP_DroneCAN::handle_FlexDebug};
+    Canard::Subscriber<dronecan_protocol_FlexDebug> FlexDebug_listener{FlexDebug_cb, _driver_index};
+#endif
+
 #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
     /*
       Hobbywing ESC support. Note that we need additional meta-data as
@@ -409,6 +419,16 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp);
     void handle_param_save_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_ExecuteOpcodeResponse& rsp);
     void handle_node_info_request(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoRequest& req);
+
+#if AP_SCRIPTING_ENABLED
+    void handle_FlexDebug(const CanardRxTransfer& transfer, const dronecan_protocol_FlexDebug& msg);
+    struct FlexDebug {
+        struct FlexDebug *next;
+        uint32_t timestamp_us;
+        uint8_t node_id;
+        dronecan_protocol_FlexDebug msg;
+    } *flexDebug_list;
+#endif
 };
 
 #endif // #if HAL_ENABLE_DRONECAN_DRIVERS

libraries/AP_Scripting/docs/docs.lua

@@ -414,6 +414,16 @@ function CAN:get_device(buffer_len) end
 ---@return ScriptingCANBuffer_ud|nil
 function CAN:get_device2(buffer_len) end
 
+
+-- get latest FlexDebug message from a CAN node
+---@param bus number -- CAN bus number, 0 for first bus, 1 for 2nd
+---@param node number -- CAN node
+---@param id number -- FlexDebug message ID
+---@param last_us uint32_t_ud|integer|number -- timestamp of last received message, new message will be returned if timestamp is different
+---@return uint32_t_ud|nil -- timestamp of message (first frame arrival time)
+---@return string|nil -- up to 255 byte buffer
+function DroneCAN_get_FlexDebug(bus,node,id,last_us) end
+
 -- Auto generated binding
 
 -- desc

libraries/AP_Scripting/examples/FlexDebug.lua

@@ -0,0 +1,50 @@
+--[[
+   ArduPilot lua script to log debug messages from AM32 DroneCAN
+   ESCs on the flight controller
+
+   To install set SCR_ENABLE=1 and put this script in APM/SCRIPTS/ on
+   the microSD of the flight controller then restart the flight
+   controller
+--]]
+
+-- assume ESCs are nodes 30, 31, 32 and 33
+local ESC_BASE = 30
+
+local AM32_DEBUG = 100
+
+local last_tstamp = {}
+local ts_zero = uint32_t(0)
+local reported_version_error = false
+
+function log_AM32()
+   for i = 0, 3 do
+      local last_ts = last_tstamp[i] or ts_zero
+      tstamp_us, msg = DroneCAN_get_FlexDebug(0, ESC_BASE+i, AM32_DEBUG, last_ts)
+      if tstamp_us and msg then
+         version, commutation_interval, num_commands, num_input, rx_errors, rxframe_error, rx_ecode, auto_advance_level = string.unpack("<BiHHHHiB", msg)
+         if not version or version ~= 1 then
+            if not reported_version_error then
+               reported_version_error = true
+               gcs:send_text(0, string.format("AM32 debug version error %u", version))
+            end
+            return
+         end
+         logger:write('AMD1','Node,CI,NC,NI,RXerr,FrE,RXec,AAL','BiHHHHiB','#-------','--------',i,commutation_interval, num_commands, num_input, rx_errors, rxframe_error, rx_ecode, auto_advance_level)
+         if rx_ecode then
+            gcs:send_named_float(string.format('AM32_EC_%u',i), rx_ecode)
+         end
+         last_tstamp[i] = tstamp_us
+      end
+   end
+end
+
+function update()
+   log_AM32()
+   return update, 5
+end
+
+gcs:send_text(0, "Loaded AM32_debug_log")
+
+return update, 5
+
+

libraries/AP_Scripting/generator/description/bindings.desc

@@ -698,6 +698,9 @@ ap_object ScriptingCANBuffer method write_frame boolean AP_HAL::CANFrame uint32_
 ap_object ScriptingCANBuffer method read_frame boolean AP_HAL::CANFrame'Null
 ap_object ScriptingCANBuffer method add_filter boolean uint32_t'skip_check uint32_t'skip_check
 
+include AP_DroneCAN/AP_DroneCAN.h
+global manual DroneCAN_get_FlexDebug lua_DroneCAN_get_FlexDebug 4 2 depends (HAL_ENABLE_DRONECAN_DRIVERS==1)
+
 include ../Tools/AP_Periph/AP_Periph.h depends defined(HAL_BUILD_AP_PERIPH)
 singleton AP_Periph_FW depends defined(HAL_BUILD_AP_PERIPH)
 singleton AP_Periph_FW rename periph

libraries/AP_Scripting/lua_bindings.cpp

@@ -1237,4 +1237,34 @@ int lua_GCS_command_int(lua_State *L)
 }
 #endif
 
+#if HAL_ENABLE_DRONECAN_DRIVERS
+/*
+  get FlexDebug from a DroneCAN node
+ */
+int lua_DroneCAN_get_FlexDebug(lua_State *L)
+{
+    binding_argcheck(L, 4);
+
+    const uint8_t bus = get_uint8_t(L, 1);
+    const uint8_t node_id = get_uint8_t(L, 2);
+    const uint16_t msg_id = get_uint16_t(L, 3);
+    uint32_t tstamp_us = get_uint32(L, 4, 0, UINT32_MAX);
+
+    const auto *dc = AP_DroneCAN::get_dronecan(bus);
+    if (dc == nullptr) {
+        return 0;
+    }
+    dronecan_protocol_FlexDebug msg;
+
+    if (!dc->get_FlexDebug(node_id, msg_id, tstamp_us, msg)) {
+        return 0;
+    }
+
+    *new_uint32_t(L) = tstamp_us;
+    lua_pushlstring(L, (const char *)msg.u8.data, msg.u8.len);
+
+    return 2;
+}
+#endif // HAL_ENABLE_DRONECAN_DRIVERS
+
 #endif  // AP_SCRIPTING_ENABLED

libraries/AP_Scripting/lua_bindings.h

@@ -34,3 +34,4 @@ int lua_mavlink_block_command(lua_State *L);
 int lua_print(lua_State *L);
 int lua_range_finder_handle_script_msg(lua_State *L);
 int lua_GCS_command_int(lua_State *L);
+int lua_DroneCAN_get_FlexDebug(lua_State *L);