AP_Scripting: scripted plane follow

libraries/AP_Scripting/applets/plane_follow.lua

@@ -0,0 +1,810 @@
+--[[
+
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+   Follow in Plane
+   Support follow "mode" in plane. This will actually use GUIDED mode with 
+   a scripting switch to allow guided to track the vehicle id in FOLL_SYSID
+   Uses the AP_Follow library so all of the existing FOLL_* parameters are used
+   as documented for Copter, + add 3 more for this script
+   ZPF_EXIT_MODE - the mode to switch to when follow is turned of using the switch
+   ZPF_FAIL_MODE - the mode to switch to if the target is lost
+   ZPF_TIMEOUT - number of seconds to try to reaquire the target after losing it before failing
+   ZPF_OVRSHT_DEG - if the target is more than this many degrees left or right, assume an overshoot
+   ZPR_TURN_DEG - if the target is more than this many degrees left or right, assume it's turning
+--]]
+
+SCRIPT_VERSION = "4.6.0-044"
+SCRIPT_NAME = "Plane Follow"
+SCRIPT_NAME_SHORT = "PFollow"
+
+-- FOLL_ALT_TYPE and Mavlink FRAME use different values 
+ALT_FRAME = { GLOBAL = 0, RELATIVE = 1, TERRAIN = 3}
+
+MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
+MAV_FRAME = {GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3,  GLOBAL_TERRAIN_ALT = 10}
+MAV_CMD_INT = { DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192, 
+                  GUIDED_CHANGE_SPEED = 43000, GUIDED_CHANGE_ALTITUDE = 43001, GUIDED_CHANGE_HEADING = 43002 }
+MAV_SPEED_TYPE = { AIRSPEED = 0, GROUNDSPEED = 1, CLIMB_SPEED = 2, DESCENT_SPEED = 3 }
+MAV_HEADING_TYPE = { COG = 0, HEADING = 1, DEFAULT = 2} -- COG = Course over Ground, i.e. where you want to go, HEADING = which way the vehicle points 
+
+FLIGHT_MODE = {AUTO=10, RTL=11, LOITER=12, GUIDED=15, QHOVER=18, QLOITER=19, QRTL=21}
+
+local ahrs_eas2tas = ahrs:get_EAS2TAS()
+local windspeed_vector = ahrs:wind_estimate()
+
+local now = millis():tofloat() * 0.001
+local now_target_heading = now
+local follow_enabled = false
+local too_close_follow_up = 0
+local lost_target_countdown = LOST_TARGET_TIMEOUT
+local save_target_heading1 = -400.0
+local save_target_heading2 = -400.0
+local save_target_altitude
+local tight_turn = false
+
+local PARAM_TABLE_KEY = 120
+local PARAM_TABLE_PREFIX = "ZPF_"
+local PARAM_TABLE_KEY2 = 121
+local PARAM_TABLE_PREFIX2 = "ZPF2_"
+
+-- add a parameter and bind it to a variable
+local 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 Parameter(PARAM_TABLE_PREFIX .. name)
+end
+-- setup follow mode specific parameters
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add param table')
+
+-- add a parameter and bind it to a variable
+local function bind_add_param2(name, idx, default_value)
+   assert(param:add_param(PARAM_TABLE_KEY2, idx, name, default_value), string.format('could not add param %s', name))
+   return Parameter(PARAM_TABLE_PREFIX2 .. name)
+end
+-- setup follow mode specific parameters- second tranche
+assert(param:add_table(PARAM_TABLE_KEY2, PARAM_TABLE_PREFIX2, 10), 'could not add param table')
+
+-- This uses the exisitng FOLL_* parameters and just adds a couple specific to this script
+-- but because most of the logic is already in AP_Follow (called by binding to follow:) there
+-- is no need to access them in the scriot
+
+-- we need these existing FOLL_ parametrs
+FOLL_ALT_TYPE = Parameter('FOLL_ALT_TYPE')
+FOLL_SYSID = Parameter('FOLL_SYSID')
+FOLL_TIMEOUT = Parameter('FOLL_TIMEOUT')
+FOLL_OFS_Y = Parameter('FOLL_OFS_Y')
+local foll_sysid = FOLL_SYSID:get() or -1
+local foll_ofs_y = FOLL_OFS_Y:get() or 0.0
+local foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL
+local foll_timeout = FOLL_TIMEOUT:get() or 1000
+
+-- Add these ZPF_ parameters specifically for this script
+--[[
+  // @Param: ZPF_FAIL_MODE
+  // @DisplayName: Plane Follow lost target mode
+  // @Description: Mode to switch to if the target is lost (no signal or > FOLL_DIST_MAX). 
+  // @User: Standard
+--]]
+ZPF_FAIL_MODE = bind_add_param('FAIL_MODE', 1, FLIGHT_MODE.LOITER)
+
+--[[
+  // @Param: ZPF_EXIT_MODE
+  // @DisplayName: Plane Follow exit mode
+  // @Description: Mode to switch to when follow mode is exited normally
+  // @User: Standard
+--]]
+ZPF_EXIT_MODE = bind_add_param('EXIT_MODE', 2, FLIGHT_MODE.LOITER)
+
+--[[
+    // @Param: ZPF_ACT_FN
+    // @DisplayName: Plane Follow Scripting ActivationFunction
+    // @Description: Setting an RC channel's _OPTION to this value will use it for Plane Follow enable/disable
+    // @Range: 300 307
+--]]
+ZPF_ACT_FN = bind_add_param("ACT_FN", 3, 301)
+
+--[[
+    // @Param: ZPF_TIMEOUT
+    // @DisplayName: Plane Follow Scripting Timeout
+    // @Description: How long to try re-aquire a target if lost
+    // @Range: 0 30
+    // @Units: s
+--]]
+ZPF_TIMEOUT = bind_add_param("TIMEOUT", 4, 10)
+
+--[[
+    // @Param: ZPF_OVRSHT_DEG
+    // @DisplayName: Plane Follow Scripting Overshoot Angle
+    // @Description: If the target is greater than this many degrees left or right, assume an overshoot 
+    // @Range: 0 180
+    // @Units: deg
+--]]
+ZPF_OVRSHT_DEG = bind_add_param("OVRSHT_DEG", 5, 75)
+
+--[[
+    // @Param: ZPF_TURN_DEG
+    // @DisplayName: Plane Follow Scripting Turn Angle
+    // @Description: If the target is greater than this many degrees left or right, assume it's turning
+    // @Range: 0 180
+    // @Units: deg
+--]]
+ZPF_TURN_DEG = bind_add_param("TURN_DEG", 6, 15)
+
+--[[
+    // @Param: ZPF_DIST_CLOSE
+    // @DisplayName: Plane Follow Scripting Close Distance
+    // @Description: When closer than this distance assume we track by heading
+    // @Range: 0 100
+    // @Units: m
+--]]
+ZPF_DIST_CLOSE = bind_add_param("DIST_CLOSE", 7, 50)
+
+--[[
+    // @Param: ZPF_WIDE_TURNS
+    // @DisplayName: Plane Follow Scripting Wide Turns
+    // @Description: Use wide turns when following a turning target. Alternative is "cutting the corner"
+    // @Range: 0 1
+--]]
+ZPF_WIDE_TURNS = bind_add_param("WIDE_TURNS", 8, 1)
+
+--[[
+    // @Param: ZPF_ALT
+    // @DisplayName: Plane Follow Scripting Altitude Override
+    // @Description: When non zero, this altitude value (in FOLL_ALT_TYPE frame) overrides the value sent by the target vehicle
+    // @Range: 0 1000
+    // @Units: m
+--]]
+ZPF_ALT_OVR = bind_add_param("ALT_OVR", 9, 0)
+
+--[[
+    // @Param: ZPF_D_P
+    // @DisplayName: Plane Follow Scripting distance P gain
+    // @Description: P gain for the speed PID controller distance component
+    // @Range: 0 1
+--]]
+ZPF2_D_P = bind_add_param2("D_P", 1, 0.3)
+
+--[[
+    // @Param: ZPF_D_I
+    // @DisplayName: Plane Follow Scripting distance I gain
+    // @Description: I gain for the speed PID  distance component
+    // @Range: 0 1
+--]]
+ZPF2_D_I = bind_add_param2("D_I", 2, 0.3)
+
+--[[
+    // @Param: ZPF_D_D
+    // @DisplayName: Plane Follow Scripting distance D gain
+    // @Description: D gain for the speed PID controller distance component
+    // @Range: 0 1
+--]]
+ZPF2_D_D = bind_add_param2("D_D", 3, 0.05)
+
+--[[
+    // @Param: ZPF_V_P
+    // @DisplayName: Plane Follow Scripting speed P gain
+    // @Description: P gain for the speed PID controller velocity component
+    // @Range: 0 1
+--]]
+ZPF2_V_P = bind_add_param2("V_P", 4, 0.3)
+
+--[[
+    // @Param: ZPF_V_I
+    // @DisplayName: Plane Follow Scripting speed I gain
+    // @Description: I gain for the speed PID controller velocity component
+    // @Range: 0 1
+--]]
+ZPF2_V_I = bind_add_param2("V_I", 5, 0.3)
+
+--[[
+    // @Param: ZPF_V_D
+    // @DisplayName: Plane Follow Scripting speed D gain
+    // @Description: D gain for the speed PID controller velocity component
+    // @Range: 0 1
+--]]
+ZPF2_V_D = bind_add_param2("V_D", 6, 0.05)
+
+--[[
+    // @Param: ZPF_LkAHD
+    // @DisplayName: Plane Follow Lookahead seconds
+    // @Description: Time to "lookahead" when calculating distance errors
+    // @Units: s
+--]]
+ZPF2_LKAHD = bind_add_param2("LKAHD", 7, 5)
+
+--[[
+    // @Param: ZPF_DIST_FUDGE
+    // @DisplayName: Plane Follow distance fudge factor
+    // @Description: THe distance returned by the AP_FOLLOW library might be off by about this factor of airspeed
+    // @Units: s
+--]]
+ZPF2_DIST_FUDGE = bind_add_param2("DIST_FUDGE", 8, 0.92)
+
+REFRESH_RATE = 0.05   -- in seconds, so 20Hz
+LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE
+OVERSHOOT_ANGLE = ZPF_OVRSHT_DEG:get() or 75.0
+TURNING_ANGLE = ZPF_TURN_DEG:get() or 20.0
+
+local fail_mode = ZPF_FAIL_MODE:get() or FLIGHT_MODE.QRTL
+local exit_mode = ZPF_EXIT_MODE:get() or FLIGHT_MODE.LOITER
+
+local use_wide_turns = ZPF_WIDE_TURNS:get() or 1
+
+local distance_fudge = ZPF2_DIST_FUDGE:get() or 0.92
+
+DISTANCE_LOOKAHEAD_SECONDS = ZPF2_LKAHD:get() or 5.0
+
+AIRSPEED_MIN = Parameter('AIRSPEED_MIN')
+AIRSPEED_MAX = Parameter('AIRSPEED_MAX')
+AIRSPEED_CRUISE = Parameter('AIRSPEED_CRUISE')
+WP_LOITER_RAD = Parameter('WP_LOITER_RAD')
+WINDSPEED_MAX = Parameter('AHRS_WIND_MAX')
+
+local airspeed_max = AIRSPEED_MAX:get() or 25.0
+local airspeed_min = AIRSPEED_MIN:get() or 12.0
+local airspeed_cruise = AIRSPEED_CRUISE:get() or 18.0
+local windspeed_max = WINDSPEED_MAX:get() or 100.0
+
+local function constrain(v, vmin, vmax)
+   if v < vmin then
+      v = vmin
+   end
+   if v > vmax then
+      v = vmax
+   end
+   return v
+end
+
+local speedpid = require("speedpid")
+local pid_controller_distance = speedpid.speed_controller(ZPF2_D_P:get() or 0.3,
+                                                            ZPF2_D_I:get() or 0.3,
+                                                            ZPF2_D_D:get() or 0.05,
+                                                            5.0, airspeed_min - airspeed_max, airspeed_max - airspeed_min)
+
+local pid_controller_velocity = speedpid.speed_controller(ZPF2_V_P:get() or 0.3,
+                                                            ZPF2_V_I:get() or 0.3,
+                                                            ZPF2_V_D:get() or 0.05,
+                                                            5.0, airspeed_min, airspeed_max)
+
+
+local mavlink_attitude = require("mavlink_attitude")
+local mavlink_attitude_receiver = mavlink_attitude.mavlink_attitude_receiver()
+
+local function follow_frame_to_mavlink(follow_frame)
+   local mavlink_frame = MAV_FRAME.GLOBAL;
+   if (follow_frame == ALT_FRAME.TERRAIN) then
+      mavlink_frame = MAV_FRAME.GLOBAL_TERRAIN_ALT
+   end
+   if (follow_frame == ALT_FRAME.RELATIVE) then
+      mavlink_frame = MAV_FRAME.GLOBAL_RELATIVE_ALT
+   end
+   return mavlink_frame
+end
+
+-- set_vehicle_target_altitude() Parameters
+-- target.alt = new target altitude in meters
+-- target.frame = Altitude frame MAV_FRAME, it's very important to get this right!
+-- target.alt = altitude in meters to acheive
+-- target.speed = z speed of change to altitude (1000.0 = max)
+local function set_vehicle_target_altitude(target)
+   local speed = target.speed or 1000.0 -- default to maximum z acceleration
+   if target.alt == nil then
+      gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": set_vehicle_target_altitude no altiude")
+      return
+   end
+   -- GUIDED_CHANGE_ALTITUDE takes altitude in meters
+   if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_ALTITUDE, {
+                              frame = follow_frame_to_mavlink(target.frame),
+                              p3 = speed,
+                              z = target.alt }) then
+      gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink CHANGE_ALTITUDE returned false")
+   end
+end
+
+-- set_vehicle_heading() Parameters
+-- heading.heading_type = MAV_HEADING_TYPE (defaults to HEADING)
+-- heading.heading = the target heading in degrees
+-- heading.accel = rate/acceleration to acheive the heading 0 = max
+local function set_vehicle_heading(heading)
+   local heading_type = heading.type or MAV_HEADING_TYPE.HEADING
+   local heading_heading = heading.heading or 0
+   local heading_accel = heading.accel or 10.0
+
+   if heading_heading == nil or heading_heading <= -400 or heading_heading > 360 then
+      gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": set_vehicle_heading no heading")
+      return
+   end
+
+   if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_HEADING, { frame = MAV_FRAME.GLOBAL,
+                                 p1 = heading_type, 
+                                 p2 = heading_heading,
+                                 p3 = heading_accel }) then
+      gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink GUIDED_CHANGE_HEADING failed")
+   end
+end
+
+-- set_vehicle_speed() Parameters
+-- speed.speed - new target speed 
+-- speed.type - speed type MAV_SPEED_TYPE
+-- speed.throttle - new target throttle (used if speed = 0)
+-- speed.slew - specified slew rate to hit the target speed, rather than jumping to it immediately 0 = maximum acceleration
+local function set_vehicle_speed(speed)
+   local new_speed = speed.speed or 0.0
+   local speed_type = speed.type or MAV_SPEED_TYPE.AIRSPEED
+   local throttle = speed.throttle or 0.0
+   local slew = speed.slew or 0.0
+   local mode = vehicle:get_mode()
+
+   if speed_type == MAV_SPEED_TYPE.AIRSPEED and mode == FLIGHT_MODE.GUIDED then
+      if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_SPEED, { frame = MAV_FRAME.GLOBAL,
+                                 p1 = speed_type,
+                                 p2 = new_speed,
+                                 p3 = slew }) then
+         gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink GUIDED_CHANGE_SPEED failed")
+      end
+   else
+      if not gcs:run_command_int(MAV_CMD_INT.DO_CHANGE_SPEED, { frame = MAV_FRAME.GLOBAL,
+                                 p1 = speed_type,
+                                 p2 = new_speed,
+                                 p3 = throttle }) then
+         gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink DO_CHANGE_SPEED failed")
+      end
+   end
+end
+
+-- set_vehicle_target_location() Parameters
+-- target.groundspeed (-1 for ignore)
+-- target.radius (defaults to 2m)
+-- target.yaw - not really yaw - it's the loiter direction 1 = CCW, -1 = CW NaN = default
+-- target.lat - latitude in decimal degrees
+-- target.lng - longitude in decimal degrees
+-- target.alt - target alitude in meters
+local function set_vehicle_target_location(target)
+   local radius = target.radius or 2.0
+   local yaw = target.yaw or 1
+   -- If we are on the right side of the vehicle make sure any loitering is CCW (moves away from the other plane)
+   -- yaw > 0 - CCW = turn to the right of the target point
+   -- yaw < 0 - Clockwise = turn to the left of the target point
+   -- if following direct we turn on the "outside"
+
+   -- if we were in HEADING mode, need to switch out of it so that REPOSITION will work
+   -- Note that MAVLink DO_REPOSITION requires altitude in meters
+   set_vehicle_heading({type = MAV_HEADING_TYPE.DEFAULT})
+   if not gcs:run_command_int(MAV_CMD_INT.DO_REPOSITION, { frame = follow_frame_to_mavlink(target.frame),
+                              p1 = target.groundspeed or -1,
+                              p2 = 1,
+                              p3 = radius,
+                              p4 = yaw,
+                              x = target.lat,
+                              y = target.lng,
+                              z = target.alt }) then  -- altitude in m
+      gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink DO_REPOSITION returned false")
+   end
+end
+
+local last_follow_active_state = rc:get_aux_cached(ZPF_ACT_FN:get())
+
+--[[
+   return true if we are in a state where follow can apply
+--]]
+local reported_target = true
+local function follow_active()
+   local mode = vehicle:get_mode()
+
+   if mode == FLIGHT_MODE.GUIDED then
+      if follow_enabled then
+        if follow:have_target() then
+            reported_target = true
+        else
+            if reported_target then
+               gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": no target: " .. follow:get_target_sysid())
+            end
+            reported_target = false
+        end
+      end
+   else
+      reported_target = false
+   end
+
+   return reported_target
+end
+
+--[[
+   check for user activating follow using an RC switch set HIGH
+--]]
+local function follow_check()
+   if ZPF_ACT_FN == nil then
+      return
+   end
+   local foll_act_fn = ZPF_ACT_FN:get()
+   if foll_act_fn == nil then
+      return
+   end
+   local active_state = rc:get_aux_cached(foll_act_fn)
+   if (active_state ~= last_follow_active_state) then
+      if( active_state == 0) then
+         if follow_enabled then
+            -- Follow disabled - return to EXIT mode
+            vehicle:set_mode(exit_mode)
+            follow_enabled = false
+            gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": disabled")
+         end
+      elseif (active_state == 2) then
+         if not (arming:is_armed()) then
+            gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": must be armed")
+         end
+         -- Follow enabled - switch to guided mode
+         vehicle:set_mode(FLIGHT_MODE.GUIDED)
+         follow_enabled = true
+         lost_target_countdown = LOST_TARGET_TIMEOUT
+         --speed_controller_pid.reset()
+         pid_controller_distance.reset()
+         pid_controller_velocity.reset()
+         gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": enabled")
+      end
+      -- Don't know what to do with the 3rd switch position right now.
+      last_follow_active_state = active_state
+   end
+end
+
+local function wrap_360(angle)
+   local res = math.fmod(angle, 360.0)
+    if res < 0 then
+        res = res + 360.0
+    end
+    return res
+end
+
+local function wrap_180(angle)
+    local res = wrap_360(angle)
+    if res > 180 then
+       res = res - 360
+    end
+    return res
+end
+
+local function calculate_airspeed_from_groundspeed(velocity_vector)
+   --[[ 
+   This is the code from AHRS.cpp    
+   Vector3f true_airspeed_vec = nav_vel - wind_vel;
+
+   This is the c++ code from AP_AHRS_DCM.cpp and also from AP_AHRS.cpp
+   float true_airspeed = airspeed_ret * get_EAS2TAS();
+        true_airspeed = constrain_float(true_airspeed,
+                                        gnd_speed - _wind_max,
+                                        gnd_speed + _wind_max);
+        airspeed_ret = true_airspeed / get_EAS2TAS(
+   --]]
+
+   local airspeed_vector = velocity_vector - windspeed_vector
+   local airspeed = airspeed_vector:length()
+   airspeed = airspeed * ahrs_eas2tas
+   airspeed = constrain(airspeed, airspeed - windspeed_max, airspeed + windspeed_max)
+   airspeed = airspeed / ahrs_eas2tas
+
+   return airspeed
+end
+
+-- main update function
+local function update()
+   now = millis():tofloat() * 0.001
+   ahrs_eas2tas = ahrs:get_EAS2TAS()
+   windspeed_vector = ahrs:wind_estimate()
+
+   follow_check()
+   if not follow_active() then
+    return
+   end
+
+   -- set the target frame as per user set parameter - this is fundamental to this working correctly
+   local close_distance = ZPF_DIST_CLOSE:get() or airspeed_cruise * 2.0
+   local long_distance = close_distance * 4.0
+   local altitude_override = ZPF_ALT_OVR:get() or 0
+
+   LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE
+   OVERSHOOT_ANGLE = ZPF_OVRSHT_DEG:get() or 75.0
+   TURNING_ANGLE = ZPF_TURN_DEG:get() or 20.0
+   foll_ofs_y = FOLL_OFS_Y:get() or 0.0
+   foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL
+   use_wide_turns = ZPF_WIDE_TURNS:get() or 1
+   distance_fudge = ZPF2_DIST_FUDGE:get() or 0.92
+
+   --[[
+      get the current navigation target. 
+   --]]
+   local target_location                     -- = Location()     of the target
+   local target_location_offset               -- Location  of the target with FOLL_OFS_* offsets applied
+   local target_velocity -- = Vector3f()     -- current velocity of lead vehicle
+   local target_velocity_offset -- Vector3f  -- velocity to the offset target_location_offset
+   local target_distance -- = Vector3f()     -- vector to lead vehicle
+   local target_distance_offset              -- vector to the target taking offsets into account
+   local xy_dist                             -- distance to target with offsets in meters
+   local target_heading                      -- heading of the target vehicle
+
+   local current_location = ahrs:get_location()
+   if current_location == nil then
+      return
+   end
+   ---@cast current_location -nil
+   local current_altitude = current_location:alt() * 0.01
+
+   local vehicle_airspeed = ahrs:airspeed_estimate()
+   local current_target = vehicle:get_target_location()
+
+   -- because of the methods available on AP_Follow, need to call these multiple methods get_target_dist_and_vel_ned() MUST BE FIRST
+   -- to get target_location, target_velocity, target distance and target 
+   -- and yes target_offsets (hopefully the same value) is returned by both methods
+   -- even worse - both internally call get_target_location_and_Velocity, but making a single method
+   -- in AP_Follow is probably a high flash cost, so we just take the runtime hit
+   --[[
+   target_distance, target_distance_offsets, target_velocity = follow:get_target_dist_and_vel_ned() -- THIS HAS TO BE FIRST
+   target_location, target_velocity = follow:get_target_location_and_velocity()
+   target_location_offset, target_velocity = follow:get_target_location_and_velocity_ofs()
+   local xy_dist = follow:get_distance_to_target() -- this value is set by get_target_dist_and_vel_ned() - why do I have to know this?
+   local target_heading = follow:get_target_heading_deg()
+   --]]
+
+   target_distance, target_distance_offset,
+      target_velocity, target_velocity_offset,
+      target_location, target_location_offset,
+      xy_dist = follow:get_target_info()
+   target_heading = follow:get_target_heading_deg() or -400
+
+   -- if we lose the target wait for LOST_TARGET_TIMEOUT seconds to try to reaquire it
+   if target_location == nil or target_location_offset == nil or
+      target_velocity == nil or target_velocity_offset == nil or
+      target_distance_offset == nil or current_target == nil or target_distance == nil or xy_dist == nil then
+      lost_target_countdown = lost_target_countdown - 1
+      if lost_target_countdown <= 0 then
+         follow_enabled = false
+         vehicle:set_mode(fail_mode)
+         gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": follow: " .. FOLL_SYSID:get() .. " FAILED")
+         return
+      end
+
+      -- maintain the current heading until we re-establish telemetry from the target vehicle -- note that this is not logged, needs fixing
+      gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": follow: lost " .. FOLL_SYSID:get() .. " FAILED hdg: " .. save_target_heading1)
+      set_vehicle_heading({heading = save_target_heading1})
+      set_vehicle_target_altitude({alt = save_target_altitude, frame = foll_alt_type}) -- pass altitude in meters (location has it in cm)
+      return
+   else
+      -- have a good target so reset the countdown 
+      lost_target_countdown = LOST_TARGET_TIMEOUT
+   end
+
+   -- target_velocity from MAVLink (via AP_Follow) is groundspeed, need to convert to airspeed, 
+   -- we can only assume the windspeed for the target is the same as the chase plane
+   local target_airspeed = calculate_airspeed_from_groundspeed(target_velocity_offset)
+
+   local vehicle_heading = math.abs(wrap_360(math.deg(ahrs:get_yaw())))
+   local heading_to_target_offset = math.deg(current_location:get_bearing(target_location_offset))
+
+   -- offset_angle is the difference between the current heading of the follow vehicle and the target_location (with offsets)
+   local offset_angle = wrap_180(vehicle_heading - heading_to_target_offset)
+
+   -- rotate the target_distance_offsets in NED to the same direction has the follow vehicle, we use this below
+   local target_distance_rotated = target_distance_offset:copy()
+   target_distance_rotated:rotate_xy(math.rad(vehicle_heading))
+
+   -- default the desired heading to the target heading (adjusted for projected turns) - we might change this below
+   local airspeed_difference = vehicle_airspeed - target_airspeed
+
+   -- distance seem to be out by about 0.92s at approximately current airspeed just eyeballing it.
+   xy_dist = math.abs(xy_dist) - vehicle_airspeed * distance_fudge
+   -- xy_dist will always be a positive value. To get -v to represent overshoot, use the offset_angle
+   -- to decide if the target is behind
+   if (math.abs(xy_dist) < long_distance) and (math.abs(offset_angle) > OVERSHOOT_ANGLE) then
+      xy_dist = -xy_dist
+   end
+
+   -- projected_distance is how far off the target we expect to be if we maintain the current airspeed for DISTANCE_LOOKAHEAD_SECONDS
+   local projected_distance = xy_dist - airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS
+
+   -- target angle is the difference between the heading of the target and what its heading was 2 seconds ago
+   local target_angle = 0.0
+   if (target_heading ~= nil and target_heading > -400) then
+      -- want the greatest angle of we might have turned
+      local angle_diff1 = wrap_180(math.abs(save_target_heading1 - target_heading))
+      local angle_diff2 = wrap_180(math.abs(save_target_heading2 - target_heading))
+      if angle_diff2 > angle_diff1 then
+         target_angle = angle_diff2
+      else
+         target_angle = angle_diff1
+      end
+      -- remember the target heading from 2 seconds ago, so we can tell if it is turning or not
+      if (now - now_target_heading) > 1 then
+         save_target_altitude = current_altitude
+         save_target_heading2 = save_target_heading1
+         save_target_heading1 = target_heading
+         now_target_heading = now
+      end
+   end
+
+   -- if the target vehicle is starting to roll we need to pre-empt a turn is coming
+   -- this is fairly simplistic and could probably be improved
+   -- got the code from Mission Planner - this is how it calculates the turn radius
+   --[[
+   public float radius
+        {
+            get
+            {
+                if (_groundspeed <= 1) return 0;
+                return (float)toDistDisplayUnit(_groundspeed * _groundspeed / (9.80665 * Math.Tan(roll * MathHelper.deg2rad)));
+            }
+        }
+   --]]
+   local turning = (math.abs(projected_distance) < long_distance and math.abs(target_angle) > TURNING_ANGLE)
+   local turn_starting = false
+   local target_attitude = mavlink_attitude_receiver.get_attitude(foll_sysid)
+   local pre_roll_target_heading = target_heading
+   local desired_heading = target_heading
+   local angle_adjustment
+   tight_turn = false
+   if target_attitude ~= nil then
+      if (now - target_attitude.timestamp_ms < foll_timeout) and
+         math.abs(target_attitude.roll) > 0.1 or math.abs(target_attitude.rollspeed) > 1 then
+         local turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll + target_attitude.rollspeed))
+
+         turning = true
+         -- predict the roll in 1s from now and use that based on rollspeed
+         -- need some more maths to convert a roll angle into a turn angle - from Mission Planner:
+         -- turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll + target_attitude.rollspeed))
+         local tangent_angle = wrap_360(math.deg(math.pi/2.0 - vehicle_airspeed / turn_radius))
+
+         angle_adjustment = tangent_angle * 0.6
+         -- if the roll direction is the same as the y offset, then we are turning on the "inside" (a tight turn) 
+         if (target_attitude.roll < 0 and foll_ofs_y < 0) or
+            (target_attitude.roll > 0 and foll_ofs_y > 0) then
+            tight_turn = true
+         end
+
+         -- if the roll direction is the same as the rollspeed then we are heading into a turn, otherwise we are finishing a turn
+         if foll_ofs_y == 0 or
+            (target_attitude.roll < 0 and target_attitude.rollspeed < 0) or
+            (target_attitude.roll > 0 and target_attitude.rollspeed > 0) then
+            turn_starting = true
+            target_angle = wrap_360(target_angle - angle_adjustment)
+            desired_heading = wrap_360(target_heading - angle_adjustment)
+            -- push the target heading back because it hasn't figured out we are turning yet
+            save_target_heading1 = save_target_heading2
+         end
+      end
+   end
+
+   -- don't count it as close if the heading is diverging (i.e. the target plane has overshot the target so extremely that it's pointing in the wrong direction)
+   local too_close = (projected_distance > 0) and (math.abs(projected_distance) < close_distance) and (offset_angle < OVERSHOOT_ANGLE)
+
+   -- we've overshot if 
+   -- the distance to the target location is not too_close but will be hit in DISTANCE_LOOKAHEAD_SECONDS (projected_distance)
+   -- or the distance to the target location is already negative AND the target is very close OR 
+   -- the angle to the target plane is effectively backwards 
+   local overshot = not too_close and (
+                     (projected_distance < 0 or xy_dist < 0) and
+                     (math.abs(xy_dist or 0.0) < close_distance)
+                                       or offset_angle > OVERSHOOT_ANGLE
+                                       )
+
+   local distance_error = constrain(math.abs(projected_distance) / (close_distance * DISTANCE_LOOKAHEAD_SECONDS), 0.1, 1.0)
+   if overshot or too_close or (too_close_follow_up > 0) then
+      if too_close_follow_up > 0 then
+         too_close = true
+         too_close_follow_up = too_close_follow_up - 1
+      else
+         too_close_follow_up = 10
+      end
+   else
+      too_close_follow_up = 0
+   end
+
+   local target_altitude = 0.0
+   local frame_type_log = foll_alt_type
+
+   if altitude_override ~= 0 then
+      target_altitude = altitude_override
+      frame_type_log = -1
+   elseif target_location_offset ~= nil then
+      -- change the incoming altitude frame from the target_vehicle to the frame this vehicle wants
+      target_location_offset:change_alt_frame(foll_alt_type)
+      target_altitude = target_location_offset:alt() * 0.01
+   end
+
+   local mechanism = 0 -- for logging 1: position/location 2:heading
+   local normalized_distance = math.abs(projected_distance)
+   local close = (normalized_distance < close_distance)
+   local too_wide = (math.abs(target_distance_rotated:y()) > (close_distance/5) and not turning)
+
+   -- xy_dist < 3.0 is a special case because mode_guided will try to loiter around the target location if within 2m
+   -- target_heading - vehicle_heading catches the circumstance where the target vehicle is heaidng in completely the opposite direction
+   if math.abs(xy_dist or 0.0) < 3.0 or
+         ((turning and ((tight_turn and turn_starting) or use_wide_turns or foll_ofs_y == 0)) or -- turning 
+         ((close or overshot) and not too_wide) -- we are very close to the target
+         --math.abs(target_heading - vehicle_heading) > 135) -- the target is going the other way
+         ) then
+      set_vehicle_heading({heading = desired_heading})
+      set_vehicle_target_altitude({alt = target_altitude, frame = foll_alt_type}) -- pass altitude in meters (location has it in cm)
+      mechanism = 2 -- heading - for logging
+   elseif target_location_offset ~= nil then
+      set_vehicle_target_location({lat = target_location_offset:lat(),
+                                    lng = target_location_offset:lng(),
+                                    alt = target_altitude,
+                                    frame = foll_alt_type,
+                                    yaw = foll_ofs_y})
+      mechanism = 1  -- position/location - for logging
+   end
+   -- dv = interim delta velocity based on the pid controller using projected_distance as the error (we want distance == 0)
+   local dv = pid_controller_distance.update(target_airspeed - vehicle_airspeed, projected_distance)
+   local airspeed_new = pid_controller_velocity.update(vehicle_airspeed, dv)
+
+   set_vehicle_speed({speed = constrain(airspeed_new, airspeed_min, airspeed_max)})
+
+   local log_too_close = 0
+   local log_too_close_follow_up = 0
+   local log_overshot = 0
+   if too_close then
+         log_too_close = 1
+   end
+   if too_close_follow_up then 
+      log_too_close_follow_up = 1
+   end
+   if overshot then
+      log_overshot = 1
+   end
+   logger:write("ZPF1",'Dst,DstP,DstE,AspT,Asp,AspO,Mech,Cls,ClsF,OSht','ffffffBBBB','mmmnnn----','----------',
+                  xy_dist,
+                  projected_distance,
+                  distance_error,
+                  target_airspeed,
+                  vehicle_airspeed,
+                  airspeed_new,
+                  mechanism, log_too_close, log_too_close_follow_up, log_overshot
+               )
+   logger:write("ZPF2",'AngT,AngO,Alt,AltT,AltFrm,HdgT,Hdg,HdgP,HdgO','ffffbffff','ddmm-dddd','---------',
+                  target_angle,
+                  offset_angle,
+                  current_altitude,
+                  target_altitude,
+                  frame_type_log,
+                  target_heading,
+                  vehicle_heading,
+                  pre_roll_target_heading,
+                  desired_heading
+               )
+end
+
+-- wrapper around update(). This calls update() at 1/REFRESH_RATE Hz
+-- and if update faults then an error is displayed, but the script is not
+-- stopped
+local function protected_wrapper()
+   local success, err = pcall(update)
+
+   if not success then
+      gcs:send_text(MAV_SEVERITY.ALERT, SCRIPT_NAME_SHORT .. "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, 1000 * REFRESH_RATE
+end
+
+function delayed_start()
+   gcs:send_text(MAV_SEVERITY.INFO, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) )
+   return protected_wrapper()
+end
+
+-- start running update loop - waiting 20s for the AP to initialize
+if FWVersion:type() == 3 then
+   return delayed_start, 20000
+else
+   gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: must run on Plane", SCRIPT_NAME_SHORT))
+end

