AP_Scripting: plane ship landing script

libraries/AP_Scripting/examples/plane_ship_landing.lua

@@ -0,0 +1,439 @@
+-- support takeoff and landing on moving platforms for VTOL planes
+
+local PARAM_TABLE_KEY = 7
+local PARAM_TABLE_PREFIX = "SHIP_"
+
+local MODE_MANUAL = 0
+local MODE_RTL = 11
+local MODE_QRTL = 21
+local MODE_AUTO = 10
+local MODE_QLOITER = 19
+
+local NAV_TAKEOFF = 22
+local NAV_VTOL_TAKEOFF = 84
+
+local ALT_FRAME_ABSOLUTE = 0
+
+-- 3 throttle position
+local THROTTLE_LOW = 0
+local THROTTLE_MID = 1
+local THROTTLE_HIGH = 2
+
+-- 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
+
+-- setup SHIP specific parameters
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), 'could not add param table')
+SHIP_ENABLE     = bind_add_param('ENABLE', 1, 0)
+SHIP_LAND_ANGLE = bind_add_param('LAND_ANGLE', 2, 0)
+SHIP_AUTO_OFS   = bind_add_param('AUTO_OFS', 3, 0)
+
+-- other parameters
+RCMAP_THROTTLE  = bind_param("RCMAP_THROTTLE")
+ALT_HOLD_RTL    = bind_param("ALT_HOLD_RTL")
+Q_RTL_ALT       = bind_param("Q_RTL_ALT")
+TRIM_ARSPD_CM   = bind_param("TRIM_ARSPD_CM")
+TECS_LAND_ARSPD = bind_param("TECS_LAND_ARSPD")
+Q_TRANS_DECEL   = bind_param("Q_TRANS_DECEL")
+WP_LOITER_RAD   = bind_param("WP_LOITER_RAD")
+FOLL_OFS_X      = bind_param("FOLL_OFS_X")
+FOLL_OFS_Y      = bind_param("FOLL_OFS_Y")
+FOLL_OFS_Z      = bind_param("FOLL_OFS_Z")
+
+-- an auth ID to disallow arming when we don't have the beacon
+local auth_id = arming:get_aux_auth_id()
+arming:set_aux_auth_failed(auth_id, "Ship: no beacon")
+
+-- current target
+local target_pos = Location()
+local current_pos = Location()
+local target_velocity = Vector3f()
+local target_heading = 0.0
+
+-- landing stages
+local STAGE_HOLDOFF = 0
+local STAGE_DESCEND = 1
+local STAGE_APPROACH = 2
+local STAGE_IDLE = 2
+local landing_stage = STAGE_HOLDOFF
+
+-- other state
+local vehicle_mode = MODE_MANUAL
+local reached_alt = false
+local throttle_pos = THROTTLE_HIGH
+local have_target = false
+
+-- square a variable
+function sq(v)
+   return v*v
+end
+
+-- return time since boot in seconds
+function time_seconds()
+   return millis():tofloat() * 0.001
+end
+
+--[[
+   parameter values which are auto-set on startup
+--]]
+local key_params = {
+   FOLL_ENABLE = 1,
+   FOLL_OFS_TYPE = 1,
+   FOLL_ALT_TYPE = 0,
+}
+
+-- check key parameters
+function check_parameters()
+   for p, v in pairs(key_params) do
+      local current = param:get(p)
+      if not current then
+         gcs:send_text(0,string.format("Parameter %s not found", p))
+      end
+      if math.abs(v-current) > 0.001 then
+         param:set_and_save(p, v)
+         gcs:send_text(0,string.format("Parameter %s set to %.2f was %.2f", p, v, current))
+      end
+   end
+end
+
+-- update the pilots throttle position
+function update_throttle_pos()
+   local tpos
+   if not rc:has_valid_input() then
+      tpos = THROTTLE_LOW
+   else
+      local tchan = rc:get_channel(RCMAP_THROTTLE:get())
+      local tval = (tchan:norm_input_ignore_trim()+1.0)*0.5
+      if tval >= 0.40 then
+         tpos = THROTTLE_HIGH
+      elseif tval >= 0.1 then
+         tpos = THROTTLE_MID
+      else
+         tpos = THROTTLE_LOW
+      end
+   end
+   if tpos ~= throttle_pos then
+      reached_alt = false
+      if landing_stage == STAGE_HOLDOFF and tpos <= THROTTLE_MID then
+         landing_stage = STAGE_DESCEND
+         gcs:send_text(0, string.format("Descending for approach (hd=%.1fm h=%.1f th=%.1f)",
+                                        get_holdoff_distance(), current_pos:alt()*0.01, get_target_alt()))
+      end
+      if landing_stage == STAGE_DESCEND and tpos == THROTTLE_HIGH then
+         gcs:send_text(0,"Climbing for holdoff")
+         landing_stage = STAGE_HOLDOFF
+      end
+   end
+   throttle_pos = tpos
+end
+
+-- get landing airspeed
+function get_land_airspeed()
+   if TECS_LAND_ARSPD:get() < 0 then
+      return TRIM_ARSPD_CM:get() * 0.01
+   end
+   return TECS_LAND_ARSPD:get()
+end
+
+--[[
+  calculate stopping distance assuming we are flying at
+  TECS_LAND_ARSPD and are approaching the landing target from
+  behind. Take account of the wind estimate to get approach
+  groundspeed
+--]]
+function stopping_distance()
+   -- get the target true airspeed for approach
+   local tas = get_land_airspeed() * ahrs:get_EAS2TAS()
+
+   -- add in wind in direction of flight
+   local wind = ahrs:wind_estimate():xy()
+
+   -- rotate wind to be in approach frame
+   wind:rotate(-math.rad(target_heading + SHIP_LAND_ANGLE:get()))
+
+   -- ship velocity rotated to the approach frame
+   local ship2d = target_velocity:xy()
+   ship2d:rotate(-math.rad(target_heading + SHIP_LAND_ANGLE:get()))
+
+   -- calculate closing speed
+   -- use pythagoras theorem to solve for the wind triangle
+   local tas_sq = sq(tas)
+   local y_sq = sq(wind:y())
+   local closing_speed
+   if tas_sq >= y_sq then
+      closing_speed = math.sqrt(tas_sq - y_sq)
+   else
+      -- min 1 m/s
+      closing_speed = 1.0
+   end
+
+   -- include the wind in the direction of the ship
+   closing_speed = closing_speed + wind:x()
+
+   -- account for the ship velocity
+   closing_speed = closing_speed - ship2d:x()
+
+   -- calculate stopping distance
+   return sq(closing_speed) / (2.0 * Q_TRANS_DECEL:get())
+end
+
+-- get holdoff distance
+function get_holdoff_distance()
+   local radius = WP_LOITER_RAD:get()
+   local holdoff_dist = math.abs(radius*1.5)
+   local stop_distance = stopping_distance()
+
+   -- increase holdoff distance by up to 50% to ensure we can stop
+   holdoff_dist = math.max(holdoff_dist, math.min(holdoff_dist*2.5, stop_distance*2))
+   return holdoff_dist
+end
+
+-- get the holdoff position
+function get_holdoff_position()
+   local radius = WP_LOITER_RAD:get()
+   local heading_deg = target_heading + SHIP_LAND_ANGLE:get()
+   local holdoff_dist = get_holdoff_distance()
+
+   local ofs = Vector2f()
+   ofs:x(-holdoff_dist)
+   ofs:y(radius)
+   ofs:rotate(math.rad(heading_deg))
+   local target = target_pos:copy()
+   target:offset(ofs:x(), ofs:y())
+   return target
+end
+
+function wrap_360(angle)
+   local res = math.fmod(angle, 360.0)
+    if res < 0 then
+        res = res + 360.0
+    end
+    return res
+end
+
+function wrap_180(angle)
+    local res = wrap_360(angle)
+    if res > 180 then
+       res = res - 360
+    end
+    return res
+end
+
+--[[
+   check if we have reached the tangent to the landing location
+--]]
+function check_approach_tangent()
+   local distance = current_pos:get_distance(target_pos)
+   local holdoff_dist = get_holdoff_distance()
+   if landing_stage == STAGE_HOLDOFF and throttle_pos <= THROTTLE_MID and distance < 4*holdoff_dist then
+      gcs:send_text(0, string.format("Descending for approach (hd=%.1fm)", holdoff_dist))
+      landing_stage = STAGE_DESCEND
+   end
+   if reached_alt and landing_stage == STAGE_DESCEND then
+      -- go to approach stage when throttle is low, we are
+      -- pointing at the ship and have reached target alt.
+      -- Also require we are within 2.5 radius of the ship, and our heading is within 20
+      -- degrees of the target heading
+      local target_bearing_deg = wrap_180(math.deg(current_pos:get_bearing(target_pos)))
+      local ground_bearing_deg = wrap_180(math.deg(ahrs:groundspeed_vector():angle()))
+      local margin = 10
+      local distance = current_pos:get_distance(target_pos)
+      local holdoff_dist = get_holdoff_distance()
+      local error1 = math.abs(wrap_180(target_bearing_deg - ground_bearing_deg))
+      local error2 = math.abs(wrap_180(ground_bearing_deg - (target_heading + SHIP_LAND_ANGLE:get())))
+      logger.write('SLND','TBrg,GBrg,Dist,HDist,Err1,Err2','ffffff',target_bearing_deg, ground_bearing_deg, distance, holdoff_dist, error1, error2)
+      if (error1 < margin and
+          distance < 2.5*holdoff_dist and
+          distance > 0.7*holdoff_dist and
+          error2 < 2*margin) then
+         -- we are on the tangent, switch to QRTL
+         gcs:send_text(0, "Starting approach")
+         landing_stage = STAGE_APPROACH
+         vehicle:set_mode(MODE_QRTL)
+      end
+   end
+end
+
+--[[
+   check if we should abort a QRTL landing
+--]]
+function check_approach_abort()
+   local alt = current_pos:alt() * 0.01
+   local target_alt = get_target_alt()
+   if alt > target_alt then
+      gcs:send_text(0, "Aborting landing")
+      landing_stage = STAGE_HOLDOFF
+      vehicle:set_mode(MODE_RTL)
+   end
+end
+
+-- update state based on vehicle mode
+function update_mode()
+   local mode = vehicle:get_mode()
+   if mode == vehicle_mode then
+      return
+   end
+   vehicle_mode = mode
+   if mode == MODE_RTL then
+      landing_stage = STAGE_HOLDOFF
+      reached_alt = false
+   elseif mode ~= MODE_QRTL then
+      landing_stage = STAGE_IDLE
+      reached_alt = false
+   end
+end
+
+-- update target state
+function update_target()
+   if not follow:have_target() then
+      if have_target then
+         gcs:send_text(0,"Lost beacon")
+         arming:set_aux_auth_failed(auth_id, "Ship: no beacon")
+      end
+      have_target = false
+   end
+   if not have_target then
+      gcs:send_text(0,"Have beacon")
+      arming:set_aux_auth_passed(auth_id)
+   end
+   have_target = true
+
+   target_pos, target_velocity = follow:get_target_location_and_velocity_ofs()
+   target_heading = follow:get_target_heading_deg()
+   -- zero vertical velocity to reduce impact of ship movement
+   target_velocity:z(0)
+end
+
+-- get the alt target for holdoff, AMSL
+function get_target_alt()
+   local base_alt = target_pos:alt() * 0.01
+   if landing_stage == STAGE_HOLDOFF then
+      return base_alt + ALT_HOLD_RTL:get() * 0.01
+   end
+   return base_alt + Q_RTL_ALT:get()
+end
+
+function update_alt()
+   local alt = current_pos:alt() * 0.01
+   local target_alt = get_target_alt()
+   if landing_stage == STAGE_HOLDOFF or landing_stage == STAGE_DESCEND then
+      if math.abs(alt - target_alt) < 3 then
+         if not reached_alt then
+            gcs:send_text(0,"Reached target altitude")
+         end
+         reached_alt = true
+      end
+   end
+end
+
+--[[
+  update automatic beacon offsets
+--]]
+
+function update_auto_offset()
+   if arming:is_armed() or math.floor(SHIP_AUTO_OFS:get()) ~= 1 then
+      return
+   end
+
+   -- get target without offsets applied
+   target_no_ofs, vel = follow:get_target_location_and_velocity()
+
+   -- setup offsets so target location will be current location
+   local new = target_no_ofs:get_distance_NED(current_pos)
+   new:rotate_xy(-math.rad(target_heading))
+
+   gcs:send_text(0,string.format("Set follow offset (%.2f,%.2f,%.2f)", new:x(), new:y(), new:z()))
+   FOLL_OFS_X:set_and_save(new:x())
+   FOLL_OFS_Y:set_and_save(new:y())
+   FOLL_OFS_Z:set_and_save(new:z())
+
+   SHIP_AUTO_OFS:set_and_save(0)
+end
+
+-- main update function
+function update()
+   if SHIP_ENABLE:get() < 1 then
+      return
+   end
+   if not follow:have_target() then
+      return
+   end
+
+   update_target()
+   if not have_target then
+      return
+   end
+
+   current_pos = ahrs:get_position()
+   if not current_pos then
+      return
+   end
+   current_pos:change_alt_frame(ALT_FRAME_ABSOLUTE)
+
+   update_throttle_pos()
+   update_mode()
+   update_alt()
+   update_auto_offset()
+
+   ahrs:set_home(target_pos)
+
+   local next_WP = vehicle:get_target_location()
+   if not next_WP then
+      -- not in a flight mode with a target location
+      return
+   end
+
+   if vehicle_mode == MODE_RTL then
+      local holdoff_pos = get_holdoff_position()
+      holdoff_pos:change_alt_frame(ALT_FRAME_ABSOLUTE)
+      holdoff_pos:alt(math.floor(get_target_alt()*100))
+      vehicle:update_target_location(next_WP, holdoff_pos)
+
+      if throttle_pos == THROTTLE_LOW then
+         check_approach_tangent()
+      end
+
+   elseif vehicle_mode == MODE_QRTL then
+      vehicle:set_velocity_match(target_velocity:xy())
+      target_pos:alt(next_WP:alt())
+      vehicle:update_target_location(next_WP, target_pos)
+
+      if throttle_pos == THROTTLE_HIGH then
+         check_approach_abort()
+      end
+      
+   elseif vehicle_mode == MODE_AUTO then
+      local id = mission:get_current_nav_id()
+      if id == NAV_VTOL_TAKEOFF or id == NAV_TAKEOFF then
+         vehicle:set_velocity_match(target_velocity:xy())
+         local tpos = current_pos:copy()
+         tpos:alt(next_WP:alt())
+         vehicle:update_target_location(next_WP, tpos)
+      end
+
+   elseif vehicle_mode == MODE_QLOITER then
+      vehicle:set_velocity_match(target_velocity:xy())
+   end
+
+end
+
+function loop()
+   update()
+   -- run at 20Hz
+   return loop, 50
+end
+
+check_parameters()
+
+-- start running update loop
+return loop()