mirror of https://github.com/ArduPilot/ardupilot
AP_Scripting: added copter_loiter_brake script
for automatic BRAKE mode when flying in LOITER in steep terrain
This commit is contained in:
Andrew Tridgell
parent
c9f4c47b42
commit
bae5859a92
|
|
@ -0,0 +1,130 @@
|
|||
--[[
|
||||
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
|
||||
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
# Copter Loiter Brake
|
||||
|
||||
This script implements an emergency change to BRAKE mode in copter if
|
||||
you are in LOITER mode and break a terrain altitude limit. The script
|
||||
is useful when flying in LOITER mode in steep terrain.
|
||||
|
||||
# Parameters
|
||||
|
||||
The script adds the following parameters to control it's behaviour.
|
||||
|
||||
## TERR_BRK_ENABLE
|
||||
|
||||
This must be set to 1 to enable the script.
|
||||
|
||||
## TERR_BRK_ALT
|
||||
|
||||
This is the terrain altitude threshold for engaging BRAKE mode. The
|
||||
onboard terrain system must be enabled with TERRAIN_ENABLE=1 and
|
||||
terrain must have either been preloaded to the vehicle (see
|
||||
https://terrain.ardupilot.org ) or be available from the ground
|
||||
station over MAVLink.
|
||||
|
||||
Make sure you set sufficient margin to cope with obstacles such as
|
||||
trees or any local towers or other obstacles.
|
||||
|
||||
## TERR_BRK_HDIST
|
||||
|
||||
This is the distance from home for the BRAKE checking to be
|
||||
enabled. The default of 100 meters is good for most operations. This
|
||||
threshold allows you to take over in LOITER mode for low altitude
|
||||
operations and takeoff/landing when close to home.
|
||||
|
||||
## TERR_BRK_SPD
|
||||
|
||||
This is a speed threshold BRAKE checking to be enabled. If both the
|
||||
horizontal speed and the descent rate are below this threshold then
|
||||
BRAKE will not be engaged. This defaults to zero which means no speed
|
||||
checking is performed.
|
||||
|
||||
You should set this to a small value if you want to be able to recover
|
||||
from BRAKE mode by climbing straight up in LOITER mode. A value of 0.5
|
||||
m/s is recommended. The value needed will be dependent on the amount
|
||||
of noise there is in your velocity measurement and how gusty the wind
|
||||
is, but 0.5 should work in most applications.
|
||||
|
||||
If you set this value then to recover in LOITER mode you should raise
|
||||
the throttle stick to demand climb before you switch back to LOITER
|
||||
mode. The positive climb rate means BRAKE will not re-engage.
|
||||
|
||||
# Operation
|
||||
|
||||
Install the lua script in the APM/SCRIPTS directory on the flight
|
||||
controllers microSD card. Review the above parameter descriptions and
|
||||
decide on the right parameter values for your vehicle and operations.
|
||||
|
||||
Make sure TERRAIN_ENABLE is 1 and you should preload terrain data for
|
||||
the flight area from https://terrain.ardupilot.org
|
||||
|
||||
It is strongly recommended that you set TERRAIN_SPACING=30 and preload
|
||||
the SRTM1 terrain data for 30m horizontal resolution of terrain data.
|
||||
|
||||
When the system engages you will see a message like this
|
||||
"Terrain 29.2m - BRAKE"
|
||||
where in this example you are 29.2m above the terrain.
|
||||
|
||||
To recover you could use GUIDED mode, or RTL (make sure you have set
|
||||
RTL_ALT_TYPE to terrain) or if you have set TERR_BRK_SPD to a positive
|
||||
value then you could raise the throttle stick and switch back to
|
||||
LOITER mode.
|
||||
|
||||
If the system is continually giving false positives then set
|
||||
TERR_BRK_ENABLE to zero to disable.
|
||||
Loading…
Reference in New Issue