libraries/AP_Scripting/applets/plane_follow.md

@@ -0,0 +1,129 @@
+# Plane Follow
+
+This script implements follow functionality for Plane. The plane must be
+flying in Guided mode and will trigger on a scripting switch. The target plane
+must be connected via MAVLink and sending mavlink updates to the chase plane
+running this script. The MAVLINK_SYSID of the target must be set in FOLL_SYSID,
+and must be different from the MAVLINK_SYSID of the following plane.
+
+
+# Parameters
+
+The script adds the following parameters to control it's behaviour. It uses 
+the existing FOLL parameters that are used for the Copter FOLLOW mode. In addition
+the following "ZPF" parameters are added: Z = scripting, P = Plane, F = Follow, in two
+banks ZPF and ZPF2.
+
+## ZPF_FAIL_MODE
+
+This is the mode the plane will change to if following fails. Failure happens
+if the following plane loses telemetry from the target, or the distance exceeds
+FOLL_DIST_MAX.
+
+## ZPF_EXIT_MODE
+
+The flight mode the plane will switch to if it exits following. 
+
+## ZPF_ACT_FN
+
+The scripting action that will trigger the plane to start following. When this
+happens the plane will switch to GUIDED mode and the script will use guided mode
+commands to steer the plane towards the target.
+
+## ZPF_TIMEOUT
+
+If the target is lost, this is the timeout to wait to re-aquire the target before 
+triggering ZPF_FAIL_MODE
+
+## ZPF_OVRSHT_DEG
+
+This is for the heuristic that uses the difference between the target vehicle heading
+and the follow vehicle heading to determine if the vehicle has overshot and should slow
+down and turn around. 75 degrees is a good start but tune for your circumstances.
+
+## ZPF_TURN_DEG
+
+This is for the heuristic that uses the difference between the target vehicle heading
+and the follow vehicle heading to determine if the target vehicle is executing a turn.
+15 degrees is a good start but tune for your circumstances.
+
+## ZPF_DIST_CLOSE
+
+One of the most important heuristics the follow logic uses to match the heading and speed
+of the target plane is to trigger different behavior when the target location is "close".
+How close is determined by this value, likely a larger number makes more sense for larger 
+and faster vehicles and lower values for smaller and slower vehicles. Tune for your circumstances.
+
+## ZPF_ALT_OVR
+
+The follow logic can have the follow vehicle track the altitude of the target, but setting a value
+in ZPF_ALT_OVR allows the follow vehicle to follow at a fixed altitude regardless of the altitude
+of the target. The ZPF_ALT_OVR is in meters in FOLL_ALT_TYPE frame. 
+
+## ZPF2_D_P
+
+The follow logic uses two PID controllers for controlling speed, the first uses distance (D) 
+as the error. This is the P gain for the "D" PID controller.
+
+## ZPF2_D_I
+
+The follow logic uses two PID controllers for controlling speed, the first uses distance (D) 
+as the error. This is the I gain for the "D" PID controller.
+
+## ZPF2_D_D
+
+The follow logic uses two PID controllers for controlling speed, the first uses distance (D) 
+as the error. This is the D gain for the "D" PID controller.
+
+## ZPF2_V_P
+
+The follow logic uses two PID controllers for controlling speed, the first uses velocity (V) 
+as the error. This is the P gain for the "V" PID controller.
+
+## ZPF2_V_I
+
+The follow logic uses two PID controllers for controlling speed, the first uses distance (V) 
+as the error. This is the I gain for the "V" PID controller.
+
+## ZPF2_V_D
+
+The follow logic uses two PID controllers for controlling speed, the first uses distance (V) 
+as the error. This is the D gain for the "V" PID controller.
+
+## ZPF2_LKAHD
+
+Time to "lookahead" when calculating distance errors.
+
+## ZPF2_DIST_FUDGE
+
+This parameter might be a bad idea, but it seems the xy_distance between the target offset location
+and the follow vehicle returned by AP_Follow appears to be off by a factor of 
+airspeed * a fudge factor
+This allows this fudge factor to be adjusted until a better solution can be found for this problem.
+
+# Operation
+
+Install the lua script in the APM/scripts directory on the flight
+controllers microSD card. Review the above parameter descriptions and
+decide on the right parameter values for your vehicle and operations.
+
+Install the speedpid.lua, mavlink_attitude.lua and mavlink_msgs.lua files
+in the APM/scripts/modules directory on the SD card.
+
+Most of the follow logic is in AP_Follow which is part of the ArduPilot c++
+code, so this script just calls the existing methods to do things like
+lookup the SYSID of the vehicle to follow and calculate the direction and distance
+to the target, which should ideally be another fixed wing plane, or VTOL in
+fixed wing mode.
+
+The target location the plane will attempt to achieve will be offset from the target
+vehicle location by FOLL_OFS_X and FOLL_OFS_Y. FOLL_OFS_Z will be offset against the 
+target vehicle, but also FOLL_ALT_TYPE will determine the altitude frame that the vehicle
+will use when calculating the target altitude. See the definitions of these
+parameters to understand how they work. ZPF2_ALT_OVR will override the operation of FOLL_OFS_Z
+setting a fixed altitude for the following plane in FOLL_ALT_TYPE frame.
+
+To ensure the follow plane gets timely updates from the target, the SRx_EXT_STAT and SRx_EXTRA1
+telemetry stream rate parameters should be increased to increase the rate that the POSITION_TARGET_GLOBAL_INT
+and ATTITUDE mavlink messages are sent. The default value is 4Hz, a good value is probably 8Hz or 10Hz but 
+some testing should be done to confirm the best rate for your telemetry radios and vehicles.

