Skip to content

Commit

Permalink
AP_Scripting: Plane scripted follow
Browse files Browse the repository at this point in the history
  • Loading branch information
@timtuxworth
timtuxworth committed Jun 24, 2024
1 parent 19bdc5f commit 696f489
Showing 1 changed file with 159 additions and 75 deletions.
234 changes: 159 additions & 75 deletions libraries/AP_Scripting/applets/plane_follow.lua
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,13 @@
a scripting switch to allow guided to track the vehicle id in FOLL_SYSID
--]]

local SCRIPT_VERSION = "4.5.4"
SCRIPT_VERSION = "4.6.0-001"
SCRIPT_NAME = "Plane Follow"
SCRIPT_NAME_SHORT = "PFollow"

local REFRESH_RATE = 0.1 -- in seconds, so 10Hz
REFRESH_RATE = 0.1 -- in seconds, so 10Hz
LOST_TARGET_TIMEOUT = 5 / REFRESH_RATE -- 5 seconds
AIRSPEED_GAIN = 1.2

local PARAM_TABLE_KEY = 83
local PARAM_TABLE_PREFIX = "FOLL_"
Expand All @@ -17,8 +21,9 @@ local MODE_RTL = 11
local MODE_LOITER = 12

local follow_enabled = false

local too_close_follow_up = 0
local lost_target_countdown = LOST_TARGET_TIMEOUT
DISTANCE_LOOKAHEAD_SECONDS = 10

-- bind a parameter to a variable
function bind_param(name)
Expand All @@ -35,11 +40,9 @@ end
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 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
-- FOLL_
-- FOLL_OFS_X = bind_param('FOLL_OFS_X')
-- FOLL_OFS_Y = bind_param('FOLL_OFS_Y')
-- FOLL_OFS_Z = bind_param('FOLL_OFS_Z')
-- FOLL_POS_P = bind_param('FOLL_POS_P')

-- we need these existing FOLL_ parametrs
FOLL_ALT_TYPE = bind_param('FOLL_ALT_TYPE')
Expand Down Expand Up @@ -71,19 +74,19 @@ FOLL_ACT_FN = bind_add_param("ACT_FN", 9, 301)

last_follow_active_state = rc:get_aux_cached(FOLL_ACT_FN:get())

AIRSPEED_MIN = bind_param('AIRSPEED_MIN')
FOLL_OFS_Y = bind_param('FOLL_OFS_Y')
--[[
return true if we are in a state where follow can apply
--]]
local function follow_active()
local mode = vehicle:get_mode()

if mode == MODE_GUIDED then
-- support precision loiter under pilot control
if follow_enabled then
if follow:have_target() then
return true
else
gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: enabled but no target")
return false
end
end
Expand All @@ -106,10 +109,13 @@ local function follow_check()
gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: disabled")
end
elseif (active_state == 2) then
if not (arming:is_armed()) then
gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: must be armed")
end
-- Follow enabled - switch to guided mode
vehicle:set_mode(MODE_GUIDED)
-- Force Guided Mode to not loiter by setting a tiny loiter radius. Otherwise Guide Mode will try loiter around the target vehichle when it gets inside WP_LOITER_RAD
vehicle:set_guided_radius_and_direction(2, false)
vehicle:set_guided_radius_and_direction(5, false)
follow_enabled = true
gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: enabled")
end
Expand All @@ -118,11 +124,29 @@ local function follow_check()
end
end

-- calculate difference between the target heading and the following vehicle heading
function follow_target_angle(target_location)

local follow_target_heading = follow:get_target_heading_deg()

-- find the current location of the vehicle and calculate the bearing to its current target
local vehicle_location = ahrs:get_location()
local vehicle_heading = math.deg(vehicle_location:get_bearing(target_location))

local angle_target = follow_target_heading - vehicle_heading + 360
if follow_target_heading > vehicle_heading then
angle_target = follow_target_heading - vehicle_heading
end
if angle_target > 180 then
angle_target = 360 - angle_target
end
-- gcs:send_text(MAV_SEVERITY.NOTICE, string.format("heading target: %f vehicle %f angle: %f", follow_target_heading, vehicle_heading, angle_target))

return angle_target
end

-- main update function
local function update()
--if follow_enabled and vehicle:get_mode() == MODE_FLY_BY_WIRE_B then
-- vehicle:set_mode(MODE_GUIDED)
--end
follow_check()
if not follow_active() then
return
Expand All @@ -137,82 +161,142 @@ local function update()
local target_velocity -- = Vector3f() -- velocity of lead vehicle
local target_distance -- = Vector3f() -- vector to lead vehicle
local target_offsets -- = Vector3f() -- vector to lead vehicle + offsets

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

