libraries/AP_Scripting/applets/plane_follow.lua - master - Sangak/ardupilot

libraries/AP_Scripting/applets/plane_follow.lua
1 018 строк39 KB
Tim Tuxworth
AP_Scripting: plane_follow.lua fix crosstrack oscillation
29 мар 2026, 00:40
29 мар 2026, 00:40d887c36
--[[

   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
   FOLLP_EXIT_MODE - the mode to switch to when follow is turned of using the switch
   FOLLP_FAIL_MODE - the mode to switch to if the target is lost
   FOLLP_TIMEOUT - number of seconds to try to reaquire the target after losing it before failing
   FOLLP_OVRSHT_DEG - if the target is more than this many degrees left or right, assume an overshoot
   FOLLP_TURN_DEG - if the target is more than this many degrees left or right, assume it's turning
--]]

SCRIPT_VERSION = "4.7.0-075"
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 = { ATTITUDE = 30, GLOBAL_POSITION_INT = 33, REQUEST_DATA_STREAM = 66,
                  DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192,
                  CMD_SET_MESSAGE_INTERVAL = 511, CMD_REQUEST_MESSAGE = 512,
                  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 

MAV_DATA_STREAM = { MAV_DATA_STREAM_ALL=0, MAV_DATA_STREAM_RAW_SENSORS=1, MAV_DATA_STREAM_EXTENDED_STATUS=2, MAV_DATA_STREAM_RC_CHANNELS=3,
                     MAV_DATA_STREAM_RAW_CONTROLLER=4, MAV_DATA_STREAM_POSITION=6, MAV_DATA_STREAM_EXTRA1=10, MAV_DATA_STREAM_EXTRA2=11 }

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_ms = millis()
local now_target_heading_ms = now_ms
local now_follow_lost_ms = now_ms
local now_heading_ms = now_ms
local too_close_follow_up = 0
local save_target_heading1 = -400.0
local save_target_heading2 = -400.0
local tight_turn = false

local PARAM_TABLE_KEY = 123
local PARAM_TABLE_PREFIX = "FOLLP_"

-- 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, 30), '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_OFS_Y = Parameter('FOLL_OFS_Y')
local foll_sysid = FOLL_SYSID:get()
local foll_ofs_y = FOLL_OFS_Y:get()
local foll_alt_type = FOLL_ALT_TYPE:get()

-- Add these FOLLP_ parameters specifically for this script
--[[
  // @Param: FOLLP_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
--]]
FOLLP_FAIL_MODE = bind_add_param('FAIL_MODE', 1, FLIGHT_MODE.RTL)

--[[
  // @Param: FOLLP_EXIT_MODE
  // @DisplayName: Plane Follow exit mode
  // @Description: Mode to switch to when follow mode is exited normally
  // @User: Standard
--]]
FOLLP_EXIT_MODE = bind_add_param('EXIT_MODE', 2, FLIGHT_MODE.LOITER)

--[[
    // @Param: FOLLP_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
--]]
FOLLP_ACT_FN = bind_add_param("ACT_FN", 3, 301)

--[[
    // @Param: FOLLP_TIMEOUT
    // @DisplayName: Plane Follow Telemetry Timeout
    // @Description: How long to try reaquire a target if telemetry from the lead vehicle is lost.
    // @Range: 0 30
    // @Units: s
--]]
FOLLP_TIMEOUT = bind_add_param("TIMEOUT", 4, 10)

--[[
    // @Param: FOLLP_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
--]]
FOLLP_OVRSHT_DEG = bind_add_param("OVRSHT_DEG", 5, 75)

--[[
    // @Param: FOLLP_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
--]]
FOLLP_TURN_DEG = bind_add_param("TURN_DEG", 6, 15)

--[[
    // @Param: FOLLP_DIST_CLOSE
    // @DisplayName: Plane Follow Scripting Close Distance
    // @Description: When closer than this distance assume we track by heading
    // @Range: 0 100
    // @Units: m
--]]
FOLLP_DIST_CLOSE = bind_add_param("DIST_CLOSE", 7, 75)

