diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index ab88cb57555..6c780fa2fa9 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/libraries/AP_Scripting/AP_Scripting.cpp @@ -24,6 +24,7 @@ #include #include "lua_scripts.h" +#include "AP_Scripting_helpers.h" // ensure that we have a set of stack sizes, and enforce constraints around it // except for the minimum size, these are allowed to be defined by the build system diff --git a/libraries/AP_Scripting/AP_Scripting_helpers.cpp b/libraries/AP_Scripting/AP_Scripting_helpers.cpp index 380ac3ef73c..f530846e4d5 100644 --- a/libraries/AP_Scripting/AP_Scripting_helpers.cpp +++ b/libraries/AP_Scripting/AP_Scripting_helpers.cpp @@ -195,4 +195,275 @@ bool Parameter::set_default(float value) return false; } +#if HAL_ENABLE_DRONECAN_DRIVERS + +#define IFACE_ALL uint8_t(((1U<<(HAL_NUM_CAN_IFACES))-1U)) + +/************************************************ + DroneCAN broadcast and request message handling + ************************************************/ + +/* + broadcast a message, takes a lua string as payload + */ +int DroneCAN_Handle::broadcast(lua_State *L) +{ + binding_argcheck(L, 2); + + DroneCAN_Handle *h = check_DroneCAN_Handle(L, 1); + + size_t data_length; + const void *data = lua_tolstring(L, 2, &data_length); + + auto &iface = h->dc->get_canard_iface(); + Canard::Transfer transfer; + + transfer.transfer_type = CanardTransferTypeBroadcast; + transfer.data_type_signature = h->signature; + transfer.data_type_id = h->data_type; + transfer.inout_transfer_id = &h->transfer_id; + transfer.priority = 0; + transfer.payload = data; + transfer.payload_len = data_length; + transfer.iface_mask = IFACE_ALL; +#if CANARD_ENABLE_CANFD + transfer.canfd = h->canfd; +#endif + transfer.timeout_ms = 10; + + bool ok = iface.broadcast(transfer); + lua_pushboolean(L, ok); + + return 1; +} + +/* + send a request message that expects a reply + */ +int DroneCAN_Handle::request(lua_State *L) +{ + binding_argcheck(L, 3); + + DroneCAN_Handle *h = check_DroneCAN_Handle(L, 1); + uint8_t target_node = luaL_checknumber(L, 2); + + size_t data_length; + const void *data = lua_tolstring(L, 3, &data_length); + + auto &iface = h->dc->get_canard_iface(); + + Canard::Transfer transfer; + + transfer.transfer_type = CanardTransferTypeRequest; + transfer.data_type_signature = h->signature; + transfer.data_type_id = h->data_type; + transfer.inout_transfer_id = &h->transfer_id; + transfer.priority = 0; + transfer.payload = data; + transfer.payload_len = data_length; + transfer.iface_mask = IFACE_ALL; +#if CANARD_ENABLE_CANFD + transfer.canfd = h->canfd; +#endif + transfer.timeout_ms = 10; + + WITH_SEMAPHORE(iface.get_sem_rx()); + + if (h->subscriber != nullptr) { + delete h->subscriber; + } + + h->subscriber = NEW_NOTHROW Subscriber(*h, CanardTransferTypeResponse); + bool ok = h->subscriber != nullptr; + + if (ok) { + h->subscriber->node_id = target_node; + ok &= iface.request(target_node, transfer); + } + + lua_pushboolean(L, ok); + + return 1; +} + +/* + subscribe to broadcast messages + */ +bool DroneCAN_Handle::subscribe(void) +{ + WITH_SEMAPHORE(dc->get_canard_iface().get_sem_rx()); + if (subscriber != nullptr) { + delete subscriber; + } + subscriber = NEW_NOTHROW Subscriber(*this, CanardTransferTypeBroadcast); + return subscriber != nullptr; +} + +DroneCAN_Handle::Subscriber::Subscriber(DroneCAN_Handle &_handle, CanardTransferType _transfer_type) : + Canard::HandlerList(_transfer_type, _handle.data_type, _handle.signature, _handle.dc->get_driver_index()) +{ + handle = &_handle; + trans_type = _transfer_type; + link(); +} + +DroneCAN_Handle::Subscriber::~Subscriber(void) +{ + unlink(); + Payload payload; + while (payloads.pop(payload)) { + free(payload.data); + } +} + +/* + handle an incoming subscribed message + */ +bool DroneCAN_Handle::Subscriber::handle_message(const CanardRxTransfer& transfer) +{ + if (transfer.transfer_type == CanardTransferTypeResponse && + (node_id != transfer.source_node_id || + ((transfer.transfer_id+1)&0xFF) != handle->transfer_id)) { + // not from right node, or not right transfer ID + return false; + } + + if (payloads.space() == 0) { + // already have the max number of messages pending, discard + // this one, but return true to say it was for us + return true; + } + Payload payload; + payload.data = (uint8_t *)malloc(transfer.payload_len); + if (payload.data == nullptr) { + // discard messages if we can't allocate + // but return true to say it was for us + return true; + } + + payload.length = transfer.payload_len; + payload.node_id = transfer.source_node_id; + payload.timestamp = transfer.timestamp_usec; +#if CANARD_ENABLE_CANFD + payload.canfd = transfer.canfd; +#endif + + uint32_t bits_remaining = transfer.payload_len * 8; + uint32_t ofs = 0; + + // decode in bytes, we don't have a canardDecodeBuffer function + while (bits_remaining > 0) { + uint8_t nbits = MIN(8U, bits_remaining); + canardDecodeScalar(&transfer, ofs*8, nbits, false, (void*)&payload.data[ofs]); + ofs++; + bits_remaining -= nbits; + } + + // push to queue + payloads.push(payload); + + return true; +} + +/* + check for an incoming message + */ +int DroneCAN_Handle::check_message(lua_State *L) +{ + /* + get the receive mutex for this driver + */ + DroneCAN_Handle *h = check_DroneCAN_Handle(L, 1); + + auto *s = h->subscriber; + + if (s == nullptr) { + return 0; + } + Subscriber::Payload payload; + /* + peek first to avoid a memory leak if lua_pushlstring() faults + */ + if (!s->payloads.peek(payload)) { + return 0; + } + + lua_pushlstring(L, (char *)payload.data, payload.length); + + s->payloads.pop(); + free(payload.data); + + lua_pushinteger(L, payload.node_id); + *new_uint64_t(L) = payload.timestamp; +#if CANARD_ENABLE_CANFD + lua_pushboolean(L, payload.canfd); +#else + lua_pushboolean(L, false); +#endif + + if (s->trans_type == CanardTransferTypeResponse) { + // request reply removes the subscriber + WITH_SEMAPHORE(h->dc->get_canard_iface().get_sem_rx()); + delete h->subscriber; + h->subscriber = nullptr; + } + + return 4; +} + +/* + garbage collect a handle + */ +int DroneCAN_Handle::__gc(lua_State *L) +{ + DroneCAN_Handle *h = check_DroneCAN_Handle(L, 1); + WITH_SEMAPHORE(h->dc->get_canard_iface().get_sem_rx()); + + if (h->subscriber != nullptr) { + delete h->subscriber; + h->subscriber = nullptr; + } + return 0; +} + +// lua constructor for DroneCAN_Handle +int DroneCAN_Handle::new_handle(lua_State *L) +{ + lua_Number driver_index = luaL_checknumber(L, 1); + const uint64_t sig = *check_uint64_t(L, 2); + const uint16_t dtype = luaL_checkinteger(L, 3); +#if CANARD_ENABLE_CANFD + const int args = lua_gettop(L); + bool send_canfd = false; + if (args > 3) { + send_canfd = (bool)lua_toboolean(L, 4); + } +#endif + + auto *dc = AP_DroneCAN::get_dronecan(driver_index); + if (dc == nullptr) { + return 0; + } + + // This chunk is the same as the auto generated constructor + void *ud = lua_newuserdata(L, sizeof(DroneCAN_Handle)); + new (ud) DroneCAN_Handle(); + + auto *h = static_cast(ud); + + h->dc = dc; + h->signature = sig; + h->data_type = dtype; +#if CANARD_ENABLE_CANFD + h->canfd = send_canfd; +#endif + + luaL_getmetatable(L, "DroneCAN_Handle"); + lua_setmetatable(L, -2); + + return 1; +} + +#endif // HAL_ENABLE_DRONECAN_DRIVERS + #endif // AP_SCRIPTING_ENABLED diff --git a/libraries/AP_Scripting/AP_Scripting_helpers.h b/libraries/AP_Scripting/AP_Scripting_helpers.h index 874cd86a177..79f9983b4b7 100644 --- a/libraries/AP_Scripting/AP_Scripting_helpers.h +++ b/libraries/AP_Scripting/AP_Scripting_helpers.h @@ -2,6 +2,7 @@ #include #include "lua/src/lua.hpp" +#include int lua_new_Parameter(lua_State *L); @@ -27,3 +28,60 @@ private: enum ap_var_type vtype; AP_Param *vp; }; + + +#if HAL_ENABLE_DRONECAN_DRIVERS + +#ifndef DRONECAN_HANDLE_MAX_PAYLOADS +#define DRONECAN_HANDLE_MAX_PAYLOADS 8 +#endif + +/* + access to sending DroneCAN broadcast messages and requests + */ +class DroneCAN_Handle { +public: + static int new_handle(lua_State *L); + static int broadcast(lua_State *L); + static int __gc(lua_State *L); + void close(void); + static int request(lua_State *L); + static int check_message(lua_State *L); + bool subscribe(void); + + uint64_t signature; + uint16_t data_type; + uint8_t transfer_id; + bool canfd; + +private: + AP_DroneCAN *dc; + + // class for subscribing to messages + class Subscriber : public Canard::HandlerList { + public: + Subscriber(DroneCAN_Handle &_handle, CanardTransferType transfer_type); + virtual ~Subscriber(void); + + uint8_t node_id; + struct Payload { + uint8_t *data; + uint16_t length; + uint8_t node_id; +#if CANARD_ENABLE_CANFD + bool canfd; +#endif + uint64_t timestamp; + }; + ObjectBuffer payloads{DRONECAN_HANDLE_MAX_PAYLOADS}; + CanardTransferType trans_type; + + private: + bool handle_message(const CanardRxTransfer& transfer) override; + DroneCAN_Handle *handle; + }; + + Subscriber *subscriber; +}; + +#endif // HAL_ENABLE_DRONECAN_DRIVERS diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index d33ded701df..d3aab71b200 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -4212,3 +4212,38 @@ function crsf:get_menu_event(events) end ---@param data string -- binary encoded response payload ---@return boolean -- true if the repsonse was successfully sent, false otherwise function crsf:send_write_response(data) end + +-- handle for DroneCAN message operations +---@class DroneCAN_Handle_ud +local DroneCAN_Handle_ud = {} + +-- create a DroneCAN_Handle, needed for all other DroneCAN message operations +---@param driver_index number -- DroneCAN driver index, 0 for first driver +---@param signature uint64_t_ud -- message signature +---@param data_type number -- message data type ID +---@param canfd? boolean -- send as CANFD +---@return DroneCAN_Handle_ud +function DroneCAN_Handle(driver_index, signature, data_type, canfd) end + +-- subscribe to the current signature and data_type +---@return boolean +function DroneCAN_Handle_ud:subscribe() end + +-- check if a new message has arrived for a request or subscription +---@return string payload -- payload of the message +---@return number nodeid -- node ID the message came from +---@return uint64_t_ud timestamp -- microseconds since 1/1/1970 +---@return boolean canfd -- true if message was CANFD +function DroneCAN_Handle_ud:check_message() end + +-- make a DroneCAN request +---@param target_node number -- node to send request to +---@param payload string -- payload for message +---@return boolean -- true if send succeeded +function DroneCAN_Handle_ud:request(target_node, payload) end + +-- send a DroneCAN broadcast +---@param payload string -- payload for message +---@return boolean -- true if send succeeded +function DroneCAN_Handle_ud:broadcast(payload) end + diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index c959ec91316..2793df7c25b 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -1103,4 +1103,11 @@ singleton SITL::SIM depends AP_SIM_ENABLED singleton SITL::SIM rename sim singleton SITL::SIM method set_pose boolean uint8_t'skip_check Location Quaternion Vector3f Vector3f +userdata DroneCAN_Handle depends HAL_ENABLE_DRONECAN_DRIVERS +userdata DroneCAN_Handle creation DroneCAN_Handle::new_handle 4 +userdata DroneCAN_Handle manual broadcast DroneCAN_Handle::broadcast 1 1 +userdata DroneCAN_Handle manual request DroneCAN_Handle::request 2 1 +userdata DroneCAN_Handle method subscribe boolean +userdata DroneCAN_Handle manual check_message DroneCAN_Handle::check_message 0 4 +userdata DroneCAN_Handle manual_operator __gc DroneCAN_Handle::__gc