From a550236fe200faaaad0aa7c49aa16db1d617c8f6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 23 Jun 2024 19:37:15 +1000
Subject: [PATCH] AP_Scripting: added gcs:command_int() binding

---
 libraries/AP_Scripting/docs/docs.lua          |  7 ++
 .../AP_Scripting/examples/command_int.lua     | 45 +++++++++++++
 .../generator/description/bindings.desc       |  1 +
 libraries/AP_Scripting/lua_bindings.cpp       | 64 +++++++++++++++++++
 libraries/AP_Scripting/lua_bindings.h         |  1 +
 5 files changed, 118 insertions(+)
 create mode 100644 libraries/AP_Scripting/examples/command_int.lua

diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua
index 7171b241c5296b..69f13f90668b1b 100644
--- a/libraries/AP_Scripting/docs/docs.lua
+++ b/libraries/AP_Scripting/docs/docs.lua
@@ -2585,6 +2585,13 @@ function gcs:send_text(severity, text) end
 ---@return uint32_t_ud -- system time in milliseconds
 function gcs:last_seen() end
 
+-- call a MAVLink MAV_CMD_xxx command via command_int interface
+---@param frame integer -- MAV_FRAME_xxx
+---@param command integer -- MAV_CMD_xxx
+---@param params table -- parameters, including x, y and z
+---@return boolean
+function gcs:command_int(frame, command, params) end
+
 -- The relay library provides access to controlling relay outputs.
 relay = {}
 
diff --git a/libraries/AP_Scripting/examples/command_int.lua b/libraries/AP_Scripting/examples/command_int.lua
new file mode 100644
index 00000000000000..af02590daf2f50
--- /dev/null
+++ b/libraries/AP_Scripting/examples/command_int.lua
@@ -0,0 +1,45 @@
+--[[
+   demonstrate using the gcs:command_int() interface to send commands from MAVLink MAV_CMD_xxx set
+--]]
+
+local MAV_FRAME_GLOBAL = 0
+
+local MAV_CMD_DO_SET_MODE = 176
+local MAV_CMD_DO_REPOSITION = 192
+
+-- some plane flight modes for testing
+local MODE_LOITER = 12
+local MODE_GUIDED = 15
+
+local MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
+
+--[[
+   test API calls. When in LOITER mode change to GUIDED and force flying to a location NE of home
+--]]
+local function test_command_int()
+   if vehicle:get_mode() ~= MODE_LOITER then
+      return
+   end
+   local home = ahrs:get_home()
+   if not home then
+      return
+   end
+
+   -- force mode GUIDED
+   gcs:command_int(MAV_FRAME_GLOBAL, MAV_CMD_DO_SET_MODE, { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, MODE_GUIDED })
+
+   -- and fly to 200m NE of home and 100m above home
+   local dest = home:copy()
+   dest:offset_bearing(45, 200)
+   dest:alt(home:alt()+100*100)
+
+   gcs:command_int(MAV_FRAME_GLOBAL, MAV_CMD_DO_REPOSITION, { -1, 0, 0, 0, dest:lat(), dest:lng(), dest:alt()*0.01 })
+end
+
+local function update()
+   test_command_int()
+   return update, 1000
+end
+
+return update, 1000
+
diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc
index 40910361296c50..2a07c34729ba80 100644
--- a/libraries/AP_Scripting/generator/description/bindings.desc
+++ b/libraries/AP_Scripting/generator/description/bindings.desc
@@ -296,6 +296,7 @@ singleton GCS method get_high_latency_status depends HAL_HIGH_LATENCY2_ENABLED =
 
 singleton GCS method enable_high_latency_connections void boolean
 singleton GCS method enable_high_latency_connections depends HAL_HIGH_LATENCY2_ENABLED == 1
+singleton GCS manual command_int lua_GCS_command_int 3 1
 
 include AP_ONVIF/AP_ONVIF.h depends ENABLE_ONVIF == 1
 singleton AP_ONVIF depends ENABLE_ONVIF == 1
diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp
index de82efd87013e3..3713e0e27ee04a 100644
--- a/libraries/AP_Scripting/lua_bindings.cpp
+++ b/libraries/AP_Scripting/lua_bindings.cpp
@@ -1025,4 +1025,68 @@ void lua_abort()
 #endif
 }
 
+#if HAL_GCS_ENABLED
+/*
+  implement gcs:command_int() access to MAV_CMD_xxx commands
+ */
+int lua_GCS_command_int(lua_State *L)
+{
+    GCS *_gcs = check_GCS(L);
+    binding_argcheck(L, 4);
+
+    const uint16_t frame = get_uint8_t(L, 2);
+    const uint16_t command = get_uint16_t(L, 3);
+    if (!lua_istable(L, 4)) {
+        // must have parameter table
+        return 0;
+    }
+
+    mavlink_command_int_t pkt {};
+
+    pkt.frame = frame;
+    pkt.command = command;
+
+    float *params = &pkt.param1;
+    int32_t *xy = &pkt.x;
+
+    // extract the first 4 parameters as floats
+    for (uint8_t i=0; i<4; i++) {
+        lua_pushinteger(L, i+1);
+        lua_gettable(L, 4);
+        if (lua_isnumber(L, -1)) {
+            params[i] = lua_tonumber(L, -1);
+        }
+        lua_pop(L, 1);
+    }
+
+    // extract the xy values
+    for (uint8_t i=0; i<2; i++) {
+        lua_pushinteger(L, i+5);
+        lua_gettable(L, 4);
+        if (lua_isinteger(L, -1)) {
+            xy[i] = lua_tointeger(L, -1);
+        }
+        lua_pop(L, 1);
+    }
+
+    // and z
+    lua_pushinteger(L, 7);
+    lua_gettable(L, 4);
+    if (lua_isnumber(L, -1)) {
+        pkt.z = lua_tonumber(L, -1);
+    }
+    lua_pop(L, 1);
+    
+    // call the interface
+    auto result = _gcs->lua_command_int_packet(pkt);
+
+    // map MAV_RESULT to a boolean
+    bool ok = result == MAV_RESULT_ACCEPTED;
+
+    lua_pushboolean(L, ok);
+
+    return 1;
+}
+#endif
+
 #endif  // AP_SCRIPTING_ENABLED
diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h
index add40faa0a7bd8..a0e4c591eb0ea4 100644
--- a/libraries/AP_Scripting/lua_bindings.h
+++ b/libraries/AP_Scripting/lua_bindings.h
@@ -27,3 +27,4 @@ int lua_mavlink_send_chan(lua_State *L);
 int lua_mavlink_block_command(lua_State *L);
 int lua_print(lua_State *L);
 int lua_range_finder_handle_script_msg(lua_State *L);
+int lua_GCS_command_int(lua_State *L);