AP_Scripting: update plane-wind-fs example

libraries/AP_Scripting/examples/plane-wind-fs.lua

@@ -12,37 +12,71 @@ local batt_info = { {0, 0, 0, 0}, -- main battery
                                   -- add more batteries to monitor here
                   }
 
--- FIXME: check that the script is actually being run on ArduPlane 
-
 local margin = 30 -- margin in seconds of flight time that should remain once we have reached 
 local time_SF = 1 -- the return time safety factor, 1.1 gives 10% extra time for return flight
 local filter = 0.9 -- filter gain
-local min_flying_time = 30 -- seconds, must have been flying for this long before script will start sampling current
+local min_flying_time = 30 -- seconds, must have been flying in a none Qmode above the min alt for this long before script will start sampling current
 local print_time = 15 -- seconds between update prints (set to zero to disable)
+local alt_min = 0 -- the minimum altitude above home the script will start working at, zero disables
 
 -- if true current draw is normalized with dynamic pressure
 -- this gives better prediction of current draws at other airspeeds
 -- airspeed sensor recommended
 local airspeed_normalize = false
 
--- hard code wind to match SITL (until param get need to set manually on line 72)
+-- hard code wind to match SITL
 -- should get exact return time estimates < +- 5 seconds
-local SITL_wind = true
+local SITL_wind = false
 
--- set these variables to match your parameters
 
-local air_speed = 22 -- TRIM_ARSPD_CM / 100
-local min_air_speed = 10 -- ARSPD_FBW_MIN
-local min_ground_speed = 0 -- MIN_GNDSPD_CM / 100
-local max_bank_angle = 65 -- LIM_ROLL_CD / 100
+-- Read in required params
+local value = param:get('TRIM_ARSPD_CM')
+if value then
+  air_speed = value / 100
+else
+  error('LUA: get TRIM_ARSPD_CM failed')
+end
+local value = param:get('ARSPD_FBW_MIN')
+if value then
+  min_air_speed = value
+else
+  error('LUA: get ARSPD_FBW_MIN failed')
+end
+local value = param:get('MIN_GNDSPD_CM')
+if value then
+  min_ground_speed = value / 100
+else
+  error('LUA: get MIN_GNDSPD_CM failed')
+end
+local value = param:get('LIM_ROLL_CD')
+if value then
+  max_bank_angle = value / 100
+else
+  error('LUA: get LIM_ROLL_CD failed')
+end
 
 -- https://en.wikipedia.org/wiki/Standard_rate_turn#Radius_of_turn_formula
 -- the radius is equal to the circumference per 1 radian
 local turn_rad = (air_speed^2) / (9.81 * math.tan(math.rad(max_bank_angle)))
 
--- Read the radius we expect to to circle at when we get home
-local home_reached_rad = 200
-
+-- Read the radius we expect to circle at when we get home
+local home_reached_rad
+local value = param:get('RTL_RADIUS')
+if value then
+  value = math.abs(value)
+  if value > 0 then
+    home_reached_rad = math.abs(value) * 2
+  else 
+    value = param:get('WP_LOITER_RAD')
+    if value then
+      home_reached_rad = math.abs(value) * 2
+    else
+      error('LUA: get WP_LOITER_RAD failed')
+    end
+  end
+else
+  error('LUA: get RTL_RADIUS failed')
+end
 
 -- internal global variables
 local return_start
@@ -50,6 +84,8 @@ local return_distance
 local return_amps
 local trigger_instance = batt_info[1][1]
 local last_print = 0
+local timer_start_time = 0
+local timer_active = true
 
 -- calculates the amount of time it will take for the vehicle to return home
 -- returns 0 if there is no position, wind or home available
@@ -69,8 +105,17 @@ local function time_to_home()
 
     -- hard code wind for testing
     if SITL_wind then