--[[
    // @Param: FOLLP_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
--]]
FOLLP_WIDE_TURNS = bind_add_param("WIDE_TURNS", 8, 1)

--[[
    // @Param: FOLLP_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
--]]
FOLLP_ALT_OVR = bind_add_param("ALT_OVR", 9, 0)

--[[
    // @Param: FOLLP_D_P
    // @DisplayName: Plane Follow Scripting distance P gain
    // @Description: P gain for the speed PID controller distance component
    // @Range: 0 1
--]]
FOLLP_D_P = bind_add_param("D_P", 10, 0.00025)

--[[
    // @Param: FOLLP_D_I
    // @DisplayName: Plane Follow Scripting distance I gain
    // @Description: I gain for the speed PID  distance component
    // @Range: 0 1
--]]
FOLLP_D_I = bind_add_param("D_I", 11, 0.00025)

--[[
    // @Param: FOLLP_D_D
    // @DisplayName: Plane Follow Scripting distance D gain
    // @Description: D gain for the speed PID controller distance component
    // @Range: 0 1
--]]
FOLLP_D_D = bind_add_param("D_D", 12, 0.00013)

--[[
    // @Param: FOLLP_V_P
    // @DisplayName: Plane Follow Scripting speed P gain
    // @Description: P gain for the speed PID controller velocity component
    // @Range: 0 1
--]]
FOLLP_V_P = bind_add_param("V_P", 13, 0.00025)

--[[
    // @Param: FOLLP_V_I
    // @DisplayName: Plane Follow Scripting speed I gain
    // @Description: I gain for the speed PID controller velocity component
    // @Range: 0 1
--]]
FOLLP_V_I = bind_add_param("V_I", 14, 0.00025)

--[[
    // @Param: FOLLP_V_D
    // @DisplayName: Plane Follow Scripting speed D gain
    // @Description: D gain for the speed PID controller velocity component
    // @Range: 0 1
--]]
FOLLP_V_D = bind_add_param("V_D", 15, 0.0005)

--[[
    // @Param: FOLLP_LkAHD
    // @DisplayName: Plane Follow Lookahead seconds
    // @Description: Time to "lookahead" when calculating distance errors
    // @Units: s
--]]
FOLLP_LKAHD = bind_add_param("LKAHD", 16, 3)

--[[
    // @Param: FOLLP_SIM_TLF_FN
    // @DisplayName: Plane Follow Simulate Telemetry fail function
    // @Description: Set this switch to simulate losing telemetry from the other vehicle
    // @Range: 300 307
--]]
FOLLP_SIM_TLF_FN = bind_add_param("SIM_TLF_FN", 17, 302)

--[[
    // @Param: FOLLP_XT_P
    // @DisplayName: Plane Follow crosstrack PID controller P term
    // @Description: P term for the crosstrack/heading PID controller
    // @Range: 0 1
--]]
FOLLP_XT_P = bind_add_param("XT_P", 20, 0.8)

--[[
    // @Param: FOLLP_XT_I
    // @DisplayName: Plane Follow crosstrack PID controller I term
    // @Description: I term for the crosstrack/heading PID controller
    // @Range: 0 1
--]]
FOLLP_XT_I = bind_add_param("XT_I", 21, 0.01)

--[[
    // @Param: FOLLP_XT_D
    // @DisplayName: Plane Follow crosstrack PID controller D term
    // @Description: D term for the crosstrack/heading PID controller
    // @Range: 0 1
--]]
FOLLP_XT_D = bind_add_param("XT_D", 22, 0.5)

--[[
    // @Param: FOLLP_XT_MAX
    // @DisplayName: Plane Follow crosstrack PID controller maximum correction
    // @Description: maximum correction retured by the crosstrack/heading PID controller
    // @Range: 0 1
    // @Units: deg
--]]
FOLLP_XT_MAX = bind_add_param("XT_MAX", 23, 45)

