ardupilot/libraries/AP_Scripting/examples/ship_vel_match.lua

91 lines
2.5 KiB
Lua
Raw Normal View History

-- support takeoff with velocity matching for quadplanes
local PARAM_TABLE_KEY = 35
local PARAM_TABLE_PREFIX = "SHIPV_"
local MODE_AUTO = 10
local NAV_TAKEOFF = 22
local NAV_VTOL_TAKEOFF = 84
local ALT_FRAME_ABSOLUTE = 0
-- bind a parameter to a variable
function bind_param(name)
local p = Parameter()
assert(p:init(name), string.format('could not find %s parameter', name))
return p
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), string.format('could not add param %s', name))
return bind_param(PARAM_TABLE_PREFIX .. name)
end
-- setup SHIPV specific parameters
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), 'could not add param table')
SHIPV_ENABLE = bind_add_param('ENABLE', 1, 0)
local takeoff_vel = nil
local takeoff_pos = nil
-- main update function
function update()
if SHIPV_ENABLE:get() < 1 then
return
end
local vehicle_mode = vehicle:get_mode()
if not arming:is_armed() then
-- when not armed record position and velocity
takeoff_vel = ahrs:get_velocity_NED()
takeoff_pos = ahrs:get_position()
takeoff_pos:change_alt_frame(ALT_FRAME_ABSOLUTE)
else
if vehicle_mode == MODE_AUTO and takeoff_pos and takeoff_vel then
local id = mission:get_current_nav_id()
if id == NAV_VTOL_TAKEOFF or id == NAV_TAKEOFF then
local next_WP = vehicle:get_target_location()
if not next_WP then
return
end
vehicle:set_velocity_match(takeoff_vel:xy())
local tpos = takeoff_pos:copy()
tpos:alt(next_WP:alt())
vehicle:update_target_location(next_WP, tpos)
else
takeoff_pos = nil
takeoff_vel = nil
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(0, "Loaded quadplane takeoff velmatch")
-- start running update loop
return protected_wrapper()