-      wind_north = 0
-      wind_east = 0
+      -- only safe to read from params at a high rate because we are in SITL
+      -- don't do this on a real vehicle
+      wind_speed = param:get('SIM_WIND_SPD')
+      wind_dir = param:get('SIM_WIND_DIR')
+      if wind_speed and wind_dir then
+        wind_dir = math.rad(wind_dir)
+        wind_north = -math.cos(wind_dir) * wind_speed
+        wind_east = -math.sin(wind_dir) * wind_speed
+      else
+        error('Could not read SITL wind')
+      end
     end
     --gcs:send_text(0, string.format("Wind: north %0.2f, east  %0.2f",wind_north,wind_east))
 
@@ -120,9 +165,13 @@ local function time_to_home()
   return 0, air_speed -- nothing useful available
 end
 
--- idle function, to stop no scripts to run warning
+-- idle function
 function idle()
-  return idle, 10000
+  -- if disarmed and not flying reset for a potential second trigger
+  if not arming:is_armed() and not vehicle:get_likely_flying() then
+    return update, 100
+  end
+  return idle, 1000
 end
 
 -- time margin update function
@@ -201,21 +250,66 @@ end
 
 -- the main update function that is used to decide when we should do a failsafe
 function update()
-  local flying_time = vehicle:get_time_flying_ms()
-  if flying_time < (min_flying_time * 1000) then
-    -- we have not been flying for long enough, skip
+  local now = millis();
+
+  -- check armed
+  if not arming:is_armed() then
+    --gcs:send_text(0, "Failsafe: disabled: not armed")
+    timer_start_time = now
+    timer_active = true
     return update, 100
   end
+
+  -- check flying
+  if not vehicle:get_likely_flying() then
+    --gcs:send_text(0, "Failsafe: disabled: not flying")
+    timer_start_time = now
+    timer_active = true
+    return update, 100
+  end
+
+  -- check mode
+  local current_mode = vehicle:get_mode()
+  if current_mode >= 17 then
+    --gcs:send_text(0, "Failsafe: disabled: Q mode")
+    timer_start_time = now
+    timer_active = true
+    return update, 100
+  end
+
+  -- check altitude
+  if alt_min ~= 0 then
+    local dist = ahrs:get_relative_position_NED_home()
+    if not dist or -1*dist:z() < alt_min then
+      --gcs:send_text(0, "Failsafe: disabled: low alt")
+      timer_start_time = now
+      timer_active = true
+      return update, 100
+    end
+  end
+
+  -- check timer
+  if now - timer_start_time < (min_flying_time * 1000) then
+    --gcs:send_text(0, "Failsafe: disabled: timer")
+    return update, 100
+  end
+
+  -- notify that we have started
+  if timer_active then
+    gcs:send_text(0, "Smart Battery RTL started monitoring")
+    timer_active = false
+  end
+
+  -- check airspeed
   local air_speed_in = ahrs:airspeed_estimate()
   if not air_speed_in then
     error("Could not read airspeed")
   end
-  if air_speed_in  < min_air_speed * 0.75 then
-    -- we are not flying fast enough, skip
+  if air_speed_in < min_air_speed * 0.75 then
+    -- we are not flying fast enough, skip but don't reset the timer
     return update, 100
   end
 
-  local now = millis()
   local min_remaining_time = 86400 -- 24 hours
 
   -- find the return time and airspeed
@@ -228,7 +322,7 @@ function update()
   -- normalize current with dynamic pressure
   if airspeed_normalize then
 
-    -- we could probably just use air speed^2 
+    -- we could probably just use air speed^2
     local press = baro:get_pressure()
     local temp = baro:get_external_temperature() + 273.2 -- convert deg centigrade to kelvin
     local density =  press / (temp * 287.058) -- calculate the air density, ideal gas law, constant is (R) specific gas constant for air
@@ -255,7 +349,7 @@ function update()
 
       local remaining_capacity = (rated_capacity_mah - consumed_mah) * 3.6 -- amp seconds (60^2 / 1000)
       local remaining_time = remaining_capacity / return_amps
-      
+
       local buffer_time = remaining_time - ((return_time * time_SF) + margin)
       if  (return_time < 0) or buffer_time < 0 then
         if return_time < 0 then