LUA example Glide into wind

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