From 42a17176767e6d89dabb332743da6c7cf0956d30 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 11 Jul 2024 17:36:12 +1000
Subject: [PATCH] AP_Scripting: added MAV_CMD_SET_HAGL example

for external height source for fixed wing landing
---
 libraries/AP_Scripting/examples/land_hagl.lua | 69 +++++++++++++++++++
 1 file changed, 69 insertions(+)
 create mode 100644 libraries/AP_Scripting/examples/land_hagl.lua

diff --git a/libraries/AP_Scripting/examples/land_hagl.lua b/libraries/AP_Scripting/examples/land_hagl.lua
new file mode 100644
index 0000000000..3bdbc36329
--- /dev/null
+++ b/libraries/AP_Scripting/examples/land_hagl.lua
@@ -0,0 +1,69 @@
+--[[
+   demonstrate proving HAGL to plane code for landing
+--]]
+
+local MAV_CMD_SET_HAGL = 43005
+
+local ROTATION_PITCH_90 = 24
+local ROTATION_PITCH_270 = 25
+
+-- for normal landing use PITCH_270, for inverted use PITCH_90
+local RANGEFINDER_ORIENT = ROTATION_PITCH_270
+
+local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
+
+local RNG_STATUS = { NotConnected = 0, NoData = 1, OutOfRangeLow = 2, OutOfRangeHigh = 3, Good = 4 }
+
+
+--[[
+ create a NaN value
+--]]
+local function NaN()
+   return 0/0
+end
+
+local last_active = false
+
+--[[
+   send HAGL data
+--]]
+local function send_HAGL()
+   local status = rangefinder:status_orient(RANGEFINDER_ORIENT)
+   if status ~= RNG_STATUS.Good then
+      last_active = false
+      return
+   end
+   local rangefinder_dist = rangefinder:distance_cm_orient(RANGEFINDER_ORIENT)*0.01
+   local correction = math.cos(ahrs:get_roll())*math.cos(ahrs:get_pitch())
+   local rangefinder_corrected = rangefinder_dist * correction
+   if RANGEFINDER_ORIENT == ROTATION_PITCH_90 then
+      rangefinder_corrected = -rangefinder_corrected
+   end
+   if rangefinder_corrected < 0 then
+      last_active = false
+      return
+   end
+
+   -- tell plane the height above ground level
+   local timeout_s = 0.2
+   local accuracy = NaN()
+   gcs:run_command_int(MAV_CMD_SET_HAGL, { p1 = rangefinder_corrected, p2 = accuracy, p3=timeout_s })
+
+   if not last_active then
+      last_active = true
+      gcs:send_text(MAV_SEVERITY.INFO, string.format("HAGL Active %.1f", rangefinder_corrected))
+   end
+
+   -- log it
+   logger:write("HAGL", "RDist,HAGL", "ff", rangefinder_dist, rangefinder_corrected)
+end
+
+local function update()
+   send_HAGL()
+   return update, 50
+end
+
+gcs:send_text(MAV_SEVERITY.INFO, "Loaded land_hagl")
+
+return update, 1000
+