From b8560345f7ca93f7b4cccacd18e63c6708dcce02 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 24 Jun 2022 17:20:06 +0900
Subject: [PATCH] AP_Scripting: add mount-test example script

---
 .../AP_Scripting/examples/mount-test.lua      | 130 ++++++++++++++++++
 1 file changed, 130 insertions(+)
 create mode 100644 libraries/AP_Scripting/examples/mount-test.lua

diff --git a/libraries/AP_Scripting/examples/mount-test.lua b/libraries/AP_Scripting/examples/mount-test.lua
new file mode 100644
index 0000000000..18ef644989
--- /dev/null
+++ b/libraries/AP_Scripting/examples/mount-test.lua
@@ -0,0 +1,130 @@
+-- moves 3-axis gimbal (aka "mount") using earth-frame and body-frame rates and angles
+--
+-- stage 0: move gimbal to neutral position
+-- stage 1: yaw CW at 10deg/s in body-frame
+-- stage 2: yaw CCW at 10 deg/s in body-frame
+-- stage 3: pitch at 10 deg/s in body-frame
+-- stage 4: pitch at -10 deg/s in body-frame
+-- stage 5: roll at 10 deg/s in body-frame
+-- stage 6: roll at -10 deg/s in body-frame
+-- stage 7: point North
+-- stage 8: point South and center
+-- stage 9: point North and Down
+-- stage 10: move angle to neutral position
+
+local stage = 0
+local stage_time_ms = 5000
+local stage_start_time_ms = 0
+local last_stage = 10
+
+-- the main update function that performs a simplified version of RTL
+function update()
+
+  -- get current system time
+  local now_ms = millis()
+
+  -- start
+  if stage_start_time_ms == 0 then
+    stage_start_time_ms = now_ms
+  end
+
+  -- check if time to move to next stage
+  local update_user = false
+  if (now_ms - stage_start_time_ms > stage_time_ms) and (stage < last_stage) then
+    stage = stage + 1
+    stage_start_time_ms = now_ms
+    update_user = true
+  end
+
+  if stage == 0 or stage >= last_stage then
+    -- move angle to neutral position
+    mount:set_angle_target(0, 0, 0, 0, false)
+    if update_user then
+      gcs:send_text(6, "stage:" .. tostring(stage) .. " move to neutral position")
+    end
+  end
+
+  if stage == 1 then
+    -- yaw CW at 10deg/s in body-frame
+    mount:set_rate_target(0, 0, 0, 10, false)
+    if update_user then
+      gcs:send_text(6, "stage:" .. tostring(stage) .. " yaw at 10deg/s")
+    end
+  end
+
+  if stage == 2 then
+    -- yaw CCW at 10 deg/s in body-frame
+    mount:set_rate_target(0, 0, 0, -10, false)
+    if update_user then
+      gcs:send_text(6, "stage:" .. tostring(stage) .. " yaw at -10deg/s")
+    end
+  end
+
+  if stage == 3 then
+    -- pitch at 10 deg/s in body-frame
+    mount:set_rate_target(0, 0, 10, 0, false)
+    if update_user then
+      gcs:send_text(6, "stage:" .. tostring(stage) .. " pitch at 10deg/s")
+    end
+  end
+
+  if stage == 4 then
+    -- pitch at -10 deg/s in body-frame
+    mount:set_rate_target(0, 0, -10, 0, false)
+    if update_user then
+      gcs:send_text(6, "stage:" .. tostring(stage) .. " pitch at -10deg/s")
+    end
+  end
+
+  if stage == 5 then
+    -- roll at 10 deg/s in body-frame
+    mount:set_rate_target(0, 10, 0, 0, false)
+    if update_user then
+      gcs:send_text(6, "stage:" .. tostring(stage) .. " roll at 10deg/s")
+    end
+  end
+
+  if stage == 6 then
+    -- roll at -10 deg/s in body-frame
+    mount:set_rate_target(0, -10, 0, 0, false)
+    if update_user then
+      gcs:send_text(6, "stage:" .. tostring(stage) .. " roll at -10deg/s")
+    end
+  end
+
+  if stage == 7 then
+    -- point North
+    mount:set_angle_target(0, 0, 0, 0, true)
+    if update_user then
+      gcs:send_text(6, "stage:" .. tostring(stage) .. " point North")
+    end
+  end
+
+  if stage == 8 then
+    -- point South and center
+    mount:set_angle_target(0, 0, 0, 180, true)
+    if update_user then
+      gcs:send_text(6, "stage:" .. tostring(stage) .. " point South")
+    end
+  end
+
+  if stage == 9 then
+    -- point North and Down
+    mount:set_angle_target(0, 0, -90, 0, false)
+    if update_user then
+      gcs:send_text(6, "stage:" .. tostring(stage) .. " point Down")
+    end
+  end
+
+  if stage > last_stage then
+    -- move angle to neutral position
+    mount:set_angle_target(0, 0, 0, 0, false)
+    if update_user then
+      gcs:send_text(6, "stage:" .. tostring(stage) .. " done!")
+    end
+  end
+
+  return update, 100
+end
+
+return update()