mirror of https://github.com/ArduPilot/ardupilot
131 lines
3.6 KiB
Lua
131 lines
3.6 KiB
Lua
--[[
|
|
script to prevent terrain impact in LOITER mode while flying copters in steep terrain
|
|
--]]
|
|
|
|
local PARAM_TABLE_KEY = 84
|
|
local PARAM_TABLE_PREFIX = "TERR_BRK_"
|
|
|
|
local MODE_LOITER = 5
|
|
local MODE_BRAKE = 17
|
|
|
|
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
|
|
|
|
-- 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 Parameter(PARAM_TABLE_PREFIX .. name)
|
|
end
|
|
|
|
-- setup script specific parameters
|
|
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 14), 'could not add param table')
|
|
|
|
--[[
|
|
// @Param: TERR_BRK_ENABLE
|
|
// @DisplayName: terrain brake enable
|
|
// @Description: terrain brake enable
|
|
// @Values: 0:Disabled,1:Enabled
|
|
// @User: Standard
|
|
--]]
|
|
local TERR_BRK_ENABLE = bind_add_param('ENABLE', 1, 1)
|
|
|
|
--[[
|
|
// @Param: TERR_BRK_ALT
|
|
// @DisplayName: terrain brake altitude
|
|
// @Description: terrain brake altitude. The altitude above the ground below which BRAKE mode will be engaged if in LOITER mode.
|
|
// @Range: 1 100
|
|
// @Units: m
|
|
// @User: Standard
|
|
--]]
|
|
local TERR_BRK_ALT = bind_add_param('ALT', 2, 30)
|
|
|
|
--[[
|
|
// @Param: TERR_BRK_HDIST
|
|
// @DisplayName: terrain brake home distance
|
|
// @Description: terrain brake home distance. The distance from home where the auto BRAKE will be enabled. When within this distance of home the script will not activate
|
|
// @Range: 0 1000
|
|
// @Units: m
|
|
// @User: Standard
|
|
--]]
|
|
local TERR_BRK_HDIST = bind_add_param('HDIST', 3, 100)
|
|
|
|
--[[
|
|
// @Param: TERR_BRK_SPD
|
|
// @DisplayName: terrain brake speed threshold
|
|
// @Description: terrain brake speed threshold. Don't trigger BRAKE if both horizontal speed and descent rate are below this threshold. By setting this to a small value this can be used to allow the user to climb up to a safe altitude in LOITER mode. A value of 0.5 is recommended if you want to use LOITER to recover from an emergency terrain BRAKE mode change.
|
|
// @Range: 0 5
|
|
// @Units: m/s
|
|
// @User: Standard
|
|
--]]
|
|
local TERR_BRK_SPD = bind_add_param('SPD', 4, 0)
|
|
|
|
local function sq(x)
|
|
return x*x
|
|
end
|
|
|
|
local function run_checks()
|
|
if TERR_BRK_ENABLE:get() ~= 1 then
|
|
return
|
|
end
|
|
if not arming:is_armed() then
|
|
return
|
|
end
|
|
if vehicle:get_mode() ~= MODE_LOITER then
|
|
return
|
|
end
|
|
|
|
if not ahrs:home_is_set() then
|
|
return
|
|
end
|
|
local home = ahrs:get_home()
|
|
local pos = ahrs:get_location()
|
|
if not pos then
|
|
return
|
|
end
|
|
local home_dist = pos:get_distance(home)
|
|
if home_dist <= TERR_BRK_HDIST:get() then
|
|
return
|
|
end
|
|
|
|
--[[
|
|
get height above terrain with extrapolation
|
|
--]]
|
|
local hagl = terrain:height_above_terrain(true)
|
|
if hagl >= TERR_BRK_ALT:get() then
|
|
return
|
|
end
|
|
|
|
--[[
|
|
allow for climbing in LOITER mode if enabled
|
|
--]]
|
|
if TERR_BRK_SPD:get() > 0 then
|
|
local spd = ahrs:get_velocity_NED()
|
|
if spd ~= nil then
|
|
local hspd = math.sqrt(sq(spd:x())+sq(spd:y()))
|
|
local drate = spd:z()
|
|
if hspd < TERR_BRK_SPD:get() and drate < TERR_BRK_SPD:get() then
|
|
return
|
|
end
|
|
end
|
|
end
|
|
|
|
if vehicle:set_mode(MODE_BRAKE) then
|
|
gcs:send_text(MAV_SEVERITY.EMERGENCY, string.format("Terrain %.1fm - BRAKE", hagl))
|
|
end
|
|
end
|
|
|
|
--[[
|
|
main update function, called at 1Hz
|
|
--]]
|
|
function update()
|
|
run_checks()
|
|
return update, 100
|
|
end
|
|
|
|
if TERR_BRK_ENABLE:get() == 1 then
|
|
gcs:send_text(MAV_SEVERITY.INFO, string.format("Loaded Loiter/Brake checker"))
|
|
end
|
|
|
|
-- start running update loop
|
|
return update, 1000
|
|
|