From 689cbd44a2349a56fd9aa3a036ed2603e3ba2160 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 2 Oct 2024 20:18:35 +0900
Subject: [PATCH] AP_Scripting: update copter-posoffset

---
 .../examples/copter-posoffset.lua             | 40 +++++++++++++++++--
 1 file changed, 36 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Scripting/examples/copter-posoffset.lua b/libraries/AP_Scripting/examples/copter-posoffset.lua
index e51941e0aaba66..b0fd3774c92a66 100644
--- a/libraries/AP_Scripting/examples/copter-posoffset.lua
+++ b/libraries/AP_Scripting/examples/copter-posoffset.lua
@@ -6,9 +6,9 @@
 --
 -- How To Use
 -- 1. copy this script to the autopilot's "scripts" directory
--- 2. within the "scripts" directory create a "modules" directory
--- 3. copy the MAVLink/mavlink_msgs_xxx files to the "scripts" directory
---
+-- 2. set PSC_OFS_xx parameter to the desired position OR velocity offsets desired
+-- 3. fly the vehicle in Auto mode (or almost any mode)
+-- 4. use the Scripting1 aux switch to enable and disable the offsets
 
 local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
 
@@ -83,6 +83,12 @@ local PSC_OFS_VEL_E = bind_add_param("VEL_E", 5, 0)
 --]]
 local PSC_OFS_VEL_D = bind_add_param("VEL_D", 6, 0)
 
+-- local variables
+local aux_sw_pos_last = 0
+
+-- welcome message to user
+gcs:send_text(MAV_SEVERITY.INFO, "copter-posoffset.lua loaded")
+
 function update()
 
   -- must be armed, flying and in auto mode
@@ -90,6 +96,32 @@ function update()
     return update, 1000
   end
 
+  -- Scripting1 aux switch enables/disables offsets
+  local aux_sw_pos = rc:get_aux_cached(300)
+  if aux_sw_pos == nil then
+    aux_sw_pos = 0
+  end
+
+  -- check for change in aux switch position
+  if aux_sw_pos ~= aux_sw_pos_last then
+    aux_sw_pos_last = aux_sw_pos
+    if aux_sw_pos == 0 then
+      -- if switch is in low position clear offsets
+      if poscontrol:set_posvelaccel_offset(Vector3f(), Vector3f(), Vector3f()) then
+        gcs:send_text(MAV_SEVERITY.ERROR, "copter-posoffset: offsets cleared")
+        return update, 1000
+      else
+        gcs:send_text(MAV_SEVERITY.ERROR, "copter-posoffset: failed to clear offsets")
+      end
+    else
+      gcs:send_text(MAV_SEVERITY.ERROR, "copter-posoffset: offsets activated")
+    end
+  end
+
+  if aux_sw_pos == 0 then
+    return update, 1000
+  end
+
   local pos_offsets_zero = PSC_OFS_POS_N:get() == 0 and PSC_OFS_POS_E:get() == 0 and PSC_OFS_POS_D:get() == 0
   local vel_offsets_zero = PSC_OFS_VEL_N:get() == 0 and PSC_OFS_VEL_E:get() == 0 and PSC_OFS_VEL_D:get() == 0
 
@@ -111,7 +143,7 @@ function update()
         print_warning("unable to get position offset")
         pos_offset_NED = Vector3f()
     end
-    -- set velocity offsets in m/s in NED frame
+    -- test velocity offsets in m/s in NED frame
     local vel_offset_NED = Vector3f()
     vel_offset_NED:x(PSC_OFS_VEL_N:get())
     vel_offset_NED:y(PSC_OFS_VEL_E:get())