From 300e5ccedd5c78fa1729b3294c238cc741af959a Mon Sep 17 00:00:00 2001 From: Andreas Gising Date: Fri, 28 Jun 2024 09:19:26 +0200 Subject: [PATCH] AP_Scripting: add glide-into-wind example without RC override --- .../AP_Scripting/examples/glide_into_wind.lua | 399 ++++++++++++++++++ 1 file changed, 399 insertions(+) create mode 100644 libraries/AP_Scripting/examples/glide_into_wind.lua diff --git a/libraries/AP_Scripting/examples/glide_into_wind.lua b/libraries/AP_Scripting/examples/glide_into_wind.lua new file mode 100644 index 0000000000..389ac155eb --- /dev/null +++ b/libraries/AP_Scripting/examples/glide_into_wind.lua @@ -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