ardupilot/libraries/AP_Scripting/examples/plane_guided_terrain.lua

235 lines
9.7 KiB
Lua

--[[
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