libraries/AP_Scripting/docs/docs.lua

@@ -414,7 +414,6 @@ function CAN:get_device(buffer_len) end
 ---@return ScriptingCANBuffer_ud|nil
 function CAN:get_device2(buffer_len) end
 
-
 -- get latest FlexDebug message from a CAN node
 ---@param bus number -- CAN bus number, 0 for first bus, 1 for 2nd
 ---@param node number -- CAN node
@@ -3826,28 +3825,42 @@ function precland:healthy() end
 -- desc
 follow = {}
 
--- desc
+-- get the SYSID_THISMAV of the target
+---@return integer
+function follow:get_target_sysid() end
+
+-- get target's heading in degrees (0 = north, 90 = east)
 ---@return number|nil
 function follow:get_target_heading_deg() end
 
--- desc
+-- get target's estimated location and velocity (in NED)
----@return Location_ud|nil
+---@return Location_ud|nil -- location
----@return Vector3f_ud|nil
+---@return Vector3f_ud|nil -- velocity
-function follow:get_target_location_and_velocity_ofs() end
-
--- desc
----@return Location_ud|nil
----@return Vector3f_ud|nil
 function follow:get_target_location_and_velocity() end
 
+-- get target's estimated location and velocity (in NED), with offsets added
+---@return Location_ud|nil -- location
+---@return Vector3f_ud|nil -- velocity
+function follow:get_target_location_and_velocity_ofs() end
+
 -- desc
 ---@return uint32_t_ud
 function follow:get_last_update_ms() end
 
