Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
51cd2dd1c077cb9e4a02d7e86c2ef9e290ff2be6
ardupilot/libraries/AP_Scripting/applets/plane_follow.lua
T
Peter Barker ff67931891 AP_Scripting: use ahrs:get_wind in examples and applets 
ahrs:wind_estimate() is deprecated.  Scripts which already checked the
returned value for nil now get a check which actually means something;
the rest fall back to a zero wind vector, which is what they were
already getting from an invalid estimate in most cases.
2026-07-28 13:48:28 +10:00

1021 lines
40 KiB
Lua
Raw Blame History

--[[
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()
-- assume zero wind if we have no estimate; this is only used to derive an
-- airspeed vector from the ground velocity
local windspeed_vector = ahrs:get_wind() or Vector3f()
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:get_wind() or Vector3f()
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
Reference in New Issue View Git Blame Copy Permalink