From efbb1cf06fe23c23875a6a49359155a44ec5cce6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 25 Mar 2025 19:52:41 +1100
Subject: [PATCH] AP_Scripting: added set_pose SITL binding

and example script
---
 libraries/AP_Scripting/docs/docs.lua          | 11 +++
 .../AP_Scripting/examples/sim_arming_pos.lua  | 75 +++++++++++++++++++
 .../generator/description/bindings.desc       |  7 ++
 3 files changed, 93 insertions(+)
 create mode 100644 libraries/AP_Scripting/examples/sim_arming_pos.lua

diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua
index 44ea395a07f..1146a136732 100644
--- a/libraries/AP_Scripting/docs/docs.lua
+++ b/libraries/AP_Scripting/docs/docs.lua
@@ -4094,3 +4094,14 @@ function AP_Servo_Telem_Data_ud:measured_position() end
 -- get commanded position
 ---@return number|nil -- comanded position in degrees or nil if not available
 function AP_Servo_Telem_Data_ud:command_position() end
+
+-- simulator specific bindings
+sim = {}
+
+-- set pose of simulated vehicle. Requires AHRS_EKF_TYPE=10
+---@param instance integer -- 0 for first vehicle
+---@param loc Location_ud
+---@param orient Quaternion_ud
+---@param velocity_bf Vector3f_ud -- body frame velocity
+---@return boolean
+function sim:set_pose(instance, loc, orient, velocity_bf) end
diff --git a/libraries/AP_Scripting/examples/sim_arming_pos.lua b/libraries/AP_Scripting/examples/sim_arming_pos.lua
new file mode 100644
index 00000000000..f6d30a7f3e1
--- /dev/null
+++ b/libraries/AP_Scripting/examples/sim_arming_pos.lua
@@ -0,0 +1,75 @@
+--[[
+    allow for force the position of the simulated vehicle when armed
+--]]
+
+local PARAM_TABLE_KEY = 16
+local PARAM_TABLE_PREFIX = "SIM_APOS_"
+
+-- setup package place specific parameters
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 16), 'could not add param table')
+
+-- add a parameter and bind it to a variable
+function bind_add_param(name, idx, default_value)
+   assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
+   return Parameter(PARAM_TABLE_PREFIX .. name)
+end
+
+local SIM_APOS_ENABLE = bind_add_param('ENABLE', 1, 0)
+local SIM_APOS_POS_N = bind_add_param('POS_N', 2, 0)
+local SIM_APOS_POS_E = bind_add_param('POS_E', 3, 0)
+local SIM_APOS_POS_D = bind_add_param('POS_D', 4, 0)
+local SIM_APOS_VEL_X = bind_add_param('VEL_X', 5, 0)
+local SIM_APOS_VEL_Y = bind_add_param('VEL_Y', 6, 0)
+local SIM_APOS_VEL_Z = bind_add_param('VEL_Z', 7, 0)
+local SIM_APOS_RLL = bind_add_param('RLL', 8, 0)
+local SIM_APOS_PIT = bind_add_param('PIT', 9, 0)
+local SIM_APOS_YAW = bind_add_param('YAW', 10, 0)
+local SIM_APOS_MODE = bind_add_param('MODE', 11, -1)
+
+local was_armed = false
+
+local function update()
+    if SIM_APOS_ENABLE:get() == 0 then
+        return
+    end
+    local armed = arming:is_armed()
+    local set_pos = armed and not was_armed
+
+    if set_pos then
+        gcs:send_text(0, "Forcing arm pose")
+        local quat = Quaternion()
+        quat:from_euler(math.rad(SIM_APOS_RLL:get()), math.rad(SIM_APOS_PIT:get()), math.rad(SIM_APOS_YAW:get()))
+
+        local vel = Vector3f()
+        vel:x(SIM_APOS_VEL_X:get())
+        vel:y(SIM_APOS_VEL_Y:get())
+        vel:z(SIM_APOS_VEL_Z:get())
+        quat:earth_to_body(vel)
+
+        local loc = ahrs:get_origin()
+        if not loc then
+            return
+        end
+        loc:offset(SIM_APOS_POS_N:get(), SIM_APOS_POS_E:get())
+        loc:alt(loc:alt() - SIM_APOS_POS_D:get()*100)
+
+        sim:set_pose(0, loc, quat, vel)
+
+        if SIM_APOS_MODE:get() >= 0 then
+            vehicle:set_mode(SIM_APOS_MODE:get())
+        end
+    end
+
+    was_armed = armed
+end
+
+local function loop()
+    if SIM_APOS_ENABLE:get() == 0 then
+        return loop, 500
+    end
+    update()
+    return loop, 50
+end
+
+gcs:send_text(0, "Loaded arm pose")
+return loop,1000
diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc
index 38e5809636b..adf38771f66 100644
--- a/libraries/AP_Scripting/generator/description/bindings.desc
+++ b/libraries/AP_Scripting/generator/description/bindings.desc
@@ -1067,3 +1067,10 @@ userdata AP_Servo_Telem::TelemetryData field motor_temperature_cdeg int16_t'skip
 userdata AP_Servo_Telem::TelemetryData field pcb_temperature_cdeg int16_t'skip_check read valid_mask present_types AP_Servo_Telem::TelemetryData::Types::PCB_TEMP
 userdata AP_Servo_Telem::TelemetryData field status_flags uint8_t'skip_check read valid_mask present_types AP_Servo_Telem::TelemetryData::Types::STATUS
 userdata AP_Servo_Telem::TelemetryData field last_update_ms uint32_t'skip_check read
+
+-- simulator only bindings
+include SITL/SITL.h depends AP_SIM_ENABLED
+singleton SITL::SIM depends AP_SIM_ENABLED
+singleton SITL::SIM rename sim
+singleton SITL::SIM method set_pose boolean uint8_t'skip_check Location Quaternion Vector3f
+