--- desc
+-- true if we have a valid target location estimate
 ---@return boolean
 function follow:have_target() end
 
+-- combo function returning all follow values calcuted in a cycle
+---@return Vector3f_ud|nil -- dist_ned - distance to the target
+---@return Vector3f_ud|nil -- dist_with_offs - distance to the target with offsets
+---@return Vector3f_ud|nil -- target_vel_ned - proposed velocity of the target
+---@return Vector3f_ud|nil -- target_vel_ned_ofs - proposed velocity of the target with offsets
+---@return Location_ud|nil -- target_loc - location of the target
+---@return Location_ud|nil -- target_loc_ofs - location of the target with offsets
+---@return number|nil -- target_dist_ofs - distance to the target in meters
+function follow:get_target_info() end
+
 -- desc
 scripting = {}
 

libraries/AP_Scripting/generator/description/bindings.desc

@@ -751,10 +751,13 @@ include AP_Follow/AP_Follow.h
 singleton AP_Follow depends AP_FOLLOW_ENABLED && (APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI)
 singleton AP_Follow rename follow
 singleton AP_Follow method have_target boolean
+singleton AP_Follow method get_target_sysid uint8_t
 singleton AP_Follow method get_last_update_ms uint32_t
 singleton AP_Follow method get_target_location_and_velocity boolean Location'Null Vector3f'Null
 singleton AP_Follow method get_target_location_and_velocity_ofs boolean Location'Null Vector3f'Null
 singleton AP_Follow method get_target_heading_deg boolean float'Null
