mirror of https://github.com/ArduPilot/ardupilot
142 lines
4.3 KiB
Lua
142 lines
4.3 KiB
Lua
-- 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()
|