From ceca374f236469b364c4e9cb3252294301de8a29 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sun, 18 Jun 2023 14:00:58 +0100
Subject: [PATCH] AP_Scripting: add MAVLink command long and ack examples

---
 .../examples/MAVLink_Commands.lua             | 65 +++++++++++++
 libraries/AP_Scripting/lua_bindings.cpp       |  2 +-
 .../MAVLink/mavlink_msg_COMMAND_ACK.lua       | 11 +++
 .../MAVLink/mavlink_msg_COMMAND_LONG.lua      | 16 ++++
 .../modules/MAVLink/mavlink_msgs.lua          | 95 +++++++++++++++++++
 5 files changed, 188 insertions(+), 1 deletion(-)
 create mode 100644 libraries/AP_Scripting/examples/MAVLink_Commands.lua
 create mode 100644 libraries/AP_Scripting/modules/MAVLink/mavlink_msg_COMMAND_ACK.lua
 create mode 100644 libraries/AP_Scripting/modules/MAVLink/mavlink_msg_COMMAND_LONG.lua
 create mode 100644 libraries/AP_Scripting/modules/MAVLink/mavlink_msgs.lua

diff --git a/libraries/AP_Scripting/examples/MAVLink_Commands.lua b/libraries/AP_Scripting/examples/MAVLink_Commands.lua
new file mode 100644
index 0000000000..dab5068fa7
--- /dev/null
+++ b/libraries/AP_Scripting/examples/MAVLink_Commands.lua
@@ -0,0 +1,65 @@
+-- Example of receiving MAVLink commands
+
+local mavlink_msgs = require("MAVLink/mavlink_msgs")
+
+local COMMAND_ACK_ID = mavlink_msgs.get_msgid("COMMAND_ACK")
+local COMMAND_LONG_ID = mavlink_msgs.get_msgid("COMMAND_LONG")
+
+local msg_map = {}
+msg_map[COMMAND_ACK_ID] = "COMMAND_ACK"
+msg_map[COMMAND_LONG_ID] = "COMMAND_LONG"
+
+-- initialize MAVLink rx with number of messages, and buffer depth
+mavlink:init(1, 10)
+
+-- register message id to receive
+mavlink:register_rx_msgid(COMMAND_LONG_ID)
+
+local MAV_CMD_DO_SET_MODE = 176
+local MAV_CMD_WAYPOINT_USER_1 = 31000
+
+-- Block AP parsing user1 so we can deal with it in the script
+-- Prevents "unsupported" ack
+mavlink:block_command(MAV_CMD_WAYPOINT_USER_1)
+
+function handle_command_long(cmd)
+    if (cmd.command == MAV_CMD_DO_SET_MODE) then
+        gcs:send_text(0, "Got mode change")
+
+    elseif (cmd.command == MAV_CMD_WAYPOINT_USER_1) then
+        -- return ack from command param value
+        return math.min(math.max(math.floor(cmd.param1), 0), 5)
+    end
+    return nil
+end
+
+function update()
+    local msg, chan = mavlink:receive_chan()
+    if (msg ~= nil) then
+        local parsed_msg = mavlink_msgs.decode(msg, msg_map)
+        if (parsed_msg ~= nil) then
+
+            local result
+            if parsed_msg.msgid == COMMAND_LONG_ID then
+                result = handle_command_long(parsed_msg)
+            end
+
+            if (result ~= nil) then
+                -- Send ack if the command is one were intrested in
+                local ack = {}
+                ack.command = parsed_msg.command
+                ack.result = result
+                ack.progress = 0
+                ack.result_param2 = 0
+                ack.target_system = parsed_msg.sysid
+                ack.target_component = parsed_msg.compid
+
+                mavlink:send_chan(chan, mavlink_msgs.encode("COMMAND_ACK", ack))
+            end
+        end
+    end
+
+    return update, 1000
+end
+
+return update()
diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp
index f52b4ab085..c73b0cba3e 100644
--- a/libraries/AP_Scripting/lua_bindings.cpp
+++ b/libraries/AP_Scripting/lua_bindings.cpp
@@ -183,7 +183,7 @@ int lua_mavlink_send_chan(lua_State *L) {
 int lua_mavlink_block_command(lua_State *L) {
 
     // Allow : and . access
-    const int arg_offset = (luaL_testudata(L, 1, "mavlnk") != NULL) ? 1 : 0;
+    const int arg_offset = (luaL_testudata(L, 1, "mavlink") != NULL) ? 1 : 0;
 
     binding_argcheck(L, 1+arg_offset);
 
diff --git a/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_COMMAND_ACK.lua b/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_COMMAND_ACK.lua
new file mode 100644
index 0000000000..0cc6834c96
--- /dev/null
+++ b/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_COMMAND_ACK.lua
@@ -0,0 +1,11 @@
+local COMMAND_ACK = {}
+COMMAND_ACK.id = 77
+COMMAND_ACK.fields = {
+             { "command", "<I2" },
+             { "result", "<B" },
+             { "progress", "<B" },
+             { "result_param2", "<i4" },
+             { "target_system", "<B" },
+             { "target_component", "<B" },
+             }
+return COMMAND_ACK
diff --git a/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_COMMAND_LONG.lua b/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_COMMAND_LONG.lua
new file mode 100644
index 0000000000..ba40100f41
--- /dev/null
+++ b/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_COMMAND_LONG.lua
@@ -0,0 +1,16 @@
+local COMMAND_LONG = {}
+COMMAND_LONG.id = 76
+COMMAND_LONG.fields = {
+             { "param1", "<f" },
+             { "param2", "<f" },
+             { "param3", "<f" },
+             { "param4", "<f" },
+             { "param5", "<f" },
+             { "param6", "<f" },
+             { "param7", "<f" },
+             { "command", "<I2" },
+             { "target_system", "<B" },
+             { "target_component", "<B" },
+             { "confirmation", "<B" },
+             }
+return COMMAND_LONG
diff --git a/libraries/AP_Scripting/modules/MAVLink/mavlink_msgs.lua b/libraries/AP_Scripting/modules/MAVLink/mavlink_msgs.lua
new file mode 100644
index 0000000000..34e5a3bd65
--- /dev/null
+++ b/libraries/AP_Scripting/modules/MAVLink/mavlink_msgs.lua
@@ -0,0 +1,95 @@
+local mavlink_msgs = {}
+
+function mavlink_msgs.get_msgid(msgname)
+  local message_map = require("MAVLink/mavlink_msg_" .. msgname)
+  if not message_map then
+    error("Unknown MAVLink message " .. msgname)
+  end
+  return message_map.id
+end
+
+function mavlink_msgs.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
+
+  _, 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 mavlink_msgs.decode(message, msg_map)
+  local result, offset = mavlink_msgs.decode_header(message)
+  local message_map = require("MAVLink/mavlink_msg_" .. msg_map[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 _,v in ipairs(message_map.fields) 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 mavlink_msgs.encode(msgname, message)
+  local message_map = require("MAVLink/mavlink_msg_" .. msgname)
+  if not message_map then
+    -- we don't know how to encode this message, bail on it
+    error("Unknown MAVLink message " .. msgname)
+  end
+
+  local packString = "<"
+  local packedTable = {}
+  local packedIndex = 1
+  for i,v in ipairs(message_map.fields) 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.fields[i][1]][j]
+        if packedTable[packedIndex] == nil then
+          packedTable[packedIndex] = 0
+        end
+        packedIndex = packedIndex + 1
+      end
+    else
+      packString = (packString .. string.sub(v[2], 2))
+      packedTable[packedIndex] = message[message_map.fields[i][1]]
+      packedIndex = packedIndex + 1
+    end
+  end
+  return message_map.id, string.pack(packString, table.unpack(packedTable))
+end
+
+return mavlink_msgs