mirror of https://github.com/ArduPilot/ardupilot
AP_Scripting: add glide-into-wind example without RC override
This commit is contained in:
Andreas Gising
Peter Barker
parent
f5d81abb42
commit
300e5ccedd
|
|
@ -0,0 +1,399 @@
|
|||
-- Glide into wind, LUA script for glide into wind functionality
|
||||
|
||||
-- Background
|
||||
-- When flying a fixed-wing drone on ad-hoc BVLOS missions, it might not be
|
||||
-- suitable for the drone to return to home if the C2 link is lost, since that
|
||||
-- might mean flying without control for an extended time and distance. One
|
||||
-- option in ArduPlane is to set FS_Long to Glide, which makes the drone glide
|
||||
-- and land in the direction it happened to have when the command was invoked,
|
||||
-- without regard to the wind. This script offers a way to decrease the kinetic
|
||||
-- energy in this blind landing by means of steering the drone towards the wind
|
||||
-- as GLIDE is initiated, hence lowering the ground speed. The intention is to
|
||||
-- minimize impact energy at landing - foremost for any third party, but also to
|
||||
-- minimize damage to the drone.
|
||||
|
||||
-- Functionality and setup
|
||||
-- 1. Set SCR_ENABLE = 1
|
||||
-- 2. Put script in scripts folder, boot twice
|
||||
-- 3. A new parameter has appeared:
|
||||
-- - GLIDE_WIND_ENABL (0=disable, 1=enable)
|
||||
-- 4. Set GLIDE_WIND_ENABL = 1
|
||||
-- 5. Read the docs on FS:
|
||||
-- https://ardupilot.org/plane/docs/apms-failsafe-function.html#failsafe-parameters-and-their-meanings
|
||||
-- 6. Set FS_LONG_ACTN = 2
|
||||
-- 7. Set FS_LONG_TIMEOUT as appropriate
|
||||
-- 8. Set FS_GCS_ENABL = 1
|
||||
-- 9. If in simulation, set SIM_WIND_SPD = 4 to get a reliable wind direction.
|
||||
-- 10. Test in simulation: Fly a mission, disable heartbeats by typing 'set
|
||||
-- heartbeat 0' into mavproxy/SITL, monitor what happens in the console. If
|
||||
-- QGC or similar GCS is used, make sure it does not send heartbeats.
|
||||
-- 11. Test in flight: Fly a mission, monitor estimated wind direction from GCS,
|
||||
-- then fail GCS link and see what happens.
|
||||
-- 12. Once heading is into wind script will stop steering and not steer again
|
||||
-- until state machine is reset and failsafe is triggered again. Steering in low
|
||||
-- airspeeds (thr=0) increases risks of stall and it is preferable touch
|
||||
-- ground in level roll attitude. If the script parameter hdg_ok_lim is set
|
||||
-- to tight or the wind estimate is not stable, the script will anyhow stop
|
||||
-- steering after override_time_lim and enter FBWA - otherwise the script
|
||||
-- would hinder the GLIDE fail safe.
|
||||
-- 13. Script will stop interfering as soon as a new goto-point is received or
|
||||
-- the flight mode is changed by the operator or the remote pilot.
|
||||
|
||||
-- During the fail safe maneuver a warning tune is played.
|
||||
|
||||
-- State machine
|
||||
-- CAN_TRIGGER
|
||||
-- - Do: Nothing
|
||||
-- - Change state: If the failsafe GLIDE is triggered: if FS_GCS_ENABL is set
|
||||
-- and FS_LONG_ACTN is 2, change to TRIGGERED else change to CANCELED
|
||||
--
|
||||
-- TRIGGERED
|
||||
-- - Do: First use GUIDED mode to steer into wind, then switch to FBWA to
|
||||
-- Glide into wind. Play warning tune.
|
||||
-- - Change state: If flight mode is changed by operator/remote pilot or
|
||||
-- operator/remote pilot sends a new goto point, change state to CANCELED
|
||||
--
|
||||
-- CANCELED
|
||||
-- - Do: Nothing
|
||||
-- - Change state: When new heart beat arrive, change state to CAN_TRIGGER
|
||||
|
||||
-- Credits
|
||||
-- This script is developed by agising at UASolutions, commissioned by, and in
|
||||
-- cooperation with Remote.aero, with funding from Swedish AeroEDIH, in response
|
||||
-- to a need from the Swedish Sea Rescue Society (Sjöräddningssällskapet, SSRS).
|
||||
|
||||
-- Disable diagnostics related to reading parameters to pass linter
|
||||
---@diagnostic disable: need-check-nil
|
||||
---@diagnostic disable: param-type-mismatch
|
||||
|
||||
-- Tuning parameters
|
||||
local looptime = 250 -- Short looptime
|
||||
local long_looptime = 2000 -- Long looptime, GLIDE_WIND is not enabled
|
||||
local tune_repeat_t = 1000 -- How often to play tune in glide into wind, [ms]
|
||||
local hdg_ok_lim = 15 -- Acceptable heading error in deg (when to stop steering)
|
||||
local hdg_ok_t_lim = 5000 -- Stop steering towards wind after hdg_ok_t_lim ms with error less than hdg_ok_lim
|
||||
local override_time_lim = 15000 -- Max time in GUIDED during GLIDE, after limit set FBWA independent of hdg
|
||||
|
||||
-- GCS text levels
|
||||
local _INFO = 6
|
||||
local _WARNING = 4
|
||||
|
||||
-- Plane flight modes mapping
|
||||
local mode_FBWA = 5
|
||||
local mode_GUIDED = 15
|
||||
|
||||
-- Tunes
|
||||
local _tune_glide_warn = "MFT240 L16 cdefgfgfgfg" -- The warning tone played during GLIDE_WIND
|
||||
|
||||
--State variable
|
||||
local fs_state = nil
|
||||
|
||||
-- Flags
|
||||
local override_enable = false -- Flag to allow RC channel loverride
|
||||
|
||||
-- Variables
|
||||
local wind_dir_rad = nil -- param for wind dir in rad
|
||||
local wind_dir_180 = nil -- param for wind dir in deg
|
||||
local hdg_error = nil -- Heading error, hdg vs wind_dir_180
|
||||
local gw_enable = nil -- glide into wind enable flag
|
||||
local hdg = nil -- vehicle heading
|
||||
local wind = Vector3f() -- wind 3Dvector
|
||||
local link_lost_for = nil -- link loss time counter
|
||||
local last_seen = nil -- timestamp last received heartbeat
|
||||
local tune_time_since = 0 -- Timer for last played tune
|
||||
local hdg_ok_t = 0 -- Timer
|
||||
local expected_flight_mode = nil -- Flight mode set by this script
|
||||
local location_here = nil -- Current location
|
||||
local location_upwind = nil -- Location to hold the target location
|
||||
local user_notified = false -- Flag to keep track user being notified or not
|
||||
local failed_location_counter = 0 -- Counter for failed location requests, possible GPS denied
|
||||
local upwind_distance = 500 -- Distance to the upwind location, minimum 4x turn radius
|
||||
local override_time = 0 -- Time since override started in ms
|
||||
|
||||
-- Add param table
|
||||
local PARAM_TABLE_KEY = 74
|
||||
local PARAM_TABLE_PREFIX = "GLIDE_WIND_"
|
||||
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 30), 'could not add param table')
|
||||
|
||||
-------
|
||||
-- Init
|
||||
-------
|
||||
|
||||
function _init()
|
||||
-- Add and init paramters
|
||||
GLIDE_WIND_ENABL = bind_add_param('ENABL', 1, 0)
|
||||
|
||||
-- Init parameters
|
||||
FS_GCS_ENABL = bind_param('FS_GCS_ENABL') -- Is set to 1 if GCS lol should trigger FS after FS_LONG_TIMEOUT
|
||||
FS_LONG_TIMEOUT = bind_param('FS_LONG_TIMEOUT') -- FS long timeout in seconds
|
||||
FS_LONG_ACTN = bind_param('FS_LONG_ACTN') -- Is set to 2 for Glide
|
||||
|
||||
send_to_gcs(_INFO, 'LUA: FS_LONG_TIMEOUT timeout: ' .. FS_LONG_TIMEOUT:get() .. 's')
|
||||
|
||||
-- Test paramter
|
||||
if GLIDE_WIND_ENABL:get() == nil then
|
||||
send_to_gcs(_INFO, 'LUA: Something went wrong, GLIDE_WIND_ENABL not created')
|
||||
return _init(), looptime
|
||||
else
|
||||
gw_enable = GLIDE_WIND_ENABL:get()
|
||||
send_to_gcs(_INFO, 'LUA: GLIDE_WIND_ENABL: ' .. gw_enable)
|
||||
end
|
||||
|
||||
-- Init last_seen.
|
||||
last_seen = gcs:last_seen()
|
||||
|
||||
-- Init link_lost_for [ms] to FS_LONG_TIMEOUT [s] to prevent link to recover without
|
||||
-- new heartbeat. This is to properly init the state machine.
|
||||
link_lost_for = FS_LONG_TIMEOUT:get() * 1000
|
||||
|
||||
-- Warn if GLIDE_WIND_ENABL is set and FS_LONG_ACTN is not GLIDE
|
||||
if gw_enable == 1 and FS_LONG_ACTN:get() ~= 2 then
|
||||
send_to_gcs(_WARNING, 'GLIDE_WIND_ENABL is set, but FS_LONG_ACTN is not GLIDE.')
|
||||
end
|
||||
|
||||
-- Init fs_state machine to CANCELED. A heartbeat is required to set the state
|
||||
-- to CAN_TRIGGER from where Glide into wind can be triggered.
|
||||
fs_state = 'CANCELED'
|
||||
|
||||
-- All set, go to update
|
||||
return update(), long_looptime
|
||||
end
|
||||
|
||||
|
||||
------------
|
||||
-- Main loop
|
||||
------------
|
||||
|
||||
function update()
|
||||
-- Check if state of GLIDE_WIND_ENABL parameter changed, print every change
|
||||
if gw_enable ~= GLIDE_WIND_ENABL:get() then
|
||||
gw_enable = GLIDE_WIND_ENABL:get()
|
||||
send_to_gcs(_INFO, 'LUA: GLIDE_WIND_ENABL: ' .. gw_enable)
|
||||
-- If GLIDE_WIND_ENABL was enabled, warn if not FS_LONG_ACTN is set accordingly
|
||||
if gw_enable == 1 then
|
||||
if FS_LONG_ACTN:get() ~=2 then
|
||||
send_to_gcs(_WARNING, 'GLIDE_WIND_ENABL is set, but FS_LONG_ACTN is not GLIDE.')
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
-- -- If feature is not enabled, loop slowly
|
||||
if gw_enable == 0 then
|
||||
return update, long_looptime
|
||||
end
|
||||
|
||||
-- GLIDE_WIND_ENABL is enabled, look for triggers
|
||||
-- Monitor time since last gcs heartbeat
|
||||
if last_seen == gcs:last_seen() then
|
||||
link_lost_for = link_lost_for + looptime
|
||||
else
|
||||
-- There has been a new heartbeat, update last_seen and reset link_lost_for
|
||||
last_seen = gcs:last_seen()
|
||||
link_lost_for = 0
|
||||
end
|
||||
|
||||
-- Run the state machine
|
||||
-- State CAN_TRIGGER
|
||||
if fs_state == 'CAN_TRIGGER' then
|
||||
if link_lost_for > FS_LONG_TIMEOUT:get() * 1000 then
|
||||
-- Double check that FS_GCS_ENABL is set
|
||||
if FS_GCS_ENABL:get() == 1 and FS_LONG_ACTN:get() == 2 then
|
||||
fs_state = "TRIGGERED"
|
||||
-- Reset some variables
|
||||
hdg_ok_t = 0
|
||||
user_notified = false
|
||||
override_enable = true
|
||||
override_time = 0
|
||||
failed_location_counter = 0
|
||||
-- Set mode to GUIDED before entering TRIGGERED state
|
||||
set_flight_mode(mode_GUIDED, 'LUA: Glide into wind state TRIGGERED')
|
||||
else
|
||||
-- Do not trigger glide into wind, require new heart beats to get here again
|
||||
fs_state = "CANCELED"
|
||||
end
|
||||
end
|
||||
-- State TRIGGERED
|
||||
elseif fs_state == "TRIGGERED" then
|
||||
-- Check for flight mode changes from outside script
|
||||
if vehicle:get_mode() ~= expected_flight_mode then
|
||||
fs_state = "CANCELED"
|
||||
send_to_gcs(_INFO, 'LUA: Glide into wind state CANCELED: flight mode change')
|
||||
end
|
||||
|
||||
-- In GUIDED, check for target location changes from outside script (operator)
|
||||
if vehicle:get_mode() == mode_GUIDED then
|
||||
if not locations_are_equal(vehicle:get_target_location(), location_upwind) then
|
||||
fs_state = "CANCELED"
|
||||
send_to_gcs(_INFO, 'LUA: Glide into wind state CANCELED: new goto-point')
|
||||
end
|
||||
end
|
||||
|
||||
-- State CANCELED
|
||||
elseif fs_state == "CANCELED" then
|
||||
-- Await link is not lost
|
||||
if link_lost_for < FS_LONG_TIMEOUT:get() * 1000 then
|
||||
fs_state = "CAN_TRIGGER"
|
||||
send_to_gcs(_INFO, 'LUA: Glide into wind state CAN_TRIGGER')
|
||||
end
|
||||
end
|
||||
|
||||
-- State TRIGGERED actions
|
||||
if fs_state == "TRIGGERED" then
|
||||
-- Get the heading angle
|
||||
hdg = math.floor(math.deg(ahrs:get_yaw()))
|
||||
|
||||
-- Get wind direction. Function wind_estimate returns x and y for direction wind blows in, add pi to get true wind dir
|
||||
wind = ahrs:wind_estimate()
|
||||
wind_dir_rad = math.atan(wind:y(), wind:x())+math.pi
|
||||
wind_dir_180 = math.floor(wrap_180(math.deg(wind_dir_rad)))
|
||||
hdg_error = wrap_180(wind_dir_180 - hdg)
|
||||
|
||||
-- Check if we are close to target heading
|
||||
if math.abs(hdg_error) < hdg_ok_lim then
|
||||
-- If we have been close to target heading for hdg_ok_t_lim, switch back to FBWA
|
||||
if hdg_ok_t > hdg_ok_t_lim then
|
||||
if override_enable then
|
||||
set_flight_mode(mode_FBWA,'LUA: Glide into wind steering complete, GLIDE in FBWA')
|
||||
end
|
||||
-- Do not override again until state machine has triggered again
|
||||
override_enable = false
|
||||
else
|
||||
hdg_ok_t = hdg_ok_t + looptime
|
||||
end
|
||||
-- Heading error is big, reset timer hdg_ok_t
|
||||
else
|
||||
hdg_ok_t = 0
|
||||
end
|
||||
|
||||
-- Play tune every tune_repeat_t [ms]
|
||||
if tune_time_since > tune_repeat_t then
|
||||
-- Play tune and reset timer
|
||||
send_to_gcs(_INFO, 'LUA: Play warning tune')
|
||||
play_tune(_tune_glide_warn)
|
||||
tune_time_since = 0
|
||||
else
|
||||
tune_time_since = tune_time_since + looptime
|
||||
end
|
||||
|
||||
-- If not steered into wind yet, update goto point into wind
|
||||
if override_enable then
|
||||
-- Check override time, if above limit, switch back to FBWA
|
||||
override_time = override_time + looptime
|
||||
if override_time > override_time_lim then
|
||||
set_flight_mode(mode_FBWA, "LUA: Glide into wind override time out, GLIDE in current heading")
|
||||
override_enable = false
|
||||
end
|
||||
-- Get current position and handle if not valid
|
||||
location_here = ahrs:get_location()
|
||||
if location_here == nil then
|
||||
-- In case we cannot get location for some time we must give up and continue with GLIDE
|
||||
failed_location_counter = failed_location_counter + 1
|
||||
if failed_location_counter > 5 then
|
||||
set_flight_mode(mode_FBWA, "LUA: Glide failed to get location, GLIDE in current heading")
|
||||
override_enable = false
|
||||
return update, looptime
|
||||
end
|
||||
gcs:send_text(_WARNING, "LUA: Glide failed to get location")
|
||||
return update, looptime
|
||||
end
|
||||
-- Calc upwind position, copy and modify location_here
|
||||
location_upwind = location_here:copy()
|
||||
location_upwind:offset_bearing(wind_dir_180, upwind_distance)
|
||||
|
||||
-- Set location_upwind as GUIDED target
|
||||
if vehicle:set_target_location(location_upwind) then
|
||||
if not user_notified then
|
||||
send_to_gcs(_INFO, "LUA: Guided target set " .. upwind_distance .. "m away at bearing " .. wind_dir_180)
|
||||
-- Just notify once
|
||||
user_notified = true
|
||||
end
|
||||
else
|
||||
-- Most likely we are not in GUIDED anymore (operator changed mode), state machine will handle this in next loop.
|
||||
gcs:send_text(_WARNING, "LUA: Glide failed to set upwind target")
|
||||
end
|
||||
end
|
||||
end
|
||||
return update, looptime
|
||||
end
|
||||
|
||||
|
||||
-------------------
|
||||
-- Helper functions
|
||||
-------------------
|
||||
|
||||
-- Set mode and wait for mode change
|
||||
function set_flight_mode(mode, message)
|
||||
expected_flight_mode = mode
|
||||
vehicle:set_mode(expected_flight_mode)
|
||||
return wait_for_mode_change(mode, message, 0)
|
||||
end
|
||||
|
||||
-- Wait for mode change
|
||||
function wait_for_mode_change(mode, message, attempt)
|
||||
-- If mode change does not go through after 10 attempts, give up
|
||||
if attempt > 10 then
|
||||
send_to_gcs(_WARNING, 'LUA: Glide into wind mode change failed.')
|
||||
return update, looptime
|
||||
-- If mode change does not go through, wait and try again
|
||||
elseif vehicle:get_mode() ~= mode then
|
||||
return wait_for_mode_change(mode, message, attempt + 1), 5
|
||||
-- Mode change has gone through
|
||||
else
|
||||
send_to_gcs(_INFO, message)
|
||||
return update, looptime
|
||||
end
|
||||
end
|
||||
|
||||
-- Function to compare two Location objects
|
||||
function locations_are_equal(loc1, loc2)
|
||||
-- If either location is nil, they are not equal
|
||||
if not loc1 or not loc2 then
|
||||
return false
|
||||
end
|
||||
-- Compare latitude and longitude, return bool
|
||||
return loc1:lat() == loc2:lat() and loc1:lng() == loc2:lng()
|
||||
end
|
||||
|
||||
-- bind a parameter to a variable
|
||||
function bind_param(name)
|
||||
local p = Parameter()
|
||||
assert(p:init(name), string.format('could not find %s parameter', name))
|
||||
return p
|
||||
end
|
||||
|
||||
-- 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 bind_param(PARAM_TABLE_PREFIX .. name)
|
||||
end
|
||||
|
||||
-- Print to GCS
|
||||
function send_to_gcs(level, mess)
|
||||
gcs:send_text(level, mess)
|
||||
end
|
||||
|
||||
-- Play tune
|
||||
function play_tune(tune)
|
||||
notify:play_tune(tune)
|
||||
end
|
||||
|
||||
-- Returns the angle in range 0-360
|
||||
function wrap_360(angle)
|
||||
local res = math.fmod(angle, 360.0)
|
||||
if res < 0 then
|
||||
res = res + 360.0
|
||||
end
|
||||
return res
|
||||
end
|
||||
|
||||
-- Returns the angle in range -180-180
|
||||
function wrap_180(angle)
|
||||
local res = wrap_360(angle)
|
||||
if res > 180 then
|
||||
res = res - 360
|
||||
end
|
||||
return res
|
||||
end
|
||||
|
||||
-- Start up the script
|
||||
return _init, 2000
|
||||
Loading…
Reference in New Issue