mirror of https://github.com/ArduPilot/ardupilot
85 lines
2.8 KiB
Lua
85 lines
2.8 KiB
Lua
|
--[[
|
||
|
|
script to auto-deploy a vehicle on descent after reaching a specified altitude
|
||
|
|
uses raw pressure to not depend on either GPS or on home alt
|
||
|
|
--]]
|
||
|
|
local PARAM_TABLE_KEY = 72
|
||
|
|
local PARAM_TABLE_PREFIX = "DEPL_"
|
||
|
|
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
|
||
|
|
|
||
|
|
---@diagnostic disable: missing-parameter
|
||
|
|
---@diagnostic disable: param-type-mismatch
|
||
|
|
|
||
|
|
-- 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
|
||
|
|
|
||
|
|
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add param table')
|
||
|
|
|
||
|
|
local end_pos = bind_add_param('OPEN_POS', 1, 1200)
|
||
|
|
local offset = bind_add_param('OPEN_OFFSET', 2, 400)
|
||
|
|
local deployment_alt = bind_add_param('ALT', 3, 1)
|
||
|
|
local DEPL_CLIMB_ALT = bind_add_param('CLIMB_ALT', 4, 2)
|
||
|
|
local base_pressure = nil
|
||
|
|
local SERVO_FUNCTION = 94
|
||
|
|
local reached_climb_alt = false
|
||
|
|
local deployed = false
|
||
|
|
|
||
|
|
local MODE_LAND = 9
|
||
|
|
|
||
|
|
function update_state()
|
||
|
|
local pressure = baro:get_pressure()
|
||
|
|
if not pressure then
|
||
|
|
return
|
||
|
|
end
|
||
|
|
if not base_pressure then
|
||
|
|
base_pressure = pressure
|
||
|
|
end
|
||
|
|
local altitude = baro:get_altitude_difference(base_pressure, pressure)
|
||
|
|
if not altitude then
|
||
|
|
return
|
||
|
|
end
|
||
|
|
gcs:send_named_float('DALT',altitude)
|
||
|
|
logger.write("DEPL",'BP,P,Alt','fff',
|
||
|
|
base_pressure, pressure, altitude)
|
||
|
|
if not reached_climb_alt then
|
||
|
|
local start_pos = end_pos:get() + offset:get()
|
||
|
|
SRV_Channels:set_output_pwm(SERVO_FUNCTION, start_pos)
|
||
|
|
if altitude > DEPL_CLIMB_ALT:get() then
|
||
|
|
reached_climb_alt = true
|
||
|
|
gcs:send_text(MAV_SEVERITY.ERROR, "DEPL: Reached climb alt")
|
||
|
|
end
|
||
|
|
return
|
||
|
|
end
|
||
|
|
if deployed then
|
||
|
|
return
|
||
|
|
end
|
||
|
|
if altitude > deployment_alt:get() then
|
||
|
|
return
|
||
|
|
end
|
||
|
|
deployed = true
|
||
|
|
gcs:send_text(MAV_SEVERITY.INFO, "DEPL: deploying")
|
||
|
|
vehicle:set_mode(MODE_LAND)
|
||
|
|
arming:arm_force()
|
||
|
|
if not vehicle:get_mode() == MODE_LAND or not arming:is_armed() then
|
||
|
|
gcs:send_text(MAV_SEVERITY.INFO, "DEPL: Arming failed")
|
||
|
|
return
|
||
|
|
end
|
||
|
|
--SRV_Channels:set_output_pwm(SERVO_FUNCTION, end_pos:get())
|
||
|
|
gcs:send_text(MAV_SEVERITY.INFO, "DEPL: Deployed successfully")
|
||
|
|
end
|
||
|
|
function update() -- this is the loop which periodically runs
|
||
|
|
update_state()
|
||
|
|
return update, 20 -- Reschedules the loop at 50Hz
|
||
|
|
end
|
||
|
|
gcs:send_text(MAV_SEVERITY.INFO, "DEPL: loaded")
|
||
|
|
return update() -- Run immediately before starting to reschedule
|