AP_Scripting: add mavlink support to scripting

libraries/AP_Scripting/AP_Scripting.cpp

@@ -140,8 +140,6 @@ 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) {
@@ -341,7 +339,7 @@ void AP_Scripting::restart_all()
 }
 
 void AP_Scripting::handle_message(const mavlink_message_t &msg, const mavlink_channel_t chan) {
-    if (mavlink_data.input == nullptr) {
+    if (mavlink_data.rx_buffer == nullptr) {
         return;
     }
 
@@ -349,45 +347,17 @@ void AP_Scripting::handle_message(const mavlink_message_t &msg, const mavlink_ch
 
     WITH_SEMAPHORE(mavlink_data.sem);
 
-    for (int32_t id : mavlink_data.accept_msg_ids) {
+    for (uint16_t i = 0; i < mavlink_data.accept_msg_ids_size; i++) {
-        if (id == -1) {
+        if (mavlink_data.accept_msg_ids[i] == -1) {
             return;
         }
-        if (id == msg.msgid) {
+        if (mavlink_data.accept_msg_ids[i] == msg.msgid) {
-            mavlink_data.input->push(data);
+            mavlink_data.rx_buffer->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

@@ -45,8 +45,6 @@ public:
 
     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[];
@@ -106,19 +104,13 @@ public:
         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_msg> *rx_buffer;
-        ObjectBuffer<struct mavlink_output> * output;
+        int *accept_msg_ids;
-        int32_t accept_msg_ids[16];
+        uint16_t accept_msg_ids_size;
         HAL_Semaphore sem;
     } mavlink_data;
 

libraries/AP_Scripting/examples/mavlink.lua

@@ -1,134 +1,37 @@
-function decode_header(message)
+mavlink_msgs = require("mavlink_msgs")
-  -- build up a map of the result
-  local result = {}
 
-  local read_marker = 3
+msg_map = {}
 
-  -- id the MAVLink version
+heartbeat_msgid = mavlink_msgs.get_msgid("HEARTBEAT")
-  result.protocol_version, read_marker = string.unpack("<B", message, read_marker)
+
-  if (result.protocol_version == 0xFE) then -- mavlink 1
+msg_map[heartbeat_msgid] = "HEARTBEAT"
-    result.protocol_version = 1
+-- initialise mavlink rx with number of messages, and buffer depth
-  elseif (result.protocol_version == 0XFD) then --mavlink 2
+mavlink.init(1, 10)
-    result.protocol_version = 2
+-- register message id to receive
-  else
+mavlink.receive_msgid(heartbeat_msgid)
-    error("Invalid magic byte")
+test_named_value = 0.0
+function str_to_bytes(str)
+    str_len = string.len(str)
+    bytes =  {}
+    for i = 1, str_len do
+        bytes[i] = string.byte(str, i)
     end
-
+    return bytes
-  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 msg = mavlink.receive()
-  local message = mavlink.receive()
+    if msg then
-  local decoded_msg
+        parsed_msg = mavlink_msgs.decode(msg, msg_map)
-  if message then
+        if parsed_msg.msgid == heartbeat_msgid then
-    decoded_msg = decode(message, messages)
+            gcs:send_text(6, string.format("Received heartbeat from %d", parsed_msg.sysid))
-    gcs:send_text(0, string.format("%d", decoded_msg.command))
         end
-
+    else
-  bar.desired = math.random()
+        gcs:send_text(6, "No heartbeat received")
-
+    end
-  mavlink.send(0, 194, encode(194, bar, messages))
+    test_named_value = test_named_value + 1.0
-
+    -- send named value float to channel 0
-  return update, 500
+    mavlink.send(0, mavlink_msgs.encode("NAMED_VALUE_FLOAT", {time_boot_ms = millis():toint(), name = str_to_bytes("test"), value = test_named_value}))
+    return update, 1000
 end
 
 return update()

libraries/AP_Scripting/generator/description/bindings.desc

@@ -720,3 +720,8 @@ userdata uint32_t manual tofloat uint32_t_tofloat 0
 
 global manual dirlist lua_dirlist 1
 global manual remove lua_removefile 1
+
+singleton mavlink manual init lua_mavlink_init 2
+singleton mavlink manual receive_msgid lua_mavlink_receive_msgid 1
+singleton mavlink manual send lua_mavlink_send 3
+singleton mavlink manual receive lua_mavlink_receive 0

libraries/AP_Scripting/lua_bindings.cpp

@@ -38,17 +38,44 @@ 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;
+int lua_mavlink_init(lua_State *L) {
-    ObjectBuffer<struct AP_Scripting::mavlink_msg> *input = AP::scripting()->mavlink_data.input;
+    binding_argcheck(L, 2);
+    WITH_SEMAPHORE(AP::scripting()->mavlink_data.sem);
+    // get the depth of receive queue
+    const int queue_size = luaL_checkinteger(L, -1);
+    // get number of msgs to accept
+    const int num_msgs = luaL_checkinteger(L, -2);
 
-    if (input == nullptr) {
+    struct AP_Scripting::mavlink &data = AP::scripting()->mavlink_data;
-        luaL_error(L, "Never subscribed to a message");
+    if (data.rx_buffer == nullptr) {
+        data.rx_buffer = new ObjectBuffer<struct AP_Scripting::mavlink_msg>(queue_size);
+        if (data.rx_buffer == nullptr) {
+            return luaL_error(L, "Failed to allocate mavlink rx buffer");
+        }
+    }
+    if (data.accept_msg_ids == nullptr) {
+        data.accept_msg_ids = new int[num_msgs];
+        if (data.accept_msg_ids == nullptr) {
+            return luaL_error(L, "Failed to allocate mavlink rx registry");
+        }
+        data.accept_msg_ids_size = num_msgs;
+        memset(data.accept_msg_ids, -1, sizeof(int) * num_msgs);
+    }
+    return 0;
 }
 
-    if (input->pop(msg)) {
+int lua_mavlink_receive(lua_State *L) {
+    binding_argcheck(L, 0);
+
+    struct AP_Scripting::mavlink_msg msg;
+    ObjectBuffer<struct AP_Scripting::mavlink_msg> *rx_buffer = AP::scripting()->mavlink_data.rx_buffer;
+
+    if (rx_buffer == nullptr) {
+        return luaL_error(L, "Never subscribed to a message");
+    }
+
+    if (rx_buffer->pop(msg)) {
         luaL_Buffer b;
         luaL_buffinit(L, &b);
         luaL_addlstring(&b, (char *)&msg.msg, sizeof(msg.msg));
@@ -61,41 +88,30 @@ static int lua_mavlink_receive(lua_State *L) {
     }
 }
 
-static int lua_mavlink_register_msgid(lua_State *L) {
+int lua_mavlink_receive_msgid(lua_State *L) {
-    check_arguments(L, 1, "register_msgid");
+    binding_argcheck(L, 1);
-    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) {
+    for (uint8_t i = 0; i < data.accept_msg_ids_size; i++) {
-        if (id == msgid) {
+        if (data.accept_msg_ids[i] == msgid) {
             lua_pushboolean(L, false);
             return 1;
         }
     }
 
     int i = 0;
-    for (i = 0; i < (int)ARRAY_SIZE(data.accept_msg_ids); i++) {
+    for (i = 0; i < data.accept_msg_ids_size; i++) {
         if (data.accept_msg_ids[i] == -1) {
             break;
         }
     }
 
-    if (i >= (int)ARRAY_SIZE(data.accept_msg_ids)) {
+    if (i >= data.accept_msg_ids_size) {
         return luaL_error(L, "Out of MAVLink ID's to monitor");
     }
 
@@ -108,8 +124,8 @@ static int lua_mavlink_register_msgid(lua_State *L) {
     return 1;
 }
 
-static int lua_mavlink_send(lua_State *L) {
+int lua_mavlink_send(lua_State *L) {
-    check_arguments(L, 3, "send");
+    binding_argcheck(L, 3);
     
     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");
@@ -119,20 +135,22 @@ static int lua_mavlink_send(lua_State *L) {
 
     const char *packet = luaL_checkstring(L, 3);
 
-    struct AP_Scripting::mavlink &mavlink_data = AP::scripting()->mavlink_data;
+    // FIXME: The data that's 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
-    if (mavlink_data.output == nullptr) {
+    //        are known at compile time. This is fine as a starting point as this is symmetrical to the
-        mavlink_data.output = new ObjectBuffer<struct AP_Scripting::mavlink_output>(AP_Scripting::mavlink_output_queue_size);
+    //        decoding side of the scripting support
-        if (mavlink_data.output == nullptr) {
+    const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
-            return luaL_error(L, "Unable to allocate MAVLink output queue");
+    if (entry == nullptr) {
-        }
+        return luaL_error(L, "Unknown MAVLink message ID (%d)", msgid);
     }
+    if (comm_get_txspace(chan) >= (GCS_MAVLINK::packet_overhead_chan(chan) + entry->max_msg_len)) {
+        _mav_finalize_message_chan_send(chan,
+                                        entry->msgid,
+                                        packet,
+                                        entry->min_msg_len,
+                                        entry->max_msg_len,
+                                        entry->crc_extra);
 
-    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);
@@ -511,25 +529,6 @@ 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);

libraries/AP_Scripting/lua_bindings.h

@@ -14,3 +14,7 @@ int lua_dirlist(lua_State *L);
 int lua_removefile(lua_State *L);
 int SRV_Channels_get_safety_state(lua_State *L);
 int lua_get_PWMSource(lua_State *L);
+int lua_mavlink_init(lua_State *L);
+int lua_mavlink_receive(lua_State *L);
+int lua_mavlink_receive_msgid(lua_State *L);
+int lua_mavlink_send(lua_State *L);