I got curious as to how this might work, so I worked up a quick script that might meet your needs (or at least serve as an example for you).
Follow the linked wiki instructions for enabling onboard scripting. If you want to use this exact script, save the example below as a file with a .lua extension.
Lua Scripts — Rover documentation (ardupilot.org)
To use the script, your mission must include SCRIPT_TIME commands that look like this:
In the command column, enter 86 (a number I picked arbitrarily as the trigger for the script). In the arg1 column, enter the allowable distance to drift after reaching the preceding waypoint.
For example, the screenshot shows that the craft will be allowed to drift 20 meters after reaching waypoint 2 and 8 meters after waypoint 4.
Drift distance can be monitored on the Quick tab of Mission Planner as MAV_DRIFT:
I tested this in simulation (SITL), and it seemed to work very well. Please do your own carefully monitored testing before deploying this (or any) script on a working vehicle. You may want to include a timeout, as well, since this script as written will require user intervention to select the subsequent waypoint if the drift distance is never achieved (on a very calm day, for example).
local DRIFT_CMD = 86
local DEFAULT_DRIFT_M = 10 -- (m) allowable drift distance if not specified
local DRIFT_MAV_NAME = 'DRIFT'
local MAV_SEVERITY_NOTICE = 5
local MAV_SEVERITY_INFO = 6
local ROVER_MODE_AUTO = 10
local RUN_INTERVAL_MS = 200
local last_id = -1
local drift_start_loc
local script_time_msn_index
local allowable_drift_dist = DEFAULT_DRIFT_M
function do_script_time()
-- exit script time on waypoint change, mode change, or disarm
if not arming:is_armed() or
vehicle:get_mode() ~= ROVER_MODE_AUTO or
mission:get_current_nav_index() ~= script_time_msn_index then
vehicle:nav_script_time_done(last_id)
allowable_drift_dist = DEFAULT_DRIFT_M
gcs:send_text(MAV_SEVERITY_NOTICE, 'Drift canceled!')
return await_script_time, RUN_INTERVAL_MS
end
-- zero steering/throttle using guided mode binding
vehicle:set_desired_turn_rate_and_speed(0, 0)
-- compute drift distance and send to GCS
local current_loc = assert(ahrs:get_location(), 'AHRS location error!')
local drift_dist = current_loc:get_distance(drift_start_loc)
gcs:send_named_float(DRIFT_MAV_NAME, drift_dist)
-- resume mission if drift distance exceeds allowable
if drift_dist > allowable_drift_dist then
vehicle:nav_script_time_done(last_id)
allowable_drift_dist = DEFAULT_DRIFT_M
gcs:send_text(MAV_SEVERITY_NOTICE, ('Drifted %.1fm. Resuming nav!'):format(drift_dist))
return await_script_time, RUN_INTERVAL_MS
end
return do_script_time, RUN_INTERVAL_MS
end
function await_script_time()
-- enter script time routine if DRIFT_CMD is received
-- first argument is allowable drift distance in meters
local id, cmd, arg1, arg2, arg3, arg4 = vehicle:nav_script_time()
if id and cmd == DRIFT_CMD then
drift_start_loc = assert(ahrs:get_location(), 'AHRS location error!')
if arg1 and arg1 > 0 then allowable_drift_dist = arg1 end
last_id = id
script_time_msn_index = mission:get_current_nav_index()
gcs:send_text(MAV_SEVERITY_NOTICE, ('Allowing %.1fm drift...'):format(allowable_drift_dist))
return do_script_time, RUN_INTERVAL_MS
end
-- report -1 for drift distance if script time is inactive
gcs:send_named_float(DRIFT_MAV_NAME, -1)
return await_script_time, RUN_INTERVAL_MS
end
gcs:send_text(MAV_SEVERITY_INFO, 'Allow drift script loaded')
return await_script_time()
