From bbd9148b4b8819cb0b409d7a0d50470bfb27c992 Mon Sep 17 00:00:00 2001
From: timtuxworth <tim@objectivedata.com>
Date: Mon, 9 Sep 2024 17:33:05 -0600
Subject: [PATCH] AP_Scripting: example for use Location::AltFrame for
guided_state.target_alt_frame
---
.../examples/plane_guided_terrain.lua | 234 ++++++++++++++++++
1 file changed, 234 insertions(+)
create mode 100644 libraries/AP_Scripting/examples/plane_guided_terrain.lua
diff --git a/libraries/AP_Scripting/examples/plane_guided_terrain.lua b/libraries/AP_Scripting/examples/plane_guided_terrain.lua
new file mode 100644
index 0000000000000..86f6ded9f329b
--- /dev/null
+++ b/libraries/AP_Scripting/examples/plane_guided_terrain.lua
@@ -0,0 +1,234 @@
+--[[
+
+ example script to show reseting guided mode to terrain height.
+ It's intended to be used when selecting "fly to altitude" in a ground station
+ for example by right clicking on the map in QGC.
+
+ This is actually a workaround to QGC and Mission Planner not having a
+ way to set guided altitude above terrain.
+
+ Depending on ZGP_MODE
+ When the GCS requests a guided altitude X above home
+ 1: reset to current terrain height ignore X
+ 2: reset to X above terrain
+ 3: reset to current alt + X
+
+ this functionality is only available in Plane and
+ requires TERRAIN_ENABLE = 1 and TERRAIN_FOLLOW =1
+--]]
+
+SCRIPT_NAME = "OverheadIntel Guided Terrain"
+SCRIPT_NAME_SHORT = "TerrGuided"
+SCRIPT_VERSION = "4.6.0-005"
+
+REFRESH_RATE = 0.2 -- in seconds, so 5Hz
+ALTITUDE_MIN = 50
+ALTITUDE_MAX = 120
+
+MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
+MAV_FRAME = { GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3}
+ALT_FRAME = { ABSOLUTE = 0, ABOVE_HOME = 1, ABOVE_ORIGIN = 2, ABOVE_TERRAIN = 3 }
+FLIGHT_MODE = {AUTO=10, RTL=11, LOITER=12, GUIDED=15, QHOVER=18, QLOITER=19, QRTL=21}
+
+PARAM_TABLE_KEY = 101
+PARAM_TABLE_PREFIX = "ZGT_"
+
+local now = millis():tofloat() * 0.001
+
+-- bind a parameter to a variable
+function bind_param(name)
+ return Parameter(name)
+end
+
+-- 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), SCRIPT_NAME_SHORT .. string.format('could not add param %s', name))
+ return bind_param(PARAM_TABLE_PREFIX .. name)
+end
+
+-- setup follow mode specific parameters
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), SCRIPT_NAME_SHORT .. 'could not add param table: ' .. PARAM_TABLE_PREFIX)
+
+--[[
+ // @Param: ZGT_MODE
+ // @DisplayName: Guided Terrain Mode
+ // @Description: When the GCS requests a guided altitude X above home 1: reset to current terrain height ignore X 2: reset to X above terrain 3: reset to current alt + X
+ // @Range: 1,2,3
+--]]
+ZGT_MODE = bind_add_param("MODE", 1, 1)
+TERRAIN_ENABLE = bind_param("TERRAIN_ENABLE")
+TERRAIN_FOLLOW = bind_param("TERRAIN_FOLLOW")
+
+local zgt_mode = ZGT_MODE:get()
+local terrain_enable = TERRAIN_ENABLE:get()
+local terrain_follow = TERRAIN_FOLLOW:get()
+
+MAV_CMD_INT = { DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192,
+ GUIDED_CHANGE_SPEED = 43000, GUIDED_CHANGE_ALTITUDE = 43001, GUIDED_CHANGE_HEADING = 43002 }
+MAV_FRAME = { GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3, GLOBAL_TERRAIN_ALT = 10}
+
+-- constrain a value between limits
+local function constrain(v, vmin, vmax)
+ if v < vmin then
+ v = vmin
+ end
+ if v > vmax then
+ v = vmax
+ end
+ return v
+end
+
+local function follow_frame_to_mavlink(follow_frame)
+ local mavlink_frame = MAV_FRAME.GLOBAL
+ if (follow_frame == ALT_FRAME.ABOVE_TERRAIN) then
+ mavlink_frame = MAV_FRAME.GLOBAL_TERRAIN_ALT
+ elseif (follow_frame == ALT_FRAME.ABOVE_HOME) then
+ mavlink_frame = MAV_FRAME.GLOBAL_RELATIVE_ALT
+ end
+ return mavlink_frame
+ end
+
+local now_altitude = millis():tofloat() * 0.001
+-- target.alt = new target altitude in meters
+-- set_vehicle_target_altitude() Parameters
+-- target.frame = Altitude frame MAV_FRAME, it's very important to get this right!
+-- target.alt = altitude in meters to acheive
+-- target.accel = z acceleration to altitude (1000.0 = max)
+local function set_vehicle_target_altitude(target)
+ local acceleration = target.accel or 1000.0 -- default to maximum z acceleration
+ if math.floor(now) ~= math.floor(now_altitude) then
+ now_altitude = millis():tofloat() * 0.001
+ end
+ if target.alt == nil then
+ gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": set_vehicle_target_altitude no altiude")
+ return
+ end
+ -- GUIDED_CHANGE_ALTITUDE takes altitude in meters
+ local mavlink_result = gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_ALTITUDE, {
+ frame = follow_frame_to_mavlink(target.frame),
+ p3 = acceleration,
+ z = target.alt })
+ if mavlink_result > 0 then
+ gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": MAVLink GUIDED_CHANGE_ALTITUDE returned %d", mavlink_result))
+ return false
+ else
+ return true
+ end
+end
+
+local vehicle_mode = vehicle:get_mode()
+local save_target_altitude = -1
+local save_old_target_altitude = -1
+local save_zgt_mode = -1
+local save_vehicle_mode = -1
+
+local function update()
+
+ vehicle_mode = vehicle:get_mode()
+
+ terrain_enable = TERRAIN_ENABLE:get()
+ terrain_follow = TERRAIN_FOLLOW:get()
+ zgt_mode = ZGT_MODE:get()
+ if zgt_mode ~= save_zgt_mode then
+ -- user changed modes, reset everything
+ save_target_altitude = -1
+ save_old_target_altitude = -1
+ end
+ save_zgt_mode = zgt_mode
+
+ -- We should only reset the altitude if newly switched to guided mode
+ if save_vehicle_mode ~= vehicle_mode and vehicle_mode == FLIGHT_MODE.GUIDED and terrain_enable == 1 and
+ ((terrain_follow & 1) == 1 or (terrain_follow & (1 << 6)) == 64) then
+ local target_location = vehicle:get_target_location()
+ if target_location ~= nil then
+ local target_location_frame = target_location:get_alt_frame()
+ -- need to convert the target_location to ABOVE_TERRAIN so we have apples and apples
+ local new_target_location = target_location:copy()
+ local new_target_altitude = save_target_altitude
+ local old_target_altitude = new_target_location:alt()/100
+
+ if save_old_target_altitude ~= old_target_altitude then
+ if target_location_frame ~= ALT_FRAME.ABOVE_TERRAIN then
+ if new_target_location:change_alt_frame(ALT_FRAME.ABOVE_TERRAIN) then
+ old_target_altitude = new_target_location:alt()/100
+ end
+ end
+ -- adjust target_location for home altitude
+ local home_location = ahrs:get_home()
+ if home_location ~= nil then
+ local home_amsl = terrain:height_amsl(home_location, true)
+ local above_home = (target_location:alt() * 0.01 - home_amsl)
+ local location = ahrs:get_location()
+ if location ~= nil then
+ if location:get_alt_frame() ~= ALT_FRAME.ABOVE_TERRAIN then
+ location:change_alt_frame(ALT_FRAME.ABOVE_TERRAIN)
+ end
+ local current_altitude = location:alt() * 0.01
+ -- @Description: When the GCS requests a guided altitude X above home
+ -- 1: reset to current terrain height ignore X
+ -- 2: reset to X above terrain
+ -- 3: reset to current alt + X
+ if zgt_mode == 1 then
+ new_target_altitude = current_altitude
+ elseif zgt_mode == 2 then
+ new_target_altitude = above_home
+ elseif zgt_mode == 3 then
+ new_target_altitude = current_altitude + above_home
+ end
+ new_target_altitude = constrain(new_target_altitude, ALTITUDE_MIN, ALTITUDE_MAX)
+ end
+ else
+ gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: FAILED to get HOME terrain height", SCRIPT_NAME_SHORT))
+ end
+
+ if new_target_altitude > 0 and
+ set_vehicle_target_altitude({alt = new_target_altitude, frame = ALT_FRAME.ABOVE_TERRAIN}) then -- pass altitude in meters (location has it in cm)
+ if new_target_altitude ~= save_target_altitude then
+ gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s: Reset to alt %.0fm above terrain", SCRIPT_NAME_SHORT,
+ new_target_altitude
+ ))
+ save_target_altitude = new_target_altitude
+ end
+ else
+ gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: FAILED to set altitude ABOVE_TERRAIN ait %.0f", SCRIPT_NAME_SHORT,
+ new_target_altitude
+ ))
+ end
+ end
+ save_old_target_altitude = old_target_altitude
+ else
+ gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: altitude not available", SCRIPT_NAME_SHORT))
+ end
+ else
+ -- we switched out of guided, so forget what we thought we knew
+ save_target_altitude = -1
+ save_old_target_altitude = -1
+ end
+ save_vehicle_mode = vehicle_mode
+
+ return update, 1000 * REFRESH_RATE
+end
+
+-- wrapper around update(). This calls update() at 1/REFRESHRATE Hz,
+-- and if update faults then an error is displayed, but the script is not
+-- stopped
+local function protected_wrapper()
+ local success, err = pcall(update)
+ if not success then
+ gcs:send_text(0, SCRIPT_NAME_SHORT .. ": 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, 1000 * REFRESH_RATE
+end
+
+gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) )
+
+-- start running update loop
+if FWVersion:type() == 3 and terrain_enable then
+ gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) )
+ return protected_wrapper()
+else
+ gcs:send_text(MAV_SEVERITY.NOTICE,string.format("%s: Must run on Plane with terrain follow", SCRIPT_NAME_SHORT))
+end