DroneCAN: support sending/receiving DroneCAN messages in lua

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];

libraries/AP_Scripting/AP_Scripting.cpp

@@ -24,6 +24,7 @@
 #include <AP_Arming/AP_Arming.h>
 
 #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

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<DroneCAN_Handle*>(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

libraries/AP_Scripting/AP_Scripting_helpers.h

@@ -2,6 +2,7 @@
 
 #include <AP_Param/AP_Param.h>
 #include "lua/src/lua.hpp"
+#include <AP_DroneCAN/AP_DroneCAN.h>
 
 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<Payload> 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