mirror of https://github.com/ArduPilot/ardupilot
AP_Scripting: example for use Location::AltFrame for guided_state.target_alt_frame
This commit is contained in:
timtuxworth
Peter Barker
parent
fdbbd320a4
commit
bbd9148b4b
|
|
@ -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
|
||||
Loading…
Reference in New Issue