--[[
    // @Param: FOLLP_XT_I_MAX
    // @DisplayName: Plane Follow crosstrack PID controller maximum integral
    // @Description: maximum I applied the crosstrack/heading PID controller
    // @Range: 0 100
    // @Units: ms
--]]
FOLLP_XT_I_MAX = bind_add_param("XT_I_MAX", 24, 100)

--[[
    // @Param: FOLLP_REFRESH
    // @DisplayName: Plane Follow refresh rate 
    // @Description: refresh rate for Plane Follow updates
    // @Range: 0 0.2
    // @Units: s
--]]
FOLLP_REFRESH = bind_add_param("REFRESH", 25, 0.05)

local refresh_rate = FOLLP_REFRESH:get()
LOST_TARGET_TIMEOUT = FOLLP_TIMEOUT:get() / refresh_rate
OVERSHOOT_ANGLE = FOLLP_OVRSHT_DEG:get()
TURNING_ANGLE = FOLLP_TURN_DEG:get()
DISTANCE_LOOKAHEAD_SECONDS = FOLLP_LKAHD:get()

local lost_target_countdown = LOST_TARGET_TIMEOUT

local fail_mode = FOLLP_FAIL_MODE:get()
local exit_mode = FOLLP_EXIT_MODE:get()

local use_wide_turns = FOLLP_WIDE_TURNS:get()

--local simulate_telemetry_failed = false

AIRSPEED_MIN = Parameter('AIRSPEED_MIN')
AIRSPEED_MAX = Parameter('AIRSPEED_MAX')
WP_LOITER_RAD = Parameter('WP_LOITER_RAD')
WINDSPEED_MAX = Parameter('AHRS_WIND_MAX')

local airspeed_max = AIRSPEED_MAX:get()
local airspeed_min = AIRSPEED_MIN:get()
local windspeed_max = WINDSPEED_MAX:get()

-------------------------------------------------------------------------------
--- Utility Functions
-------------------------------------------------------------------------------

---@diagnostic disable-next-line:lowercase-global
function constrain(v, vmin, vmax)
   if v < vmin then
      v = vmin
   end
   if v > vmax then
      v = vmax
   end
   return v
end

---@diagnostic disable-next-line:lowercase-global
function wrap_360(angle)
   local res = math.fmod(angle, 360.0)
    if res < 0 then
        res = res + 360.0
    end
    return res
end

---@diagnostic disable-next-line:lowercase-global
function wrap_180(angle)
    local res = wrap_360(angle)
    if res > 180 then
       res = res - 360
    end
    return res
end


-------------------------------------------------------------------------------
--- PID Controllers
-------------------------------------------------------------------------------

local pid = require("pid")
local pid_controller_distance = pid.speed_controller(FOLLP_D_P:get(),
                                                            FOLLP_D_I:get(),
                                                            FOLLP_D_D:get(),
                                                            0.5, airspeed_min - airspeed_max, airspeed_max - airspeed_min)

local pid_controller_velocity = pid.speed_controller(FOLLP_V_P:get(),
                                                            FOLLP_V_I:get(),
                                                            FOLLP_V_D:get(),
                                                            0.5, airspeed_min, airspeed_max)

-- We need a PID controller to manage cross track errors
-- start of crosstrackpid {} class definition
local crosstrackpid = {}
crosstrackpid.__index = crosstrackpid

function crosstrackpid.new(kp, ki, kd, max_correction, integral_limit)
   local self = {}
   setmetatable(self, { __index = crosstrackpid })

   self.kp = kp or 0.8
   self.ki = ki or 0.01
   self.kd = kd or 0.5
   self.max_correction = max_correction -- degrees
   self.integral_limit = integral_limit -- m·s

   self.integral = 0
   self.last_error = 0

   return self
end

-- reset integrator to an initial value
function crosstrackpid.reset(self)
   if self == nil then
      self = setmetatable({}, { __index = crosstrackpid })
   end
   self.integral = 0
   self.last_error = 0
end

