AP_Scripting: Support sending MAVLink buffers from scripts

libraries/AP_Scripting/AP_Scripting.cpp

@@ -140,6 +140,8 @@ AP_Scripting::AP_Scripting() {
     }
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
     _singleton = this;
+
+    memset(mavlink_data.accept_msg_ids, -1, sizeof(mavlink_data.accept_msg_ids));
 }
 
 void AP_Scripting::init(void) {
@@ -338,6 +340,54 @@ void AP_Scripting::restart_all()
     _stop = true;
 }
 
+void AP_Scripting::handle_message(const mavlink_message_t &msg, const mavlink_channel_t chan) {
+    if (mavlink_data.input == nullptr) {
+        return;
+    }
+
+    struct mavlink_msg data {msg, chan};
+
+    WITH_SEMAPHORE(mavlink_data.sem);
+
+    for (int32_t id : mavlink_data.accept_msg_ids) {
+        if (id == -1) {
+            return;
+        }
+        if (id == msg.msgid) {
+            mavlink_data.input->push(data);
+            return;
+        }
+    }
+}
+
+void AP_Scripting::send_message(const mavlink_channel_t chan) {
+    if (mavlink_data.output == nullptr) {
+        return;
+    }
+
+    struct mavlink_output data;
+    if (mavlink_data.output->peek(data) && (data.chan == chan)) {
+        // FIXME: The data thats in this mavlink_msg_entry_t should be provided from the script, which allows
+        //        sending entirely new messages as outputs. At the moment we can only encode messages that
+        //        are known at compile time. This is fine as a starting point as this is symmetrical to the
+        //        decoding side of the scripting support
+        const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(data.msgid);
+        if (entry == nullptr) {
+            return;
+        }
+        if (comm_get_txspace(chan) >= (GCS_MAVLINK::packet_overhead_chan(chan) + entry->max_msg_len)) {
+            _mav_finalize_message_chan_send(chan,
+                                            entry->msgid,
+                                            data.data,
+                                            entry->min_msg_len,
+                                            entry->max_msg_len,
+                                            entry->crc_extra);
+
+            UNUSED_RESULT(mavlink_data.output->pop(data));
+        }
+    }
+}
+
 AP_Scripting *AP_Scripting::_singleton = nullptr;
 
 namespace AP {

libraries/AP_Scripting/AP_Scripting.h

@@ -43,6 +43,10 @@ public:
     bool enabled(void) const { return _enable != 0; };
     bool should_run(void) const { return enabled() && !_stop; }
 
+    void handle_message(const mavlink_message_t &msg, const mavlink_channel_t chan);
+
+    void send_message(const mavlink_channel_t chan);
+
     static AP_Scripting * get_singleton(void) { return _singleton; }
 
     static const struct AP_Param::GroupInfo var_info[];
@@ -97,6 +101,27 @@ public:
     int get_current_ref() { return current_ref; }
     void set_current_ref(int ref) { current_ref = ref; }
 
+    struct mavlink_msg {
+        mavlink_message_t msg;
+        mavlink_channel_t chan;
+    };
+
+    struct mavlink_output {
+        char data[256]; // maximum payload size
+        int32_t msgid;
+        mavlink_channel_t chan;
+    };
+
+    static const int mavlink_input_queue_size = 5;
+    static const int mavlink_output_queue_size = 3;
+
+    struct mavlink {
+        ObjectBuffer<struct mavlink_msg> * input;
+        ObjectBuffer<struct mavlink_output> * output;
+        int32_t accept_msg_ids[16];
+        HAL_Semaphore sem;
+    } mavlink_data;
+
 private:
 
     bool repl_start(void);

libraries/AP_Scripting/examples/mavlink.lua

@@ -0,0 +1,134 @@
+function decode_header(message)
+  -- build up a map of the result
+  local result = {}
+
+  local read_marker = 3
+
+  -- id the MAVLink version
+  result.protocol_version, read_marker = string.unpack("<B", message, read_marker)
+  if (result.protocol_version == 0xFE) then -- mavlink 1
+    result.protocol_version = 1
+  elseif (result.protocol_version == 0XFD) then --mavlink 2
+    result.protocol_version = 2
+  else
+    error("Invalid magic byte")
+  end
+
+  local payload_len, read_marker = string.unpack("<B", message, read_marker) -- payload is always the second byte
+
+  -- strip the incompat/compat flags
+  result.incompat_flags, result.compat_flags, read_marker = string.unpack("<BB", message, read_marker)
+
+  -- fetch seq/sysid/compid
+  result.seq, result.sysid, result.compid, read_marker = string.unpack("<BBB", message, read_marker)
+
+  -- fetch the message id
+  result.msgid, read_marker = string.unpack("<I3", message, read_marker)
+
+  return result, read_marker
+end
+
+function decode(message, messages)
+  local result, offset = decode_header(message)
+  local message_map = messages[result.msgid]
+  if not message_map then
+    -- we don't know how to decode this message, bail on it
+    return nil
+  end
+
+  -- map all the fields out
+  for i,v in ipairs(message_map) do
+    if v[3] then
+      result[v[1]] = {}
+      for j=1,v[3] do
+        result[v[1]][j], offset = string.unpack(v[2], message, offset)
+      end
+    else
+      result[v[1]], offset = string.unpack(v[2], message, offset)
+    end
+  end
+
+  -- ignore the idea of a checksum
+
+  return result;
+end
+
+function encode(msgid, message, messages)
+  local message_map = messages[msgid]   
+  if not message_map then                 
+    -- we don't know how to encode this message, bail on it
+    error("Unknown MAVLink message " .. msgid)
+  end
+
+  local packString = "<"
+  local packedTable = {}                  
+  local packedIndex = 1
+  for i,v in ipairs(message_map) do
+    if v[3] then
+      packString = (packString .. string.rep(string.sub(v[2], 2), v[3]))
+      for j = 1, v[3] do
+        packedTable[packedIndex] = message[message_map[i][1]][j]
+        packedIndex = packedIndex + 1
+      end
+    else                          
+      packString = (packString .. string.sub(v[2], 2))
+      packedTable[packedIndex] = message[message_map[i][1]]
+      packedIndex = packedIndex + 1
+    end
+  end
+
+  return string.pack(packString, table.unpack(packedTable))
+end
+
+messages = {}
+messages[76] = { -- COMMAND_LONG
+             { "param1", "<f" },
+             { "param2", "<f" },
+             { "param3", "<f" },
+             { "param4", "<f" },
+             { "param5", "<f" },
+             { "param6", "<f" },
+             { "param7", "<f" },
+             { "command", "<I2" },
+             { "target_system", "<B" },
+             { "target_component", "<B" },
+             { "confirmation", "<B" },
+             }
+messages[194] = { -- PID_TUNING
+             { "desired", "<f" },
+             { "achieved", "<f" },
+             { "FF", "<f" },
+             { "P", "<f" },
+             { "I", "<f" },
+             { "D", "<f" },
+             { "axis", "<B" },
+             } 
+
+bar = {}
+bar.desired = 12.5
+bar.achieved = 13.5
+bar.FF = 14.5
+bar.P = 15.5
+bar.I = 16.5
+bar.D = 17.5
+bar.axis = 6
+
+mavlink.register_msgid(76)
+
+function update()
+
+  local message = mavlink.receive()
+  local decoded_msg
+  if message then
+    decoded_msg = decode(message, messages)
+    gcs:send_text(0, string.format("%d", decoded_msg.command))
+  end
+
+  bar.desired = math.random()
+
+  mavlink.send(0, 194, encode(194, bar, messages))
+
+  return update, 500
+end
+
+return update()

libraries/AP_Scripting/lua_bindings.cpp

@@ -11,6 +11,8 @@
 #include <AP_Scripting/AP_Scripting.h>
 #include <string.h>
 
+extern const AP_HAL::HAL& hal;
+
 extern "C" {
 #include "lua/src/lmem.h"
 }
@@ -36,6 +38,108 @@ int lua_micros(lua_State *L) {
 
     return 1;
 }
+static int lua_mavlink_receive(lua_State *L) {
+    check_arguments(L, 0, "receive");
+
+    struct AP_Scripting::mavlink_msg msg;
+    ObjectBuffer<struct AP_Scripting::mavlink_msg> *input = AP::scripting()->mavlink_data.input;
+
+    if (input == nullptr) {
+        luaL_error(L, "Never subscribed to a message");
+    }
+
+    if (input->pop(msg)) {
+        luaL_Buffer b;
+        luaL_buffinit(L, &b);
+        luaL_addlstring(&b, (char *)&msg.msg, sizeof(msg.msg));
+        luaL_pushresult(&b);
+        lua_pushinteger(L, msg.chan);
+        return 2;
+    } else {
+        // no MAVLink to handle, just return no results
+        return 0;
+    }
+}
+
+static int lua_mavlink_register_msgid(lua_State *L) {
+    check_arguments(L, 1, "register_msgid");
+    luaL_checkstack(L, 1, "Out of stack");
+    
+    const int msgid = luaL_checkinteger(L, -1);
+    luaL_argcheck(L, ((msgid >= 0) && (msgid < (1 << 24))), 1, "msgid out of range");
+
+    struct AP_Scripting::mavlink &data = AP::scripting()->mavlink_data;
+
+    if (data.input == nullptr) {
+        { // WITH_SEMAPHORE cannot be used in the same scope as a luaL_error (or any thing else that jumps away)
+            WITH_SEMAPHORE(data.sem);
+            data.input = new ObjectBuffer<struct AP_Scripting::mavlink_msg>(AP_Scripting::mavlink_input_queue_size);
+        }
+        if (data.input == nullptr) {
+            return luaL_error(L, "Unable to allocate MAVLink buffer");
+        }
+    }
+
+    // check that we aren't currently watching this ID
+    for (int id : data.accept_msg_ids) {
+        if (id == msgid) {
+            lua_pushboolean(L, false);
+            return 1;
+        }
+    }
+
+    int i = 0;
+    for (i = 0; i < (int)ARRAY_SIZE(data.accept_msg_ids); i++) {
+        if (data.accept_msg_ids[i] == -1) {
+            break;
+        }
+    }
+
+    if (i >= (int)ARRAY_SIZE(data.accept_msg_ids)) {
+        return luaL_error(L, "Out of MAVLink ID's to monitor");
+    }
+
+    {
+        WITH_SEMAPHORE(data.sem);
+        data.accept_msg_ids[i] = msgid;
+    }
+
+    lua_pushboolean(L, true);
+    return 1;
+}
+
+static int lua_mavlink_send(lua_State *L) {
+    check_arguments(L, 3, "send");
+    
+    const mavlink_channel_t chan = (mavlink_channel_t)luaL_checkinteger(L, 1);
+    luaL_argcheck(L, (chan < MAVLINK_COMM_NUM_BUFFERS), 1, "channel out of range");
+
+    const int msgid = luaL_checkinteger(L, 2);
+    luaL_argcheck(L, ((msgid >= 0) && (msgid < (1 << 24))), 2, "msgid out of range");
+
+    const char *packet = luaL_checkstring(L, 3);
+
+    struct AP_Scripting::mavlink &mavlink_data = AP::scripting()->mavlink_data;
+
+    if (mavlink_data.output == nullptr) {
+        mavlink_data.output = new ObjectBuffer<struct AP_Scripting::mavlink_output>(AP_Scripting::mavlink_output_queue_size);
+        if (mavlink_data.output == nullptr) {
+            return luaL_error(L, "Unable to allocate MAVLink output queue");
+        }
+    }
+
+    AP_Scripting::mavlink_output data {};
+    memcpy(data.data, packet, MIN(sizeof(data.data), lua_rawlen(L, 3)));
+    data.chan = chan;
+    data.msgid = msgid;
+    if (mavlink_data.output->push(data)) {
+        lua_pushboolean(L, true);
+    } else {
+        lua_pushboolean(L, false);
+    }
+
+    return 1;
+}
 
 int lua_mission_receive(lua_State *L) {
     binding_argcheck(L, 0);
@@ -407,6 +511,25 @@ int lua_get_i2c_device(lua_State *L) {
 
     return 1;
 }
+static const luaL_Reg mavlink_functions[] =
+{
+    {"receive", lua_mavlink_receive},
+    {"register_msgid", lua_mavlink_register_msgid},
+    {"send", lua_mavlink_send},
+    {NULL, NULL}
+};
+
+void load_lua_bindings(lua_State *L) {
+    lua_pushstring(L, "logger");
+    luaL_newlib(L, AP_Logger_functions);
+    lua_settable(L, -3);
+
+    lua_pushstring(L, "mavlink");
+    luaL_newlib(L, mavlink_functions);
+    lua_settable(L, -3);
+
+    luaL_setfuncs(L, global_functions, 0);
+}
 
 int AP_HAL__I2CDevice_read_registers(lua_State *L) {
     const int args = lua_gettop(L);