+singleton AP_Follow method get_target_info boolean Vector3f'Null Vector3f'Null Vector3f'Null Vector3f'Null Location'Null Location'Null float'Null
+singleton AP_Follow method get_target_info depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)
 
 include AC_PrecLand/AC_PrecLand.h
 singleton AC_PrecLand depends AC_PRECLAND_ENABLED && (APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI)

libraries/AP_Scripting/modules/mavlink_attitude.lua

@@ -0,0 +1,168 @@
+--[[
+
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+   MAVLinkAttitude
+   A MAVlink message receiver for ATTITUDE messages specifically
+--]]
+
+local MAVLinkAttitude = {}
+
+MAVLinkAttitude.SCRIPT_VERSION = "4.6.0-004"
+MAVLinkAttitude.SCRIPT_NAME = "MAVLink Attitude"
+MAVLinkAttitude.SCRIPT_NAME_SHORT = "MAVATT"
+
+--[[
+   import mavlink support for NAMED_VALUE_FLOAT, only used for
+   DUAL_AIRCRAFT operation
+--]]
+
+ATTITUDE_MESSAGE = "ATTITUDE"
+
+--[[
+   a lua implementation of the jitter correction algorithm from libraries/AP_RTC
+
+   note that the use of a 32 bit float lua number for a uint32_t
+   milliseconds means we lose accuracy over time. At 9 hours we have
+   an accuracy of about 1 millisecond
+--]]
+function MAVLinkAttitude.JitterCorrection(_max_lag_ms, _convergence_loops)
+    local self = {}
+
+    local max_lag_ms = _max_lag_ms
+    local convergence_loops = _convergence_loops
+    local link_offset_ms = 0
+    local min_sample_ms = 0
+    local initialised = false
+    local min_sample_counter = 0
+
+    function self.correct_offboard_timestamp_msec(offboard_ms, local_ms)
+       local diff_ms = local_ms - offboard_ms
+       if not initialised or diff_ms < link_offset_ms then
+          --[[
+             this message arrived from the remote system with a
+             timestamp that would imply the message was from the
+             future. We know that isn't possible, so we adjust down the
+             correction value
+          --]]
+         link_offset_ms = diff_ms
+         initialised = true
+       end
+
+       local estimate_ms = offboard_ms + link_offset_ms
+
+       if estimate_ms + max_lag_ms < local_ms then
+          --[[
+             this implies the message came from too far in the past. clamp the lag estimate
+             to assume the message had maximum lag
+          --]]
+          estimate_ms = local_ms - max_lag_ms
+          link_offset_ms = estimate_ms - offboard_ms
+       end
+ 
+       if min_sample_counter == 0 then
+          min_sample_ms = diff_ms
+       end
+ 
+       min_sample_counter = (min_sample_counter+1)
+       if diff_ms < min_sample_ms then
+          min_sample_ms = diff_ms
+       end
+       if min_sample_counter == convergence_loops then
+          --[[
+             we have the requested number of samples of the transport
+             lag for convergence. To account for long term clock drift
+             we set the diff we will use in future to this value
+          --]]
+          link_offset_ms = min_sample_ms
+          min_sample_counter = 0
+       end
+       return estimate_ms
+    end
+    return self
+ end
+
+function MAVLinkAttitude.mavlink_attitude_receiver()
+    local self = {}
+    local ATTITUDE_map = {}
+    ATTITUDE_map.id = 30
+    ATTITUDE_map.fields = {
+             { "time_boot_ms", "<I4" },
+             { "roll", "<f" },
+             { "pitch", "<f" },
+             { "yaw", "<f" },
+             { "rollspeed", "<f" },
+             { "pitchspeed", "<f" },
+             { "yawspeed", "<f" },
+             }
+    ATTITUDE_map[ATTITUDE_map.id] = ATTITUDE_MESSAGE
+    local mavlink_msgs = require("mavlink_msgs")
+    local jitter_correction = MAVLinkAttitude.JitterCorrection(5000, 100)
+
+    -- initialise mavlink rx with number of messages, and buffer depth
+    mavlink.init(1, 10)
+
+    -- register message id to receive
+    --mavlink.register_rx_msgid(ATTITUDE_msgid)
+    mavlink.register_rx_msgid(ATTITUDE_map.id)
+
+    function self.decode(message, message_map)
+        local result, offset = mavlink_msgs.decode_header(message)
+        -- map all the fields out
+        for _,v in ipairs(message_map.fields) do
+          if v[3] then
+            result[v[1]] = {}
+            for j=1,v[3] do
+              result[v[1]][j], offset = string.unpack(v[2], message, offset)
+            end
+          else
+            result[v[1]], offset = string.unpack(v[2], message, offset)
+          end
+        end
+        return result;
+    end
+    --[[
+       get an ATTITUDE incoming message from the target vehicle, handling jitter correction
+    --]]
+    function self.get_attitude(target_sysid)
+       local msg,_,timestamp_ms = mavlink.receive_chan()
+       if msg then
+          local parsed_msg = self.decode(msg, ATTITUDE_map)
+
+          if (parsed_msg ~= nil) and (parsed_msg.msgid == ATTITUDE_map.id) then
+             local sysid = parsed_msg.sysid
+             local attitude = {}
+             attitude.timestamp_ms = jitter_correction.correct_offboard_timestamp_msec(parsed_msg.time_boot_ms, timestamp_ms:toint())
+             attitude.roll = parsed_msg.roll
+             attitude.pitch = parsed_msg.pitch
+             attitude.yaw = parsed_msg.yaw
+             attitude.rollspeed = parsed_msg.rollspeed
+             attitude.pitchspeed = parsed_msg.pitchspeed
+             attitude.yawspeed = parsed_msg.yawspeed
+             if sysid == target_sysid then
+                 return attitude
+             end
+          end
+       end
+       return nil
+    end
+    return self
+end
+
+MAVLinkAttitude.mavlink_handler = nil
+MAVLinkAttitude.mavlink_handler = MAVLinkAttitude.mavlink_attitude_receiver()
+
+gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s module loaded", MAVLinkAttitude.SCRIPT_NAME, MAVLinkAttitude.SCRIPT_VERSION) )
+
+return MAVLinkAttitude