function crosstrackpid:compute(desired_track_heading, cross_track_error, dt)
   -- Derivative
   local error_rate = (cross_track_error - self.last_error) / dt

   -- Integral with clamp
   self.integral = self.integral + cross_track_error * dt
   if self.integral > self.integral_limit then self.integral = self.integral_limit end
   if self.integral < -self.integral_limit then self.integral = -self.integral_limit end

   -- PID correction (heading offset in degrees)
   local correction = self.kp * cross_track_error +
                     self.kd * error_rate +
                     self.ki * self.integral

   -- Clamp heading correction
   if correction > self.max_correction then correction = self.max_correction end
   if correction < -self.max_correction then correction = -self.max_correction end

   -- Apply correction (subtract because +error = left of track)
   local corrected_heading = desired_track_heading - correction
   corrected_heading = (corrected_heading + 360) % 360

   -- Save state
   self.last_error = cross_track_error

   return corrected_heading
end
-- end of crosstrackpid {} class definition

-- Instantiate the crosstrack/heading PID controller (outside update loop) 
local xt_pid = crosstrackpid.new(FOLLP_XT_P:get(),
                                 FOLLP_XT_I:get(),
                                 FOLLP_XT_D:get(),
                                 FOLLP_XT_MAX:get(),
                                 FOLLP_XT_I_MAX:get())

-------------------------------------------------------------------------------
--- MAVLink utility objects and functions
-------------------------------------------------------------------------------

local mavlink_attitude = require("mavlink_attitude")
local mavlink_attitude_receiver = mavlink_attitude.mavlink_attitude_receiver()

---@diagnostic disable-next-line:lowercase-global
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)
---@diagnostic disable-next-line:lowercase-global
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 altitude")
      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
---@diagnostic disable-next-line:lowercase-global
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
---@diagnostic disable-next-line:lowercase-global
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

-------------------------------------------------------------------------------
--- Allow for simulation of telemetry fail for testing
-------------------------------------------------------------------------------

--[[
      -- checks for user simulating telemetry fail using FOLLP_SIM_TLF_FN
         - enables (HIGH)/disables (LOW) 
--]]
local simulate_failure = {
   telemetry = false,
}
(function ()
   local last_tel_fail_state = rc:get_aux_cached(FOLLP_SIM_TLF_FN:get())
   function simulate_failure.check()
      local sim_tel_fail = FOLLP_SIM_TLF_FN:get()
      local tel_fail_state = rc:get_aux_cached(sim_tel_fail)
      if tel_fail_state ~= last_tel_fail_state then
         if tel_fail_state == 0 then
            --simulate_telemetry_failed = false
            simulate_failure.telemetry = false
         else
            --simulate_telemetry_failed = true
            simulate_failure.telemetry = true
         end
         last_tel_fail_state = tel_fail_state
      end
   end
end) ()

