mirror of https://github.com/ArduPilot/ardupilot
91 lines
2.5 KiB
Lua
91 lines
2.5 KiB
Lua
|
-- 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()
|
||
|
|
|