From 801142707e273cd3b5b824abacae63ae30de164d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Oct 2024 15:34:43 +0900 Subject: [PATCH 1/4] DroneCAN: update DSDL --- modules/DroneCAN/DSDL | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneCAN/DSDL b/modules/DroneCAN/DSDL index 993be80a62ec9..0856687a5acb4 160000 --- a/modules/DroneCAN/DSDL +++ b/modules/DroneCAN/DSDL @@ -1 +1 @@ -Subproject commit 993be80a62ec957c01fb41115b83663959a49f46 +Subproject commit 0856687a5acb42b55d29bbad8f4fc9e19b4989fc From 96752653334bd700f2e4dad6e8b0893edb16752a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Oct 2024 10:44:17 +0900 Subject: [PATCH 2/4] AP_Scripting: added support for FlexDebug message allows lua to retrieve vendor specific data from CAN nodes --- libraries/AP_Scripting/docs/docs.lua | 10 +++++++ .../generator/description/bindings.desc | 3 ++ libraries/AP_Scripting/lua_bindings.cpp | 30 +++++++++++++++++++ libraries/AP_Scripting/lua_bindings.h | 1 + 4 files changed, 44 insertions(+) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 059d6ce83a9e8..3a34534316de8 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/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 diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 67c11a646019d..ae6ba0a48f426 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/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 diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index 1c7db4a2df10f..dfb0b0a1689c8 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/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 diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h index b0a7e2c36b77f..d064df4552df6 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/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); From 7aa207fc7bcac7a2fb8d7a76ad9a04a49732a996 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Oct 2024 10:44:56 +0900 Subject: [PATCH 3/4] AP_DroneCAN: support FlexDebug message --- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 58 ++++++++++++++++++++++++++- libraries/AP_DroneCAN/AP_DroneCAN.h | 20 +++++++++ 2 files changed, 77 insertions(+), 1 deletion(-) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 49aace0952631..21243e5d08c40 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/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 ×tamp_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 */ diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 7264fad9472fc..2d6e5ae2b7f78 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/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 relay_hardpoint{canard_iface}; #endif +#if AP_SCRIPTING_ENABLED + bool get_FlexDebug(uint8_t node_id, uint16_t msg_id, uint32_t ×tamp_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 node_info_server{canard_iface, node_info_req_cb}; uavcan_protocol_GetNodeInfoResponse node_info_rsp; +#if AP_SCRIPTING_ENABLED + Canard::ObjCallback FlexDebug_cb{this, &AP_DroneCAN::handle_FlexDebug}; + Canard::Subscriber 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 From 3eaa07ba2914afdb74011cb9dc0230d805a6c965 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 24 Oct 2024 17:03:44 +0900 Subject: [PATCH 4/4] AP_Scripting: added FlexDebug example --- libraries/AP_Scripting/examples/FlexDebug.lua | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 libraries/AP_Scripting/examples/FlexDebug.lua diff --git a/libraries/AP_Scripting/examples/FlexDebug.lua b/libraries/AP_Scripting/examples/FlexDebug.lua new file mode 100644 index 0000000000000..5b15906ae1d1d --- /dev/null +++ b/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("