AP_Scripting: added copter_loiter_brake script

for automatic BRAKE mode when flying in LOITER in steep terrain

libraries/AP_Scripting/applets/copter_terrain_brake.lua

@@ -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
+

libraries/AP_Scripting/applets/copter_terrain_brake.md

@@ -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.