libraries/AP_Scripting/modules/speedpid.lua

@@ -0,0 +1,215 @@
+--[[
+
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+   SpeedPI
+   A simple "PI" controller for airspeed. Copied from Andrew Tridgell's original
+   work on the ArduPilot Aerobatics Lua scripts.
+
+    Usage: 
+    1. drop it in the scripts/modules directory
+    2. include in your own script using 
+         local speedpi = requre("speedpi.lua")
+    3. create an instance - you may need to tune the gains
+        local speed_controller = speedpi.speed_controller(0.1, 0.1, 2.5, airspeed_min, airspeed_max)
+    4. call it's update() from your update() with the current airspeed and airspeed error
+        local airspeed_new = speed_controller.update(vehicle_airspeed, desired_airspeed - vehicle_airspeed)
+    5. Set the vehicle airspeed based on the airspeed_new value returned from speedpi    
+
+--]]
+
+local SpeedPID = {}
+
+SpeedPID.SCRIPT_VERSION = "4.6.0-003"
+SpeedPID.SCRIPT_NAME = "Speed PID Controller"
+SpeedPID.SCRIPT_NAME_SHORT = "SpeedPID"
+
+SpeedPID.MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
+
+-- constrain a value between limits
+function SpeedPID.constrain(v, vmin, vmax)
+    if v < vmin then
+       v = vmin
+    end
+    if v > vmax then
+       v = vmax
+    end
+    return v
+end
+
+function SpeedPID.is_zero(x)
+    return math.abs(x) < 1.1920928955078125e-7
+end
+
+function SpeedPID.PID_controller(kP,kI,kD,iMax,min,max)
+    -- the new instance. You can put public variables inside this self
+    -- declaration if you want to
+    local self = {}
+
+    -- private fields as locals
+    local _kP = kP or 0.0
+    local _kI = kI or 0.0
+    local _kD = kD or 0.0
+    local _iMax = iMax
+    local _min = min
+    local _max = max
+    local _last_t = nil
+    local _error = 0.0
+    local _derivative = 0.0
+    local _reset_filter = true
+    local _filt_E_hz = 0.01
+    local _filt_D_hz = 0.005
+    local _D = 0
+    local _I = 0
+    local _P = 0
+    local _total = 0
+    local _counter = 0
+    local _target = 0
+    local _current = 0
+    local nowPI = millis():tofloat() * 0.001
+
+    function self.calc_lowpass_alpha_dt(dt, cutoff_freq)
+        if (dt <= 0.0 or cutoff_freq <= 0.0) then
+            --INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
+            return 1.0
+        end
+        if (SpeedPID.is_zero(cutoff_freq)) then
+            return 1.0
+        end
+        if (SpeedPID.is_zero(dt)) then
+            return 0.0
+        end
+        local rc = 1.0 / (math.pi * 2.0 * cutoff_freq)
+        return dt / (dt + rc);
+    end
+
+    function self.get_filt_E_alpha(dt)
+        return self.calc_lowpass_alpha_dt(dt, _filt_E_hz);
+    end
+
+    function self.get_filt_D_alpha(dt)
+        return self.calc_lowpass_alpha_dt(dt, _filt_D_hz);
+    end
+
+    -- update the controller.
+    function self.update(target, current)
+        local now = micros()
+        if not _last_t then
+           _last_t = now
+        end
+        local dt = (now - _last_t):tofloat() * 0.000001
+        _last_t = now
+        local err = target - current
+        _counter = _counter + 1
+ 
+        -- reset input filter to value received
+        if (_reset_filter) then
+            _reset_filter = false
+            _error = _target - current
+            _derivative = 0.0
+        else 
+            local error_last = _error;
+            _error = _error + self.get_filt_E_alpha(dt) * ((_target - current) - _error);
+
+            -- calculate and filter derivative
+            if (dt >= 0) then
+                local derivative = (_error - error_last) / dt;
+                _derivative = _derivative + self.get_filt_D_alpha(dt) * (derivative - _derivative);
+            end
+        end
+        local D = _derivative * _kD
+        _D = D
+
+        local P = _kP * err
+        if ((_total < _max and _total > _min) or
+            (_total >= _max and err < 0) or
+            (_total <= _min and err > 0)) then
+            _I = _I + _kI * err * dt
+        end
+        if _iMax then
+            _I = SpeedPID.constrain(_I, -_iMax, iMax)
+        end
+        local I = _I
+        local ret = target + P + I + D
+        if math.floor(now:tofloat() * 0.000001) ~= math.floor(nowPI) then
+            nowPI = micros():tofloat() * 0.000001
+        end
+        _target = target
+        _current = current
+        _P = P
+
+        ret = SpeedPID.constrain(ret, _min, _max)
+        _total = ret
+        return ret
+    end
+
+    -- reset integrator to an initial value
+    function self.reset(integrator)
+       _I = integrator
+       _reset_filter = true
+    end
+ 
+    function self.set_D(D)
+        _D = D
+        _D_error = 0
+     end
+ 
+     function self.set_I(I)
+       _kI = I
+    end
+
+    function self.set_P(P)
+       _kP = P
+    end
+ 
+    function self.set_Imax(Imax)
+       _iMax = Imax
+    end
+ 
+    -- log the controller internals
+    function self.log(name, add_total)        
+        -- allow for an external addition to total
+        -- Targ = Current + error ( target airspeed )
+        -- Curr = Current airspeed input to the controller
+        -- P = calculated Proportional component
+        -- I = calculated Integral component
+        -- Total = calculated new Airspeed 
+        -- Add - passed in as 0 
+        logger:write(name,'Targ,Curr,P,I,D,Total,Add','fffffff',_target,_current,_P,_I,_D,_total,add_total)
+    end
+ 
+    -- return the instance
+    return self
+ end
+ 
+function SpeedPID.speed_controller(kP_param, kI_param, kD_param, iMax, sMin, sMax)
+    local self = {}
+    local speedpid = SpeedPID.PID_controller(kP_param, kI_param, kD_param, iMax, sMin, sMax)
+
+    function self.update(spd_current, spd_error)
+        local adjustment = speedpid.update(spd_current + spd_error, spd_current)
+        speedpid.log("ZSPI", 0) -- Z = scripted, S = speed, PI = PI controller
+        return adjustment
+    end
+
+    function self.reset()
+        speedpid.reset(0)
+    end
+
+    return self
+end
+
+gcs:send_text(SpeedPID.MAV_SEVERITY.NOTICE, string.format("%s %s module loaded", SpeedPID.SCRIPT_NAME, SpeedPID.SCRIPT_VERSION) )
+
+return SpeedPID