-- just because of the methods available on AP_Follow, need to call these two methods
-- 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_lcoation_and_Velocity, but making a single method
-- 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_location, target_offsets = follow:get_target_location_and_velocity_ofs()
target_distance, target_offsets, target_velocity = follow:get_target_dist_and_vel_ned()
if target_distance == nil or target_offsets == nil or target_velocity == nil then
if FOLL_FAIL_MODE:get() ~= nil then
vehicle:set_mode(FOLL_FAIL_MODE:get())
lost_target_countdown = lost_target_countdown - 1
if lost_target_countdown <= 0 then
if FOLL_FAIL_MODE:get() ~= nil then
vehicle:set_mode(FOLL_FAIL_MODE:get())
end
follow_enabled = false
gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": lost target")
return
end
follow_enabled = false
gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: lost target")
else
-- have a good target so reset the countdown
lost_target_countdown = LOST_TARGET_TIMEOUT
end


if target_location == nil or target_velocity == nil or target_offsets == nil or current_target == nil then
return
end

-- will delete this debug later
-- gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Follow: xy_dist %f", xy_dist))
-- gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Follow: target_distance:length() %f", target_distance:length()))
--gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Location: x %f y %f z %f", target_offsets:x(), target_offsets:y(), target_offsets:z()))
--gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Velocity: x %f y %f z %f", target_velocity:x(), target_velocity:y(), target_velocity:z()))
--gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: --------- ")
--gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Follow: target velocity %f", target_airspeed))
-- gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Follow: vehicle airspeed %f %f", vehicle_airspeed, xy_dist - vehicle_airspeed * REFRESH_RATE * 2))
if target_location ~= nil and target_velocity ~= nil and target_offsets ~= nil then
local xy_dist = target_offsets:length()
local vehicle_airspeed = ahrs:airspeed_estimate()
local target_airspeed = target_velocity:length()

-- first we tell the vehicle where we want to fly to. Guided mode figures how how to fly there.
local current_target = vehicle:get_target_location()
if not current_target then
return
end
-- default the desired airspeed to the value returned by AP_Follow (this mgith get overrride below)
local xy_dist = target_offsets:length()
local target_airspeed = target_velocity:length()
local desired_airspeed = target_airspeed
local airspeed_difference = desired_airspeed - vehicle_airspeed
-- this is the projected distance to the target if we acheive the incremental airspeed increase targeted by AP_Follow
local projected_distance = xy_dist + (target_airspeed * DISTANCE_LOOKAHEAD_SECONDS - vehicle_airspeed * DISTANCE_LOOKAHEAD_SECONDS)
--projected_distance = xy_dist + airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS

-- set the target frame as per user set parameter this might be a bad idea
--[[
local altitude_frame = FOLL_ALT_TYPE:get()
if altitude_frame == nil then
altitude_frame = 0
end
target_location:change_alt_frame(altitude_frame)
--]]

-- set the target frame as per user set parameter
local altitude_frame = FOLL_ALT_TYPE:get()
if altitude_frame == nil then
altitude_frame = 0
end
target_location:change_alt_frame(altitude_frame)

-- next we try to match the airspeed of the target vehicle, calculating if we
-- need to speed up if too far behind, or slow down if too close
-- next we try to match the airspeed of the target vehicle, calculating if we
-- need to speed up if too far behind, or slow down if too close

-- if the current velocity will not catch up to the target then we need to speed up
-- use DISTANCE_LOOKAHEAD_SECONDS seconds as a reasonable time to try to catch up
-- first figure out how far away we will be from the required location in DISTANCE_LOOKAHEAD_SECONDS seconds if we maintain the current vehicle and target airspeeds
-- There is nothing magic about 12, it is just "what works"
local target_angle = follow_target_angle(target_location)

--gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": heading: " .. target_heading )
--gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": bearing: " .. vehicle_heading )
--gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": angle: " .. target_angle )

--[[
local overshot = (xy_dist > target_airspeed * 2 and xy_dist < (target_airspeed) * DISTANCE_LOOKAHEAD_SECONDS / 1 and target_angle > 45)
if overshot then
gcs:send_text(MAV_SEVERITY.NOTICE, string.format("OVERSHOT xy_dist: %f target arspeed: %f max target airspeed %f target angle %f", xy_dist, target_airspeed, target_airspeed * DISTANCE_LOOKAHEAD_SECONDS / 1, target_angle))
end
local too_close = xy_dist < target_airspeed * 2
if too_close then
gcs:send_text(MAV_SEVERITY.NOTICE, string.format("TOO CLOSE xy_dist: %f target arspeed: %f max target airspeed %f target angle %f", xy_dist, target_airspeed, target_airspeed * DISTANCE_LOOKAHEAD_SECONDS / 1, target_angle))
end
]]--
-- gcs:send_text(MAV_SEVERITY.NOTICE, string.format("TOO CLOSE xy_dist: %f target arspeed: %f target angle %f", xy_dist, airspeed_difference, target_angle))
local too_close = airspeed_difference < 0 or xy_dist < vehicle_airspeed * 2
--if too_close then
-- gcs:send_text(MAV_SEVERITY.NOTICE, string.format("TOO CLOSE xy_dist: %f target arspeed: %f max target airspeed %f target angle %f", xy_dist, airspeed_difference, target_airspeed * DISTANCE_LOOKAHEAD_SECONDS / 1, target_angle))
--end
-- we've overshot if the distance to the target location is not too_close but will be hit in DISTANCE_LOOKAHEAD_SECONDS
-- AND the vehicles are heading in the same direction (more or less) - based on target_angle
if not too_close then
--gcs:send_text(MAV_SEVERITY.NOTICE, string.format("OVERSHOT? xy_dist: %f airspeed difference: %f max distance %f target angle %f", xy_dist, vehicle_airspeed, vehicle_airspeed * DISTANCE_LOOKAHEAD_SECONDS, target_angle))
end
local overshot = not too_close and (xy_dist < vehicle_airspeed * DISTANCE_LOOKAHEAD_SECONDS) and target_angle > 30
--if overshot then
-- gcs:send_text(MAV_SEVERITY.NOTICE, string.format("OVERSHOT xy_dist: %f airspeed difference: %f max distance %f target angle %f", xy_dist, vehicle_airspeed, vehicle_airspeed * DISTANCE_LOOKAHEAD_SECONDS, target_angle))
--end