-------------------------------------------------------------------------------
--- FOLLOW mode managmenet
-------------------------------------------------------------------------------
local follow_mode = {
   enabled = false,
}
(function ()
   local reported_target = true
   local lost_target_now_ms = now_ms
   --[[
      follow_active() - return true if we are in a state where follow can apply
   --]]
   function follow_mode.active()
      local mode = vehicle:get_mode()

      if not follow_mode.enabled then
         return false
      end

      if mode ~= FLIGHT_MODE.GUIDED then
         reported_target = false
         return reported_target
      end
      if follow:have_target() then
            reported_target = true
            lost_target_now_ms = now_ms
      else
         if reported_target then -- i.e. if we previously reported a target but lost it
            if (now_ms - lost_target_now_ms) > 5000 then
               gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": lost prior SYSID: " .. tostring(follow:get_target_sysid()))
               lost_target_now_ms = now_ms
            end
         end
         reported_target = false
         gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": no target SYSID: " .. tostring(follow:get_target_sysid()))
      end
      return reported_target
   end
   function follow_mode.enable()
      if follow_mode.enabled then
         return
      end
      if not arming:is_armed() then
         gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": must be armed")
         return
      end
      -- Follow enabled - switch to guided mode but only if armed 
      vehicle:set_mode(FLIGHT_MODE.GUIDED)
      follow_mode.enabled = true
      lost_target_countdown = LOST_TARGET_TIMEOUT
      pid_controller_distance.reset()
      pid_controller_velocity.reset()
      xt_pid.reset()
      gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": enabled")
   end
   function follow_mode.disable()
      if not follow_mode.enabled then
         return
      end
      -- Follow switched to disabled - return to EXIT mode - but not if disarmed
      if not arming:is_armed() then
         vehicle:set_mode(exit_mode)
      end
      follow_mode.enabled = false
      gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": disabled")
   end

   --[[
      follow_check() - check for user activating follow using an RC switch 
         - set HIGH and if so
            - switches to GUIDED
            - enables follow
            - resets the PID controllers
         -- set LOW and if so
            - exits from GUIDED to the FOLLP_EXIT_MODE
            - disables follow
   --]]
   local last_follow_active_state = rc:get_aux_cached(FOLLP_ACT_FN:get())
   function follow_mode.check()
      if FOLLP_ACT_FN == nil then
         return
      end
      local foll_act_fn = FOLLP_ACT_FN:get()
      local active_state = rc:get_aux_cached(foll_act_fn)
      if (active_state == last_follow_active_state) then
         return
      end
      if( active_state == 0) then
         follow_mode.disable()
      elseif (active_state == 2) then
         follow_mode.enable()
      end
      last_follow_active_state = active_state
   end
end) ()

-- convert a groundspeed to airspeed using windspeed and EAS2TAS (from AHRS)
---@diagnostic disable-next-line:lowercase-global
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

-- ChatGPT code to calculate the distance to the target along_track and cross_track
---@diagnostic disable-next-line:lowercase-global
function calculate_track_distance(P_f, P_l)
    -- Get the follower's ground-relative velocity
    local V_f = ahrs:get_velocity_NED()
    if V_f == nil or (V_f:x() == 0 and V_f:y() == 0) then
        -- No valid velocity, return zeros
        return 0, 0
    end

    -- Create horizontal velocity vector
    local D_f = Vector3f()
    D_f:x(V_f:x())
    D_f:y(V_f:y())
    D_f:z(0)         -- need to do a dot product with 3d vector R below

    -- Normalize in-place
    D_f:normalize()

    -- Get relative position vector
    local R = P_f:get_distance_NED(P_l)

    -- Along-track distance: projection of R onto D_f
    local along_track_distance = R:dot(D_f)

    -- Cross-track distance: projection onto perpendicular to D_f
    local perp_D_f = Vector3f()
    perp_D_f:x(-D_f:y())
    perp_D_f:y(D_f:x())
    perp_D_f:z(0)    -- need to do a dot product with 3d vector R below
    perp_D_f:normalize()

    local cross_track_distance = R:dot(perp_D_f)

    -- -ve cross_track_distance to align to the sign of FOLL_OFS_Y where +ve is to the right
    return along_track_distance, -cross_track_distance
end

---@diagnostic disable-next-line:lowercase-global
function calculate_target_angle(heading)
   local new_target_angle = 0.0
   if (heading ~= nil and heading > -400) then
      -- want the greatest angle of we might have turned
      local angle_diff1 = wrap_180(math.abs(save_target_heading1 - heading))
      local angle_diff2 = wrap_180(math.abs(save_target_heading2 - heading))
      if angle_diff2 > angle_diff1 then
         new_target_angle = angle_diff2
      else
         new_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_ms - now_target_heading_ms) > 1000 then
         save_target_heading2 = save_target_heading1
         save_target_heading1 = heading
         now_target_heading_ms = now_ms
      end
   end
   return new_target_angle
end

