From b06df0da49d3e1af20fcdac85e935088a94c5116 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 17 Jan 2023 18:07:39 +1100
Subject: [PATCH] AP_Scripting: added quadplane payload place script

---
 .../applets/plane_package_place.lua           | 186 ++++++++++++++++++
 .../applets/plane_package_place.md            |  65 ++++++
 2 files changed, 251 insertions(+)
 create mode 100644 libraries/AP_Scripting/applets/plane_package_place.lua
 create mode 100644 libraries/AP_Scripting/applets/plane_package_place.md

diff --git a/libraries/AP_Scripting/applets/plane_package_place.lua b/libraries/AP_Scripting/applets/plane_package_place.lua
new file mode 100644
index 0000000000..b5d7da1f5a
--- /dev/null
+++ b/libraries/AP_Scripting/applets/plane_package_place.lua
@@ -0,0 +1,186 @@
+--[[
+ support package place for quadplanes
+--]]
+
+local PARAM_TABLE_KEY = 9
+local PARAM_TABLE_PREFIX = "PKG_"
+
+local MODE_AUTO = 10
+
+local NAV_TAKEOFF = 22
+local NAV_VTOL_PAYLOAD_PLACE = 94
+
+-- 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
+
+-- setup package place specific parameters
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 5), 'could not add param table')
+local PKG_ENABLE        = bind_add_param('ENABLE', 1, 0)
+local PKG_RELEASE_FUNC  = bind_add_param('RELEASE_FUNC', 2, 94)
+local PKG_RELEASE_HGT   = bind_add_param('RELEASE_HGT',  3, 10)
+local PKG_RELEASE_HOLD  = bind_add_param('RELEASE_HOLD', 4, 1)
+
+local Q_LAND_SPEED = Parameter("Q_LAND_SPEED")
+local Q_LAND_FINAL_ALT = Parameter("Q_LAND_FINAL_ALT")
+
+local MAV_SEVERITY_INFO = 6
+local MAV_SEVERITY_NOTICE = 5
+local MAV_SEVERITY_EMERGENCY = 0
+
+local RNG_ORIENT_DOWN = 25
+
+-- motors state
+local MOTORS_SHUT_DOWN = 0
+
+-- release state
+local RELEASE_NONE    = 0
+local RELEASE_DESCENT = 1
+local RELEASE_HOLD1   = 2
+local RELEASE_HOLD2   = 3
+local RELEASE_DONE    = 4
+local release_state = RELEASE_NONE
+
+local release_start_t = 0
+
+if PKG_ENABLE:get() ~= 1 then
+   -- not enabled
+   return
+end
+
+SRV_Channels:set_range(PKG_RELEASE_FUNC:get(), 1000)
+SRV_Channels:set_output_scaled(PKG_RELEASE_FUNC:get(), 0)
+
+-- get time in seconds
+function get_time()
+   return millis():tofloat() * 0.001
+end
+
+-- reset state
+function reset()
+   release_state = RELEASE_NONE
+   SRV_Channels:set_output_scaled(PKG_RELEASE_FUNC:get(), 0)
+end
+
+--[[
+   main update function, called at 20Hz
+--]]
+function update()
+   if PKG_ENABLE:get() ~= 1 then
+      -- not enabled
+      return
+   end
+
+   -- only do something if in AUTO and in a PACKAGE_PLACE waypoint
+   if vehicle:get_mode() ~= MODE_AUTO or mission:get_current_nav_id() ~= NAV_VTOL_PAYLOAD_PLACE then
+      reset()
+      return
+   end
+
+   if not arming:is_armed() then
+      -- nothing to do when disarmed
+      reset()
+      return
+   end
+
+   -- check spool state for if we are landed
+   local landed = false
+   if motors:get_desired_spool_state() == MOTORS_SHUT_DOWN and release_state >= RELEASE_DESCENT then
+      landed = true
+   end
+
+   -- wait till we are in the descent
+   if not quadplane:in_vtol_land_descent() and release_state == RELEASE_NONE then
+      reset()
+      return
+   end
+
+   if release_state == RELEASE_NONE then
+      -- we have started the descent
+      release_state = RELEASE_DESCENT
+   end
+
+   -- see if we have valid rangefinder data
+   if not landed and not rangefinder:has_data_orient(RNG_ORIENT_DOWN) then
+      return
+   end
+
+   -- check the distance, if less than RNG_ORIENT_DOWN then release
+   local dist_m
+   if not landed then
+      dist_m = rangefinder:distance_cm_orient(RNG_ORIENT_DOWN) * 0.01
+   else
+      dist_m = 0.0
+   end
+
+   -- slow down when within Q_LAND_FINAL_ALT of target
+   local remaining_m = dist_m - PKG_RELEASE_HGT:get()
+   if remaining_m > 0 and remaining_m < Q_LAND_FINAL_ALT:get() then
+      vehicle:set_land_descent_rate(Q_LAND_SPEED:get()*0.01)
+   end
+
+   if remaining_m <= 0 then
+      if PKG_RELEASE_HGT:get() <= 0 and not landed then
+         -- wait for landing
+         return
+      end
+
+      local now = get_time()
+      -- we are at the target altitude
+      if release_state == RELEASE_DESCENT then
+         -- start timer
+         release_start_t = now
+         release_state = RELEASE_HOLD1
+         vehicle:set_land_descent_rate(0)
+      elseif release_state == RELEASE_HOLD1 then
+         -- start waiting for the hold time for vehicle to settle
+         vehicle:set_land_descent_rate(0)
+         if now - release_start_t > PKG_RELEASE_HOLD:get() then
+            gcs:send_text(MAV_SEVERITY_INFO, string.format("Package released at %.1fm", dist_m))
+            SRV_Channels:set_output_scaled(PKG_RELEASE_FUNC:get(), 1000)
+            release_state = RELEASE_HOLD2
+         end
+      elseif release_state == RELEASE_HOLD2 then
+         -- do the release and wait for hold time again to ensure clean release
+         vehicle:set_land_descent_rate(0)
+         if now - release_start_t > PKG_RELEASE_HOLD:get()*2 then
+            release_state = RELEASE_DONE
+            -- aborting the landing causes us to climb back up and continue the mission
+            if quadplane:abort_landing() then
+               gcs:send_text(MAV_SEVERITY_INFO, string.format("Climbing"))
+            else
+               gcs:send_text(MAV_SEVERITY_NOTICE, string.format("land abort failed"))
+            end
+         end
+      end
+   end
+
+end
+
+function loop()
+   update()
+   -- run at 20Hz
+   return loop, 50
+end
+
+-- wrapper around update(). This calls update() at 20Hz,
+-- and if update faults then an error is displayed, but the script is not
+-- stopped
+function protected_wrapper()
+  local success, err = pcall(update)
+  if not success then
+     gcs:send_text(0, "Internal Error: " .. err)
+     -- when we fault we run the update function again after 1s, slowing it
+     -- down a bit so we don't flood the console with errors
+     return protected_wrapper, 1000
+  end
+  return protected_wrapper, 50
+end
+
+gcs:send_text(MAV_SEVERITY_INFO, "Loaded package place script")
+
+-- start running update loop
+return protected_wrapper()
+
diff --git a/libraries/AP_Scripting/applets/plane_package_place.md b/libraries/AP_Scripting/applets/plane_package_place.md
new file mode 100644
index 0000000000..77fb0d1892
--- /dev/null
+++ b/libraries/AP_Scripting/applets/plane_package_place.md
@@ -0,0 +1,65 @@
+# Quadplane Package Place Support
+
+This script implements support for package place in quadplanes.
+
+To use this script you need to install it in the APM/scripts folder on
+your microSD card (or build it into the firmware in ROMFS). Then
+enable scripting with SCR_ENABLE=1 and reboot.
+
+# Parameters
+
+The script adds the following parameters:
+
+## PKG_ENABLE
+
+You need to set PKG_ENABLE=1 to enable this script
+
+## PKG_RELEASE_FUNC
+
+This needs to be set to the SERVOn_FUNCTION of the release servo. It
+is recommended that you leave it at the default of 94 and set
+SERVOn_FUNCTION to 94 for the servo you want to use for package
+release.
+
+## PKG_RELEASE_HGT
+
+The parameter PKG_RELEASE_HGT controls the rangefinder height at which
+the package will be released. This can be zero if you want to release
+the package after you land.
+
+## PKG_RELEASE_HOLD
+
+This controls the time that the vehicle will stop the descent before
+it releases the package. This defaults to 1 second and is used to let
+the vehicle get into a steady hover.
+
+After package release the vehicle will hold for another
+PKG_RELEASE_HOLD seconds to let the package cleanly release before the
+vehicle climbs.
+
+# Operation
+
+To setup a mission for package place you should setup your vehicle for
+rangefinder landings. Setup a good lidar or radar and test that it
+works. Then set RNGFND_LANDING=1 to enable use of rangefinder for VTOL
+landing.
+
+In the mission you should add a PAYLOAD_PLACE waypoint at the desired
+location of the payload placement. The altitude of the PAYLOAD_PLACE
+command should be set to zero as a relative height.
+
+The PAYLOAD_PLACE command has a parametere "max descent" (parameter 1
+of the command). If this is non-zero then this is the maximum amount
+of height that the aircraft will descend from the start of the
+descent. If the aircraft tries to descend more than this height then
+the payload place will abort and the aircraft will climb back up to
+the initial descent height then continue the mission.
+
+# Landing Then Release
+
+You can also do payload place where you wait till the vehicle fully
+lands before doing the release. To do that set PGK_RELEASE_HGT to 0
+and the aircraft will land fully and shutdown the motors for
+PKG_RELEASE_HOLD seconds before releasing the servo. It will then hold
+for another PKG_RELEASE_HOLD seconds, then will climb back up to the
+original descent start height before continuing with the mission.