AP_Scripting: added example script for ship takeoff vel match

match velocity of the vehicle from when last disarmed

libraries/AP_Scripting/examples/ship_vel_match.lua

@@ -0,0 +1,90 @@
+-- support takeoff with velocity matching for quadplanes
+
+local PARAM_TABLE_KEY = 35
+local PARAM_TABLE_PREFIX = "SHIPV_"
+
+local MODE_AUTO = 10
+
+local NAV_TAKEOFF = 22
+local NAV_VTOL_TAKEOFF = 84
+
+local ALT_FRAME_ABSOLUTE = 0
+
+-- 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 SHIPV specific parameters
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), 'could not add param table')
+SHIPV_ENABLE     = bind_add_param('ENABLE', 1, 0)
+
+local takeoff_vel = nil
+local takeoff_pos = nil
+
+-- main update function
+function update()
+   if SHIPV_ENABLE:get() < 1 then
+      return
+   end
+
+   local vehicle_mode = vehicle:get_mode()
+
+   if not arming:is_armed() then
+      -- when not armed record position and velocity
+      takeoff_vel = ahrs:get_velocity_NED()
+      takeoff_pos = ahrs:get_position()
+      takeoff_pos:change_alt_frame(ALT_FRAME_ABSOLUTE)
+   else
+      if vehicle_mode == MODE_AUTO and takeoff_pos and takeoff_vel then
+         local id = mission:get_current_nav_id()
+         if id == NAV_VTOL_TAKEOFF or id == NAV_TAKEOFF then
+            local next_WP = vehicle:get_target_location()
+            if not next_WP then
+               return
+            end
+            vehicle:set_velocity_match(takeoff_vel:xy())
+            local tpos = takeoff_pos:copy()
+            tpos:alt(next_WP:alt())
+            vehicle:update_target_location(next_WP, tpos)
+         else
+            takeoff_pos = nil
+            takeoff_vel = nil
+         end
+      end
+   end
+end
+
+function loop()
+   update()
+   -- run at 20Hz
+   return loop, 50
+end
+
+-- wrapper around update(). This calls update() at 20Hz,
+-- and if update faults then an error is displayed, but the script is not
+-- stopped
+function protected_wrapper()
+  local success, err = pcall(update)
+  if not success then
+     gcs:send_text(0, "Internal Error: " .. err)
+     -- when we fault we run the update function again after 1s, slowing it
+     -- down a bit so we don't flood the console with errors
+     return protected_wrapper, 1000
+  end
+  return protected_wrapper, 50
+end
+
+gcs:send_text(0, "Loaded quadplane takeoff velmatch")
+
+-- start running update loop
+return protected_wrapper()
+