mirror of https://github.com/ArduPilot/ardupilot
92 lines
2.4 KiB
Lua
92 lines
2.4 KiB
Lua
|
--[[
|
||
|
|
add ALT_OFFSET parameter for copter
|
||
|
|
This behaves similarly to ALT_OFFSET in plane. It operators only in AUTO mode, and slews the BARO_ALT_OFFSET to allow
|
||
|
|
for change of altitude without mission change
|
||
|
|
--]]
|
||
|
|
|
||
|
|
|
||
|
|
local PARAM_TABLE_KEY = 79
|
||
|
|
local PARAM_TABLE_PREFIX = "ALT_"
|
||
|
|
|
||
|
|
local MODE_AUTO = 3
|
||
|
|
|
||
|
|
local LOOP_RATE = 50
|
||
|
|
|
||
|
|
-- 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 SHIP specific parameters
|
||
|
|
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 2), 'could not add param table')
|
||
|
|
ALT_OFFSET = bind_add_param('OFFSET', 1, 0)
|
||
|
|
ALT_OFFSET_RT = bind_add_param('OFFSET_RT', 2, 1)
|
||
|
|
BARO_ALT_OFFSET = bind_param("BARO_ALT_OFFSET")
|
||
|
|
|
||
|
|
local vehicle_mode = nil
|
||
|
|
|
||
|
|
-- constrain a value between limits
|
||
|
|
function constrain(v, vmin, vmax)
|
||
|
|
if v < vmin then
|
||
|
|
v = vmin
|
||
|
|
end
|
||
|
|
if v > vmax then
|
||
|
|
v = vmax
|
||
|
|
end
|
||
|
|
return v
|
||
|
|
end
|
||
|
|
|
||
|
|
-- main update function
|
||
|
|
function update()
|
||
|
|
vehicle_mode = vehicle:get_mode()
|
||
|
|
|
||
|
|
if vehicle_mode ~= MODE_AUTO then
|
||
|
|
BARO_ALT_OFFSET:set(0)
|
||
|
|
return
|
||
|
|
end
|
||
|
|
|
||
|
|
if not arming:is_armed() then
|
||
|
|
-- zero offset when disarmed
|
||
|
|
BARO_ALT_OFFSET:set(0)
|
||
|
|
ALT_OFFSET:set(0)
|
||
|
|
end
|
||
|
|
|
||
|
|
local target = ALT_OFFSET:get() * -1.0
|
||
|
|
|
||
|
|
local current = BARO_ALT_OFFSET:get()
|
||
|
|
local delta = target - current
|
||
|
|
local dt = 1.0 / LOOP_RATE
|
||
|
|
local delta_max = ALT_OFFSET_RT:get() * dt
|
||
|
|
|
||
|
|
delta = constrain(delta, -delta_max, delta_max)
|
||
|
|
BARO_ALT_OFFSET:set(current + delta)
|
||
|
|
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, 1000/LOOP_RATE
|
||
|
|
end
|
||
|
|
|
||
|
|
gcs:send_text(0, "ALT_OFFSET handler loaded")
|
||
|
|
|
||
|
|
-- start running update loop
|
||
|
|
return protected_wrapper()
|
||
|
|
|