From 77b87edda3adca12fcf898f9e6faf9563431ab58 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Apr 2025 07:56:50 +1000 Subject: [PATCH 1/4] DroneCAN: update libcanard added get_semaphore() and allow for multiple handlers for an ID --- modules/DroneCAN/libcanard | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneCAN/libcanard b/modules/DroneCAN/libcanard index e4f3056c9f3c9b..5d7b725ce114d0 160000 --- a/modules/DroneCAN/libcanard +++ b/modules/DroneCAN/libcanard @@ -1 +1 @@ -Subproject commit e4f3056c9f3c9befd5fa8af8dffb92508367daff +Subproject commit 5d7b725ce114d079588cd9a6cabc73e333ea1cff From 80d8561b84b77232293b0fd50a062bdc53efad6b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 May 2025 08:44:56 +1000 Subject: [PATCH 2/4] AP_DroneCAN: added get_sem_rx() --- libraries/AP_DroneCAN/AP_Canard_iface.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.h b/libraries/AP_DroneCAN/AP_Canard_iface.h index 671722028dffc7..9da183e0b79826 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.h +++ b/libraries/AP_DroneCAN/AP_Canard_iface.h @@ -64,6 +64,10 @@ class CanardInterface : public Canard::Interface { void update_rx_protocol_stats(int16_t res); uint8_t get_node_id() const override { return canard.node_id; } + + // get reference to the semaphore that is held during message receive + HAL_Semaphore &get_sem_rx(void) { return _sem_rx; } + private: CanardInstance canard; AP_HAL::CANIface* ifaces[HAL_NUM_CAN_IFACES]; From 2e09428d99745e8251dab9731657b0a78d87be4c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 15 Feb 2025 19:42:07 +1100 Subject: [PATCH 3/4] AP_Scripting: support DroneCAN messaging in lua allow for sending broadcast messages, subscribing to broadcast messages and sending requests --- libraries/AP_Scripting/AP_Scripting.cpp | 1 + .../AP_Scripting/AP_Scripting_helpers.cpp | 271 ++++++++++++++++++ libraries/AP_Scripting/AP_Scripting_helpers.h | 58 ++++ libraries/AP_Scripting/docs/docs.lua | 35 +++ .../generator/description/bindings.desc | 7 + 5 files changed, 372 insertions(+) diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index ab88cb5755568d..6c780fa2fa9be7 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 380ac3ef73ca0b..f530846e4d582b 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 874cd86a17757f..79f9983b4b7237 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 @@ class Parameter 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 d33ded701df447..d3aab71b200e65 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 c959ec91316df6..2793df7c25b2df 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 From 43f4c27504d529eff06a7c72ad2eb8691bc32e61 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Feb 2025 14:58:49 +1100 Subject: [PATCH 4/4] AP_Scripting: example of using DroneCAN messaging --- .../AP_Scripting/examples/DroneCAN_test.lua | 256 ++++++++++++++++++ 1 file changed, 256 insertions(+) create mode 100644 libraries/AP_Scripting/examples/DroneCAN_test.lua diff --git a/libraries/AP_Scripting/examples/DroneCAN_test.lua b/libraries/AP_Scripting/examples/DroneCAN_test.lua new file mode 100644 index 00000000000000..007039cde18490 --- /dev/null +++ b/libraries/AP_Scripting/examples/DroneCAN_test.lua @@ -0,0 +1,256 @@ +--[[ + 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} + +local TARGET_NODE = Parameter('SCR_USER1') + +-- a handle we will use for broadcasting, sent as CANFD +local dc_handle = DroneCAN_Handle(0, MAGNETICFIELDSTRENGTHHIRES_SIGNATURE, MAGNETICFIELDSTRENGTHHIRES_ID, true) + +-- a handle for fetching parameters +local param_handle = DroneCAN_Handle(0, PARAM_GETSET_SIGNATURE, PARAM_GETSET_ID) + +--[[ + setup subscription to NodeStatus +--]] +local nodestatus_handle = DroneCAN_Handle(0, NODESTATUS_SIGNATURE, NODESTATUS_ID) +nodestatus_handle:subscribe() + +--[[ + setup subscription to raw air data +--]] +local airspeed_handle = DroneCAN_Handle(0, RAWAIRDATA_SIGNATURE, RAWAIRDATA_ID) +airspeed_handle:subscribe() + +-- table of all nodes +local node_status = {} + + +--[[ + send highres mag using a global handle +--]] +local function send_mag_highres() + 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, MAGNETICFIELDSTRENGTHHIRES_SIGNATURE, MAGNETICFIELDSTRENGTHHIRES_ID) + local payload = string.pack("Bfff", 8, 10, 11, 12) + h:broadcast(payload) + 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 not 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)) + logger:write("ASPL", 'SP,DP,ST,DT,SAT,PIT', 'ffffff', + static_pressure, differential_pressure, static_pressure_sensor_temperature, differential_pressure_sensor_temperature, + static_air_temperature, pitot_temperature) + end +end + +--[[ + parse a parameter GetSet NumericValue +--]] +local function parse_param_NumericValue(payload, byte_offset) + local vtype = string.unpack("B", payload, byte_offset[1]) + byte_offset[1] = byte_offset[1] + 1 + if vtype == 0 then + return nil + elseif vtype == 1 then + -- integer (treat as 32 bit for now, actually 64 bit) + local ret = string.unpack("i", payload, byte_offset[1]) + byte_offset[1] = byte_offset[1] + 8 + return ret + elseif vtype == 2 then + -- float32 + local ret = string.unpack("f", payload, byte_offset[1]) + byte_offset[1] = byte_offset[1] + 4 + return ret + 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[1]) + byte_offset[1] = byte_offset[1] + 1 + if vtype == 0 then + return nil + elseif vtype == 1 then + -- signed integer (64 bit), return as signed 32 bit number + local ret_lo, ret_hi = string.unpack("II", payload, byte_offset[1]) + byte_offset[1] = byte_offset[1] + 8 + if ret_hi & 0x80000000 then + return -ret_lo + end + return ret_lo + elseif vtype == 2 then + -- float32 + local ret = string.unpack("f", payload, byte_offset[1]) + byte_offset[1] = byte_offset[1] + 4 + return ret + elseif vtype == 3 then + -- bool + local v = string.unpack("B", payload, byte_offset[1]) + byte_offset[1] = byte_offset[1] + 1 + return v == 1 + elseif vtype == 4 then + -- string + local slen = string.unpack("B", payload, byte_offset[1]) + local ret = string.sub(payload, byte_offset[1]+1, slen+2) + byte_offset[1] = byte_offset[1] + 1 + slen + return ret + else + return nil + end +end + + +--[[ + parse a parameter GetSet reply +--]] +local function parse_param_reply(payload) + local byte_offset = {1} + local value = parse_param_Value(payload, byte_offset) + local default_value = parse_param_Value(payload, byte_offset) + local max_value = parse_param_NumericValue(payload, byte_offset) + local min_value = parse_param_NumericValue(payload, byte_offset) + local name = string.sub(payload, byte_offset[1], #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 = -1 + elseif type(pvalue) == "string" then + gcs:send_text(MAV_SEVERITY.INFO, string.format("got param reply from %u idx=%u '%s' : %s", nodeid, next_param_index, pname, pvalue)) + else + gcs:send_text(MAV_SEVERITY.INFO, string.format("got param reply from %u idx=%u '%s' : %f", nodeid, next_param_index, pname, pvalue)) + end + next_param_index = next_param_index + 1 + end + param_handle:request(TARGET_NODE:get(), 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, _ = 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