-- main update function
function Update()
   now_ms = millis()
   ahrs_eas2tas = ahrs:get_EAS2TAS()
   windspeed_vector = ahrs:wind_estimate()

   simulate_failure.check()
   follow_mode.check()
   if not follow_mode.active() then
      return
   end

   -- set the target frame as per user set parameter - this is fundamental to this working correctly
   local close_distance = FOLLP_DIST_CLOSE:get()
   local long_distance = close_distance * 4.0
   local altitude_override = FOLLP_ALT_OVR:get()

   LOST_TARGET_TIMEOUT = FOLLP_TIMEOUT:get() / refresh_rate
   OVERSHOOT_ANGLE = FOLLP_OVRSHT_DEG:get()
   TURNING_ANGLE = FOLLP_TURN_DEG:get()
   foll_ofs_y = FOLL_OFS_Y:get()
   foll_alt_type = FOLL_ALT_TYPE:get()
   use_wide_turns = FOLLP_WIDE_TURNS:get()

   --[[
      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_heading                      -- heading of the target vehicle

   local current_location = ahrs:get_location()
   if current_location == nil then
      return
   end
   local current_altitude = current_location:alt() * 0.01

   local vehicle_airspeed = ahrs:airspeed_EAS()
   local current_target = vehicle:get_target_location()

   target_location, target_velocity = follow:get_target_location_and_velocity()
   target_location_offset, target_velocity_offset = follow:get_target_location_and_velocity_ofs()

   target_heading = follow:get_target_heading_deg() or -400

   -- if we lose the target wait for LOST_TARGET_TIMEOUT seconds to try to re-aquire it
   if target_location == nil or target_location_offset == nil or
      target_velocity == nil or target_velocity_offset == nil or current_target == nil or
      simulate_failure.telemetry then
      lost_target_countdown = lost_target_countdown - 1
      if lost_target_countdown <= 0 then
         follow_mode.enabled = false
         vehicle:set_mode(fail_mode)
         gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": follow: %.0f FAILED", foll_sysid))
         return
      end

      -- maintain the current heading for 2 seconds until we re-establish telemetry from the target vehicle
      if (now_ms - now_follow_lost_ms) > 2000 then
         gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. string.format(": follow: lost %.0f set hdg: %.0f", foll_sysid, save_target_heading1))
         now_follow_lost_ms = now_ms
         set_vehicle_heading({heading = save_target_heading1})
      end
      return
   else
      -- have a good target so reset the countdown 
      lost_target_countdown = LOST_TARGET_TIMEOUT
      now_follow_lost_ms = now_ms
   end

   -- calculate the target_distance from target distance offset so we don't need to call get_target_dist_and_vel_NED
   local new_target_distance = current_location:get_distance_NED(target_location_offset)
   local along_track_distance, cross_track_distance = calculate_track_distance(current_location, target_location_offset)

   -- 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)

   local vehicle_heading = math.abs(wrap_360(math.deg(ahrs:get_yaw_rad())))
   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 heading to 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()
   local target_distance_rotated = new_target_distance: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

   -- 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 = along_track_distance - 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 = calculate_target_angle(target_heading)

   -- 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 MP calculates the turn radius in c#
   --[[
   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 turning_airspeed_ratio = 1.0
   tight_turn = false
   if target_attitude ~= nil and target_airspeed ~= nil and target_airspeed > airspeed_min then
      if ((now_ms - target_attitude.timestamp_ms) < LOST_TARGET_TIMEOUT) and
         math.abs(target_attitude.roll) > 0.1 or math.abs(target_attitude.rollspeed) > 1 then
         -- the roll and rollspeed are delayed values from the target plane, try to extrapolate them to at least "now" + 1 second
         local rollspeed_impact = target_attitude.rollspeed * (target_attitude.delay_ms + 1000)
         local target_turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll + rollspeed_impact))

         turning = true

         -- 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

         -- calculate the path/distance around the turn for the lead and follow vehicle so we can slow down or speed up
         -- depending on which is flying the shorter path
         local turn_radius = target_turn_radius + foll_ofs_y
         if tight_turn then
            turn_radius = target_turn_radius - foll_ofs_y
         end
         -- theta = l/r - i.e. the angle of the arc is the length of the arc divided by the radius
         local theta = target_airspeed / target_turn_radius
         -- now calculate what my airspeed would need to be to match the target, given I'm flying an arc with a different radius
         local my_airspeed = theta * turn_radius
         if my_airspeed > 0 and my_airspeed > airspeed_min and target_airspeed > 0 then 
            turning_airspeed_ratio = constrain(my_airspeed / target_airspeed, 0.0, 2.0)
         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 along_track_distance < 0) and
                     (math.abs(along_track_distance) < close_distance)
                                       or offset_angle > OVERSHOOT_ANGLE
                                       )

   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 close = (math.abs(projected_distance) < close_distance)
   local too_wide = (math.abs(target_distance_rotated:y()) > (close_distance/4) and not turning)
   local desired_heading = target_heading

   -- target_heading - vehicle_heading catches the circumstance where the target vehicle is heading in completely the opposite direction
   if (math.abs(along_track_distance) < airspeed_max * 0.75 or (math.abs(cross_track_distance) < airspeed_max * 0.25)) 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
         ) then
      -- set the desired heading to the targt heading
      mechanism = 2 -- heading - for logging
   elseif target_location_offset ~= nil then
      -- override the heading to point directly to the target location with offsets.
      desired_heading = heading_to_target_offset
      mechanism = 1  -- position/location - for logging
   end

   -- The desired heading needs a PID controller for crosstrack, but only when it gets close.
   if close or too_close_follow_up > 0 then
      desired_heading = xt_pid:compute(desired_heading, cross_track_distance, (now_ms - now_heading_ms):tofloat() * 0.001)
   end

   -- dv = interim delta velocity based on the pid controller using projected_distance per loop as the error (we want distance == 0)
   local dv_error = along_track_distance * refresh_rate * 2.0
   if dv_error < 0 then
      dv_error = dv_error * 5.0  -- fudge factor to deal with it being harder to slow down from overshoot
   end
   -- re-project the distance error based on the turning angle
   -- if (turning and (tight_turn and turn_starting)) and math.abs(offset_angle) > TURNING_ANGLE  and turning_airspeed_ratio > 0 then
   if turning_airspeed_ratio > 0 and turning_airspeed_ratio < 2.0 then
      dv_error = dv_error * turning_airspeed_ratio
   end
   local airspeed_new = vehicle_airspeed
   local airspeed_error = (target_airspeed - vehicle_airspeed)
   local dv = 0.0
   if dv_error ~= nil then
      dv = pid_controller_distance.update(airspeed_error, dv_error)
      airspeed_new = pid_controller_velocity.update(vehicle_airspeed, dv)
   end

   -- Finally after all the calculations - send the target heading, altitude and airspeed to AP
   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)
   set_vehicle_speed({speed = constrain(airspeed_new, airspeed_min, airspeed_max)})

   -- now log everything
   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("PF1",'Dst,DstP,DstE,AspT,Asp,AspD,AspE,AspO,TrnR,Mech,Cls,CF,OS','fffffffffBBBB','mmmnnnnnn----','-------------',
                  along_track_distance,
                  projected_distance,
                  dv_error,
                  target_airspeed,
                  vehicle_airspeed,
                  dv,
                  airspeed_error,
                  airspeed_new,
                  turning_airspeed_ratio,
                  mechanism, log_too_close, log_too_close_follow_up, log_overshot
               )
   logger:write("PF2",'AngT,AngO,Alt,AltT,AltFrm,HdgT,Hdg,HdgP,HdgO,XTD','ffffbfffff','ddmm-ddddm','----------',
                  target_angle,
                  offset_angle,
                  current_altitude,
                  target_altitude,
                  frame_type_log,
                  target_heading,
                  vehicle_heading,
                  pre_roll_target_heading,
                  desired_heading,
                  cross_track_distance
               )
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
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_Startup()
   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 25s for the AP to initialize if not armed
if FWVersion:type() == 3 then
   if arming:is_armed() then
      return Delayed_Startup()
   else
      return Delayed_Startup, 25000
   end
else
   gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: must run on Plane", SCRIPT_NAME_SHORT))
end