AP_Scripting: added a VTOL failsafe example script

if fwd motor is lost try to make best effort to get home

libraries/AP_Scripting/examples/fw_vtol_failsafe.lua

@@ -0,0 +1,141 @@
+-- This script allows for best effort return for quadplanes
+-- on loss of forward motor
+
+-- When an engine failure is detected (RPM and Vibration values drop below a certain threshold), the aircraft will:
+-- Send a warning ground station
+-- Start prioritizing airspeed completely
+-- Glide back towards the home location or rally point, monitoring the distance away from the point as well as Altitude.
+-- If the aircraft drops below a predetermined minimum altitude, QLAND mode is engaged and the aircraft lands at its current position.
+-- If the aircraft arrives within Q_FW_LND_APR_RAD of the return point before dropping below the minimum altitude, it should loiter down to the minimum altitude before switching to QRTL and landing.
+
+-- consider engine stopped when vibe is low and RPM low for more than 4s
+local ENGINE_STOPPED_MS = 4000
+
+-- RPM threshold below which engine may be stopped
+local RPM_LOW_THRESH = 500
+
+-- vibration threshold below which engine may be stopped
+local VIBE_LOW_THRESH = 2
+
+-- altitude threshold for QLAND
+local LOW_ALT_THRESH = 70
+
+-- when below MIN_AIRSPEED assume we are not in forward flight
+local MIN_AIRSPEED = 5
+
+-- time when engine stopped
+local engine_stop_ms = -1
+local engine_stopped = false
+
+-- have we triggered failsafe? Only trigger once
+local triggered_failsafe = false
+
+-- flight mode numbers for plane
+local MODE_AUTO = 10
+local MODE_RTL = 11
+local MODE_QLAND = 20
+local MODE_QRTL = 21
+
+-- update engine running status
+function check_engine()
+  if not arming:is_armed() then
+     engine_stopped = false
+     engine_stop_ms = -1
+     return true
+  end
+
+  local rpm = RPM:get_rpm(0)
+  local vibe = ahrs:get_vibration():length()
+
+  -- if either RPM is high or vibe is high then assume engine is running
+  if (rpm and (rpm > RPM_LOW_THRESH)) or (vibe > VIBE_LOW_THRESH) then
+     -- engine is definately running
+     engine_stop_ms = -1
+     if engine_stopped then
+        -- notify user engine has started
+        gcs:send_text(0, "Engine: STARTED")
+        engine_stopped = false
+     end
+     return true
+  end
+  local now = millis()
+
+  if engine_stop_ms == -1 then
+     -- start timeout period
+     engine_stop_ms = now
+     return true
+  end
+  if now - engine_stop_ms < ENGINE_STOPPED_MS then
+     return false
+  end
+  -- engine has been stopped for ENGINE_STOPPED_MS milliseconds, notify user
+  if not engine_stopped then
+     engine_stopped = true
+     gcs:send_text(0, "Engine: STOPPED")
+  end
+  return engine_stopped
+end
+
+-- trigger failsafe function
+function trigger_failsafe()
+  if param:get('RTL_AUTOLAND') == 2 then
+     -- we don't want to do the mission based autoland, so disable
+     -- RTL_AUTOLAND
+     param:set('RTL_AUTOLAND', 0)
+  end
+  if param:get('TECS_SPDWEIGHT') < 2 then
+     -- force airspeed priority
+     param:set('TECS_SPDWEIGHT', 2)
+  end
+  -- trigger an RTL to start bringing the vehicle home
+  -- it will automatically go to the nearest rally point if set or go to home
+  -- if no rally point available within the RALLY_LIMIT_KM
+  vehicle:set_mode(MODE_RTL)
+end
+
+-- check if we should switch to QLAND
+function check_qland()
+  local target = vehicle:get_target_location()
+  local pos = ahrs:get_position()
+  if not target or not pos then
+    -- we can't estimate distance
+    return
+  end
+  local dist = target:get_distance(pos)
+  local terrain_height = terrain:height_above_terrain(true)
+  local threshold = param:get('Q_FW_LND_APR_RAD')
+  if dist < threshold and (terrain_height and (terrain_height < LOW_ALT_THRESH)) then
+    gcs:send_text(0, "Failsafe: LANDING QRTL")
+    vehicle:set_mode(MODE_QRTL)
+  elseif terrain_height and terrain_height < LOW_ALT_THRESH then
+    gcs:send_text(0, "Failsafe: LANDING QLAND")
+    vehicle:set_mode(MODE_QLAND)
+  end
+end
+
+function update()
+  -- check engine status
+  check_engine()
+
+  -- if armed and in AUTO mode then consider triggering failsafe
+  if engine_stopped and vehicle:get_mode() == MODE_AUTO and arming:is_armed() and not triggered_failsafe then
+    triggered_failsafe = true
+    trigger_failsafe()
+    gcs:send_text(0, "Failsafe: TRIGGERED")
+  end
+
+  if not engine_stopped and triggered_failsafe then
+    triggered_failsafe = false
+    gcs:send_text(0, "Failsafe: RECOVERED")
+  end
+
+  if triggered_failsafe and vehicle:get_mode() == MODE_RTL then
+    check_qland()
+  end
+
+  -- run at 5Hz
+  return update, 200
+end
+
+-- start running update loop
+return update()