if overshot or too_close or too_close_follow_up > 0 then
-- we have overshot or are just about to hit the target, so artificially jump it ahead 500m
-- note that is after setting xy_dist so that we still calculate airspeed below
-- according to the desired location not the one we are telling vehicle about

-- if the current velocity will not catch up to the target then we need to speed up
-- use 12 seconds as a reasonable time to try to catch up
-- first figure out how far away we will be from the required location in 12 seconds if we maintain the current vehicle and target airspeeds
-- There is nothing magic about 12, it is just "what works"
local projected_distance = xy_dist + (target_airspeed * 10 - vehicle_airspeed * 10)

if math.abs(projected_distance) < target_airspeed or too_close_follow_up > 0 then
-- we are just about to hit the target, so artificially jump it ahead 500m
-- note that is after setting xy_dist so that we still calculate airspeed below
-- according to the desired location not the one we are telling vehicle about
local current_bearing = vehicle:get_wp_bearing_deg()
target_location:offset_bearing(current_bearing, 500)

-- force a maxium deceleration
xy_dist = 5000
-- if we are still too close then make sure that we follow_up on this track
-- for 20xREFRESH_RATE (so about 2 seconds) to enable the decelaratino to kick in
if math.abs(projected_distance) < target_airspeed then
too_close_followup = 20
else
too_close_followup = too_close_follow_up - 1
end
local target_heading = follow:get_target_heading_deg()
target_location:offset_bearing(target_heading,500)

if overshot or too_close then
too_close_follow_up = too_close_follow_up - 1
else
too_close_follow_up = 20
-- try to reduce the amount we ajust the airspeed also
end
desired_airspeed = target_airspeed
-- now calculate what airspeed we will need to fly for a few seconds to catch up the projected distance
-- need to slow down dramatically
--gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": desired speed: " .. desired_airspeed )
--desired_airspeed = desired_airspeed * (xy_dist / target_airspeed)
if overshot then
desired_airspeed = target_airspeed - (target_airspeed - AIRSPEED_MIN:get())/2
--gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": OVERSHOT desired speed: " .. desired_airspeed )
else
desired_airspeed = target_airspeed - (target_airspeed - AIRSPEED_MIN:get())/8
--gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": TOO CLOSE desired speed: " .. desired_airspeed )
end
else
-- AP_Follow doesn't speed up enough if wa are a long way from the target
if projected_distance > xy_dist * 2 then
--gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Follow: long distance %f projected %f", xy_dist, projected_distance ))
desired_airspeed = target_airspeed * AIRSPEED_GAIN
else
desired_airspeed = target_airspeed
end
vehicle:update_target_location(current_target, target_location)
end

if vehicle:update_target_location(current_target, target_location) then
local foll_ofs_y = FOLL_OFS_Y:get()
local is_ccw = foll_ofs_y ~= nil and foll_ofs_y > 0

-- now calculate what airspeed we will need to fly for that 12 seconds to catch up the projected distance
--gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Follow: incremental airspeed %f", ((projected_distance - xy_dist) / 12) * 1.1))
local desired_airspeed = vehicle_airspeed + ((projected_distance - xy_dist) / 12) * 1.1
vehicle:set_desired_speed(desired_airspeed)
-- the desired airspeed will never be acheived, don't worry -we will do this calculation again in REFRESH_RATE seconds, so we can adjust
if not vehicle:set_guided_radius_and_direction(2, is_ccw) then
gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": failed set_guided_radius_and_direction(2, %d)", is_ccw))
end
else
gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": failed update_target_location")
end
-- gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": desired speed: " .. desired_airspeed )
vehicle:set_desired_airspeed(desired_airspeed)
-- the desired airspeed will never be acheived, don't worry -we will do this calculation again in REFRESH_RATE seconds, so we can adjust
end

-- wrapper around update(). This calls update() at 20Hz,
Expand All @@ -229,8 +313,8 @@ local function protected_wrapper()
return protected_wrapper, 1000 * REFRESH_RATE
end

gcs:send_text(MAV_SEVERITY.NOTICE, string.format("Plane Follow %s script loaded", SCRIPT_VERSION) )

gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) )
-- start running update loop
return protected_wrapper()

0 comments on commit 696f489

Please sign in to comment.