From e6162807477f1c0632b4857a82b5bc02564df99b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 30 Sep 2024 11:44:03 +0900
Subject: [PATCH] AP_Scripting: copter-pos-offsets supports z-axis

---
 .../examples/copter-posoffset.lua             | 45 +++++++++++++------
 1 file changed, 32 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_Scripting/examples/copter-posoffset.lua b/libraries/AP_Scripting/examples/copter-posoffset.lua
index ae0f0a711abd1e..e51941e0aaba66 100644
--- a/libraries/AP_Scripting/examples/copter-posoffset.lua
+++ b/libraries/AP_Scripting/examples/copter-posoffset.lua
@@ -15,7 +15,7 @@ local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTI
 -- setup script specific parameters
 local PARAM_TABLE_KEY = 71
 local PARAM_TABLE_PREFIX = "PSC_OFS_"
-assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 4), 'could not add param table')
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 6), 'could not add param table')
  
 -- add a parameter and bind it to a variable
 function bind_add_param(name, idx, default_value)
@@ -43,6 +43,16 @@ local PSC_OFS_POS_N = bind_add_param("POS_N", 1, 0)
 --]]
 local PSC_OFS_POS_E = bind_add_param("POS_E", 2, 0)
 
+--[[
+  // @Param: PSC_OFS_POS_D
+  // @DisplayName: Position Controller Position Offset Down
+  // @Description: Position controller offset Down
+  // @Range: 0 1000
+  // @Units: m
+  // @User: Standard
+--]]
+local PSC_OFS_POS_D = bind_add_param("POS_D", 3, 0)
+
 --[[
   // @Param: PSC_OFS_VEL_N
   // @DisplayName: Position Controller Velocity Offset North
@@ -51,7 +61,7 @@ local PSC_OFS_POS_E = bind_add_param("POS_E", 2, 0)
   // @Units: m/s
   // @User: Standard
 --]]
-local PSC_OFS_VEL_N = bind_add_param("VEL_N", 3, 0)
+local PSC_OFS_VEL_N = bind_add_param("VEL_N", 4, 0)
 
 --[[
   // @Param: PSC_OFS_VEL_E
@@ -61,10 +71,17 @@ local PSC_OFS_VEL_N = bind_add_param("VEL_N", 3, 0)
   // @Units: m/s
   // @User: Standard
 --]]
-local PSC_OFS_VEL_E = bind_add_param("VEL_E", 4, 0)
+local PSC_OFS_VEL_E = bind_add_param("VEL_E", 5, 0)
 
--- welcome message to user
-gcs:send_text(MAV_SEVERITY.INFO, "copter-posoffset.lua loaded")
+--[[
+  // @Param: PSC_OFS_VEL_D
+  // @DisplayName: Position Controller Velocity Offset Down
+  // @Description: Position controller velocity offset Down
+  // @Range: 0 10
+  // @Units: m/s
+  // @User: Standard
+--]]
+local PSC_OFS_VEL_D = bind_add_param("VEL_D", 6, 0)
 
 function update()
 
@@ -73,8 +90,8 @@ function update()
     return update, 1000
   end
 
-  local pos_offsets_zero = PSC_OFS_POS_N:get() == 0 and PSC_OFS_POS_E:get() == 0
-  local vel_offsets_zero = PSC_OFS_VEL_N:get() == 0 and PSC_OFS_VEL_E:get() == 0
+  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
 
   -- if position offsets are non-zero or all offsets are zero then send position offsets
   if not pos_offsets_zero or vel_offsets_zero then
@@ -82,21 +99,23 @@ function update()
     local pos_offset_NED = Vector3f()
     pos_offset_NED:x(PSC_OFS_POS_N:get())
     pos_offset_NED:y(PSC_OFS_POS_E:get())
+    pos_offset_NED:z(PSC_OFS_POS_D:get())
     if not poscontrol:set_posvelaccel_offset(pos_offset_NED, Vector3f(), Vector3f()) then
       gcs:send_text(MAV_SEVERITY.ERROR, "copter-posoffset: failed to set pos offset")
     end
     test_position = not pos_offsets_zero
   else
-    -- 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())
-    -- get position offset (cumulative effect of velocity offsets) and use to slowly move back to waypoint
+    -- first get position offset (cumulative effect of velocity offsets)
     local pos_offset_NED, _, _ = poscontrol:get_posvelaccel_offset()
     if pos_offset_NED == nil then
-        print_warning("unable to get dist to waypoint")
+        print_warning("unable to get position offset")
         pos_offset_NED = Vector3f()
     end
+    -- set 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())
+    vel_offset_NED:z(PSC_OFS_VEL_D:get())
     if not poscontrol:set_posvelaccel_offset(pos_offset_NED, vel_offset_NED, Vector3f()) then
       gcs:send_text(MAV_SEVERITY.ERROR, "copter-posoffset: failed to set vel offset")
     end