diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index 1d19108bd1d43..7365fb1a65e7b 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/libraries/AP_Scripting/AP_Scripting.cpp @@ -22,6 +22,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 @@ -352,6 +353,9 @@ void AP_Scripting::thread(void) { } #endif // AP_NETWORKING_ENABLED + // clear DroneCAN_Handle instances + DroneCAN_Handle::destroy_all(); + #if AP_SCRIPTING_SERIALDEVICE_ENABLED // clear data in serial buffers that hasn't been transmitted _serialdevice.clear(); diff --git a/libraries/AP_Scripting/AP_Scripting_helpers.cpp b/libraries/AP_Scripting/AP_Scripting_helpers.cpp index 380ac3ef73ca0..92d017561abec 100644 --- a/libraries/AP_Scripting/AP_Scripting_helpers.cpp +++ b/libraries/AP_Scripting/AP_Scripting_helpers.cpp @@ -195,4 +195,301 @@ 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; + transfer.canfd = h->canfd; + 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; + transfer.canfd = h->canfd; + transfer.timeout_ms = 10; + + auto *hsem = Canard::HandlerList::get_semaphore(h->dc->get_driver_index()); + if (hsem == nullptr) { + return 0; + } + WITH_SEMAPHORE(*hsem); + + 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) +{ + auto *hsem = Canard::HandlerList::get_semaphore(dc->get_driver_index()); + if (hsem == nullptr) { + return false; + } + WITH_SEMAPHORE(*hsem); + if (subscriber != nullptr) { + delete subscriber; + } + subscriber = NEW_NOTHROW Subscriber(*this, CanardTransferTypeBroadcast); + return subscriber != nullptr; +} + +/* + destroy all handles + */ +void DroneCAN_Handle::destroy_all(void) +{ + WITH_SEMAPHORE(sem); + while (handles != nullptr) { + handles->close(); + } +} + +/* + close a DroneCAN_Handle, removing from the list + */ +void DroneCAN_Handle::close(void) +{ + if (closed) { + return; + } + + auto *hsem = Canard::HandlerList::get_semaphore(dc->get_driver_index()); + if (hsem == nullptr) { + return; + } + WITH_SEMAPHORE(*hsem); + WITH_SEMAPHORE(sem); + + if (prev != nullptr) { + prev->next = next; + } + if (next != nullptr) { + next->prev = prev; + } + if (this == handles) { + handles = next; + } + closed = true; + + + if (subscriber != nullptr) { + delete subscriber; + 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; + WITH_SEMAPHORE(sem[index]); + link(); +} + +DroneCAN_Handle::Subscriber::~Subscriber(void) +{ + WITH_SEMAPHORE(sem[index]); + unlink(); + free(payload); + payload = nullptr; +} + +/* + handle an incoming subscribed message + */ +void DroneCAN_Handle::Subscriber::handle_message(const CanardRxTransfer& transfer) +{ + WITH_SEMAPHORE(handle->sem); + + if (payload != nullptr) { + // already have a message pending, discard this one + return; + } + + 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; + } + + uint8_t *buffer = (uint8_t *)malloc(transfer.payload_len); + if (buffer == nullptr) { + // discard messages if we can't allocate + return; + } + + node_id = transfer.source_node_id; +#if CANARD_ENABLE_CANFD + handle->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*)&buffer[ofs]); + ofs++; + bits_remaining -= nbits; + } + + // free old payload if any + free(payload); + + // fill in the payload + payload = buffer; + payload_length = transfer.payload_len; +} + +/* + check for an incoming message + */ +int DroneCAN_Handle::check_message(lua_State *L) +{ + /* + get mutex on the HandlerList for this driver then the + DroneCAN_Handle mutex. Note that order is important + */ + DroneCAN_Handle *h = check_DroneCAN_Handle(L, 1); + auto *hsem = Canard::HandlerList::get_semaphore(h->dc->get_driver_index()); + if (hsem == nullptr) { + return 0; + } + WITH_SEMAPHORE(*hsem); + WITH_SEMAPHORE(h->sem); + + auto *s = h->subscriber; + + if (s == nullptr || s->payload == nullptr) { + return 0; + } + + lua_pushlstring(L, (char *)s->payload, s->payload_length); + lua_pushinteger(L, s->node_id); + + // remove the payload to make room for the next message + free(s->payload); + s->payload = nullptr; + + if (s->trans_type == CanardTransferTypeResponse) { + // request reply removes the subscriber + delete h->subscriber; + h->subscriber = nullptr; + } + + return 2; +} + +// lua constructor for DroneCAN_Handle +int DroneCAN_Handle::new_handle(lua_State *L) +{ + lua_Number bus_index = luaL_checknumber(L, 1); + + auto *dc = AP_DroneCAN::get_dronecan(bus_index); + if (dc == nullptr) { + return luaL_error(L, "Invalid DroneCAN bus: %d", int(bus_index)); + } + + // 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; + + { + // add to the linked list + WITH_SEMAPHORE(h->sem); + h->next = h->handles; + if (h->handles != nullptr) { + h->handles->prev = h; + } + h->handles = h; + } + + + luaL_getmetatable(L, "DroneCAN_Handle"); + lua_setmetatable(L, -2); + + return 1; +} + +HAL_Semaphore DroneCAN_Handle::sem; +DroneCAN_Handle *DroneCAN_Handle::handles; + +#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 cb4dd23a22b10..ef3da1de33cee 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); @@ -28,3 +29,51 @@ class Parameter AP_Param *vp; }; +#if HAL_ENABLE_DRONECAN_DRIVERS +/* + 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); + void close(void); + static int request(lua_State *L); + static int check_message(lua_State *L); + bool subscribe(void); + static void destroy_all(void); + + uint64_t signature; + uint16_t data_type; + uint8_t transfer_id; + bool canfd; + +private: + // linked list to support for destroying all handles on scripting stop + static HAL_Semaphore sem; + static DroneCAN_Handle *handles; + DroneCAN_Handle *next, *prev; + + AP_DroneCAN *dc; + bool closed; + + // 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; + uint8_t *payload; + uint16_t payload_length; + CanardTransferType trans_type; + + private: + void 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 c4ba15485c874..b7764afffdcdb 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -4101,3 +4101,52 @@ function AP_Servo_Telem_Data_ud:measured_position() end -- get commanded position ---@return number|nil -- comanded position in degrees or nil if not available function AP_Servo_Telem_Data_ud:command_position() 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 +-- use close() method when finished +---@param bus_index number -- DroneCAN bus index, 0 for first bus +---@return DroneCAN_Handle_ud +function DroneCAN_Handle(bus_index) end + +-- return true if last message was used CANFD +---@return boolean +function DroneCAN_Handle_ud:canfd() end + +-- control if next sent message on this handle should be sent as CANFD +---@param value boolean +function DroneCAN_Handle_ud:canfd(value) end + +-- set the 64 bit DroneCAN signature for the message +---@param value uint64_t_ud +function DroneCAN_Handle_ud:signature(value) end + +-- set the DroneCAN data type of the message +---@param value number +function DroneCAN_Handle_ud:data_type(value) end + +-- subscribe to the current signature and data_type +---@return boolean +function DroneCAN_Handle_ud:subscribe() end + +-- close the handle +function DroneCAN_Handle_ud:close() end + +-- check if a new message has arrived for a request or subscription +---@return string payload -- payload of the message +---@return nodeid number -- node ID the message came from +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/examples/DroneCAN_test.lua b/libraries/AP_Scripting/examples/DroneCAN_test.lua new file mode 100644 index 0000000000000..512aeb9a3791c --- /dev/null +++ b/libraries/AP_Scripting/examples/DroneCAN_test.lua @@ -0,0 +1,243 @@ +--[[ + example of creating and sending DroneCAN messages +--]] + +local MAGNETICFIELDSTRENGTHHIRES_ID = 1043 +local MAGNETICFIELDSTRENGTHHIRES_SIGNATURE = uint64_t(0x3053EBE3, 0xD750286F) + +local RAWAIRDATA_ID = 1027 +local RAWAIRDATA_SIGNATURE = uint64_t(0xC77DF38B, 0xA122F5DA) + +local PARAM_GETSET_ID = 11 +local PARAM_GETSET_SIGNATURE = uint64_t(0xA7B622F9, 0x39D1A4D5) + +local NODESTATUS_ID = 341 +local NODESTATUS_SIGNATURE = uint64_t(0x0F0868D0, 0xC1A7C6F1) + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +-- a handle we will use for broadcasting +local dc_handle = DroneCAN_Handle(0) + +-- a handle for fetching parameters +local param_handle = DroneCAN_Handle(0) +param_handle:data_type(PARAM_GETSET_ID) +param_handle:signature(PARAM_GETSET_SIGNATURE) + +--[[ + setup subscription to NodeStatus +--]] +local nodestatus_handle = DroneCAN_Handle(0) +nodestatus_handle:data_type(NODESTATUS_ID) +nodestatus_handle:signature(NODESTATUS_SIGNATURE) +nodestatus_handle:subscribe() + +--[[ + setup subscription to raw air data +--]] +local airspeed_handle = DroneCAN_Handle(0) +airspeed_handle:data_type(RAWAIRDATA_ID) +airspeed_handle:signature(RAWAIRDATA_SIGNATURE) +airspeed_handle:subscribe() + +-- table of all nodes +local node_status = {} + + +--[[ + send highres mag using a global handle +--]] +local function send_mag_highres() + dc_handle:signature(MAGNETICFIELDSTRENGTHHIRES_SIGNATURE) + dc_handle:data_type(MAGNETICFIELDSTRENGTHHIRES_ID) + local payload = string.pack("Bfff", 7, 1, 2, 3) + dc_handle:broadcast(payload) + gcs:send_text(MAV_SEVERITY.INFO, "mag highres broadcast done") +end + +--[[ + send highres mag using a handle that will be closed after being used +--]] +local function send_mag_highres2() + local h = DroneCAN_Handle(0) + h:signature(MAGNETICFIELDSTRENGTHHIRES_SIGNATURE) + h:data_type(MAGNETICFIELDSTRENGTHHIRES_ID) + local payload = string.pack("Bfff", 8, 10, 11, 12) + h:broadcast(payload) + h:close() + gcs:send_text(MAV_SEVERITY.INFO, "mag highres broadcast2 done") +end + +--[[ + unpack a float16 into a floating point number +--]] +local function unpackFloat16(v16) + -- Extract the sign (bit 15), exponent (bits 10–14) and fraction (bits 0–9) + local sign = (v16 >> 15) & 0x1 + local exponent = (v16 >> 10) & 0x1F + local fraction = v16 & 0x3FF + + local value + if exponent == 0 then + if fraction == 0 then + -- Zero (positive or negative) + value = 0.0 + else + -- Subnormal numbers (exponent = -14, no implicit leading 1) + value = (fraction / 1024.0) * 2.0^-14 + end + elseif exponent == 0x1F then + if fraction == 0 then + -- Infinity (positive or negative) + value = math.huge + else + -- NaN (Not a Number) + value = 0/0 + end + else + -- Normalized numbers: implicit 1 before the fraction and exponent bias of 15. + value = (1 + fraction / 1024.0) * 2.0^(exponent - 15) + end + + -- Apply the sign bit + if sign == 1 then + value = -value + end + + return value +end + +--[[ + check for incoming airspeed broadcast messages +--]] +local function check_airspeed() + local payload, nodeid = airspeed_handle:check_message() + if payload then + return + end + local flags, static_pressure, differential_pressure, static_pressure_sensor_temperature, + differential_pressure_sensor_temperature, static_air_temperature, pitot_temperature = string.unpack("BffHHHH", payload) + if flags then + local temp_C = unpackFloat16(static_air_temperature) - 273.15; + gcs:send_text(MAV_SEVERITY.INFO, string.format("Rawairdata(%u): %f %.2fC", + nodeid, differential_pressure, temp_C)) + end +end + +--[[ + parse a parameter GetSet NumericValue +--]] +local function parse_param_NumericValue(payload, byte_offset) + local vtype = string.unpack("B", payload, byte_offset) + if vtype == 0 then + return nil, byte_offset+1 + elseif vtype == 1 then + -- integer (treat as 32 bit for now, actually 64 bit) + return string.unpack("i", payload, byte_offset+1), byte_offset+9 + elseif vtype == 2 then + -- float32 + return string.unpack("f", payload, byte_offset+1), byte_offset+5 + else + return nil + end +end + +--[[ + parse a parameter GetSet Value +--]] +local function parse_param_Value(payload, byte_offset) + local vtype = string.unpack("B", payload, byte_offset) + if vtype == 0 then + return nil, byte_offset+1 + elseif vtype == 1 then + -- integer (treat as 32 bit for now, actually 64 bit) + return string.unpack("i", payload, byte_offset+1), byte_offset+9 + elseif vtype == 2 then + -- float32 + return string.unpack("f", payload, byte_offset+1), byte_offset+5 + elseif vtype == 3 then + -- bool + local v = string.unpack("B", payload, byte_offset+1), byte_offset+2 + return v == 1 + elseif vtype == 4 then + -- string + local slen = string.unpack("B", payload, byte_offset+1) + return string.sub(payload, byte_offset+2, slen+2), byte_offset+2+slen + else + return nil + end +end + + +--[[ + parse a parameter GetSet reply +--]] +local function parse_param_reply(payload) + local byte_offset = 1 + local value, byte_offset = parse_param_Value(payload, byte_offset) + local default_value, byte_offset = parse_param_Value(payload, byte_offset) + local max_value, byte_offset = parse_param_NumericValue(payload, byte_offset) + local min_value, byte_offset = parse_param_NumericValue(payload, byte_offset) + local name = string.sub(payload, byte_offset, #payload) + return name, value, default_value, min_value, max_value +end + +local next_param_index = 0 + +--[[ + encode a 16 bit number as a DroneCAN int13 +--]] +local function encode_int13(v) + return (v & 0xFF) | (v&0xFF00)<<3 +end + +local function fetch_param() + local payload, nodeid = param_handle:check_message() + if payload then + local pname, pvalue = parse_param_reply(payload) + if not pname or not pvalue then + gcs:send_text(MAV_SEVERITY.INFO, string.format("param restart loop %u", next_param_index)) + next_param_index = 0 + else + gcs:send_text(MAV_SEVERITY.INFO, string.format("got param reply from %u idx=%u '%s' : %f", nodeid, next_param_index, pname, pvalue)) + next_param_index = next_param_index + 1 + end + end + param_handle:request(125, string.pack("H",encode_int13(next_param_index))) +end + +--[[ + check for new NodeStatus messages +--]] +local function check_node_status() + local payload, nodeid = nodestatus_handle:check_message() + if not payload then + return + end + local uptime_sec, bits, vssc = string.unpack("IBH", payload) + local health = bits&3 + local mode = (bits>>2)&7 + local sub_mode = (bits>>5)&7 + if not node_status[nodeid] then + gcs:send_text(MAV_SEVERITY.INFO, string.format("Found node %u", nodeid)) + end + node_status[nodeid] = { uptime_sec=uptime_sec, health=health, mode=mode, sub_mode=sub_mode } +end + +local last_low_rate_ms = uint32_t(0) + +local function update() + local now = millis() + if now - last_low_rate_ms >= 1000 then + last_low_rate_ms = now + send_mag_highres() + send_mag_highres2() + check_airspeed() + end + check_node_status() + fetch_param() + + return update, 10 +end + +return update, 1000 diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 856097a9be470..0cfd03994bdaf 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -1070,3 +1070,14 @@ userdata AP_Servo_Telem::TelemetryData field motor_temperature_cdeg int16_t'skip userdata AP_Servo_Telem::TelemetryData field pcb_temperature_cdeg int16_t'skip_check read valid_mask present_types AP_Servo_Telem::TelemetryData::Types::PCB_TEMP userdata AP_Servo_Telem::TelemetryData field status_flags uint8_t'skip_check read valid_mask present_types AP_Servo_Telem::TelemetryData::Types::STATUS userdata AP_Servo_Telem::TelemetryData field last_update_ms uint32_t'skip_check read + +userdata DroneCAN_Handle depends HAL_ENABLE_DRONECAN_DRIVERS +userdata DroneCAN_Handle creation DroneCAN_Handle::new_handle 1 +userdata DroneCAN_Handle method close void +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 2 +userdata DroneCAN_Handle field data_type uint16_t'skip_check write +userdata DroneCAN_Handle field signature uint64_t write +userdata DroneCAN_Handle field canfd boolean read write diff --git a/modules/DroneCAN/libcanard b/modules/DroneCAN/libcanard index 6f74bc6765688..1715920d88323 160000 --- a/modules/DroneCAN/libcanard +++ b/modules/DroneCAN/libcanard @@ -1 +1 @@ -Subproject commit 6f74bc67656882a4ee51966c7c0022d04fa1a3fb +Subproject commit 1715920d88323494d7fb919880650ecacfa58b17