Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Scripting: simplify Rover quick tune 

Only tunes FF.  P and I are set as ratio to FF
This commit is contained in:
Randy Mackay 2023-11-24 21:26:19 +09:00 committed by Andrew Tridgell
parent 926896eb3a
commit fa6a7abe8a
2 changed files with 149 additions and 325 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

395
libraries/AP_Scripting/applets/rover-quicktune.lua
View File

@ -1,6 +1,6 @@
--[[
Rover QuickTune tunes the steering (aka turn rate), speed and position controller velocity gains for rovers and boats
Rover QuickTune tunes the steering (aka turn rate) and speed controller gains for rovers and boats
The script is designed to be used in Circle mode and updates the following parameters
@ -13,9 +13,6 @@ ATC_SPEED_I
ATC_SPEED_D
CRUISE_SPEED
CRUISE_THROTTLE
PSC_VEL_P
PSC_VEL_I
PSC_VEL_D
See the accompanying rover-quiktune.md file for instructions on how to use
@ -24,8 +21,9 @@ See the accompanying rover-quiktune.md file for instructions on how to use
-- global definitions
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
local PARAM_TABLE_KEY = 12
local PARAM_TABLE_KEY = 15
local PARAM_TABLE_PREFIX = "RTUN_"
local PARAM_TABLE_SIZE = 11
-- bind a parameter to a variable
function bind_param(name)
@ -41,7 +39,7 @@ function bind_add_param(name, idx, default_value)
end
-- setup quicktune specific parameters
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 12), "RTun: could not add param table")
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, PARAM_TABLE_SIZE), "RTun: could not add param table")
--[[
// @Param: RTUN_ENABLE
@ -50,72 +48,70 @@ assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 12), "RTun: could no
// @Values: 0:Disabled,1:Enabled
// @User: Standard
--]]
local RTUN_ENABLE = bind_add_param('ENABLE', 1, 1)
local RTUN_ENABLE = bind_add_param('ENABLE', 1, 1)
--[[
// @Param: RTUN_AXES
// @DisplayName: Rover Quicktune axes
// @Description: axes to tune
// @Bitmask: 0:Steering,1:Speed,2:Velocity
// @Bitmask: 0:Steering,1:Speed
// @User: Standard
--]]
local RTUN_AXES = bind_add_param('AXES', 2, 7)
local RTUN_AXES = bind_add_param('AXES', 2, 3)
--[[
// @Param: RTUN_DOUBLE_TIME
// @DisplayName: Rover Quicktune doubling time
// @Description: Time to double a tuning parameter. Raise this for a slower tune.
// @Range: 5 20
// @Units: s
// @Param: RTUN_STR_FFRATIO
// @DisplayName: Rover Quicktune Steering Rate FeedForward ratio
// @Description: Ratio between measured response and FF gain. Raise this to get a higher FF gain
// @Range: 0 1.0
// @User: Standard
--]]
local RTUN_DOUBLE_TIME = bind_add_param('DOUBLE_TIME', 3, 10)
local RTUN_STR_FFRATIO = bind_add_param('STR_FFRATIO', 3, 0.9)
--[[
// @Param: RTUN_PD_GAINMARG
// @DisplayName: Rover Quicktune P and D gain margin
// @Description: Reduction in P and D gain after oscillation detected. Raise this number to get a more conservative tune
// @Range: 20 90
// @Units: %
// @Param: RTUN_STR_P_RATIO
// @DisplayName: Rover Quicktune Steering FF to P ratio
// @Description: Ratio between steering FF and P gains. Raise this to get a higher P gain, 0 to leave P unchanged
// @Range: 0 2.0
// @User: Standard
--]]
local RTUN_PD_GAINMARG = bind_add_param('PD_GAINMARG', 4, 80)
local RTUN_STR_P_RATIO = bind_add_param('STR_P_RATIO', 4, 0.5)
--[[
// @Param: RTUN_OSC_SMAX
// @DisplayName: Rover Quicktune oscillation rate threshold
// @Description: Threshold for oscillation detection. A lower value will lead to a more conservative tune.
// @Range: 1 10
// @Param: RTUN_STR_I_RATIO
// @DisplayName: Rover Quicktune Steering FF to I ratio
// @Description: Ratio between steering FF and I gains. Raise this to get a higher I gain, 0 to leave I unchanged
// @Range: 0 2.0
// @User: Standard
--]]
local RTUN_OSC_SMAX = bind_add_param('OSC_SMAX', 5, 1)
local RTUN_STR_I_RATIO = bind_add_param('STR_I_RATIO', 5, 0.5)
--[[
// @Param: RTUN_SPD_P_MAX
// @DisplayName: Rover Quicktune Speed P max
// @Description: Maximum value for speed P gain
// @Range: 0.1 10
// @Param: RTUN_SPD_FFRATIO
// @DisplayName: Rover Quicktune Speed FeedForward (equivalent) ratio
// @Description: Ratio between measured response and CRUISE_THROTTLE value. Raise this to get a higher CRUISE_THROTTLE value
// @Range: 0 1.0
// @User: Standard
--]]
local RTUN_SPD_P_MAX = bind_add_param('SPD_P_MAX', 6, 5.0)
local RTUN_SPD_FFRATIO = bind_add_param('SPD_FFRATIO', 6, 1.0)
--[[
// @Param: RTUN_SPD_D_MAX
// @DisplayName: Rover Quicktune Speed D max
// @Description: Maximum value for speed D gain
// @Range: 0.001 1
// @Param: RTUN_SPD_P_RATIO
// @DisplayName: Rover Quicktune Speed FF to P ratio
// @Description: Ratio between speed FF and P gain. Raise this to get a higher P gain, 0 to leave P unchanged
// @Range: 0 2.0
// @User: Standard
--]]
local RTUN_SPD_D_MAX = bind_add_param('SPD_D_MAX', 7, 0.5)
local RTUN_SPD_P_RATIO = bind_add_param('SPD_P_RATIO', 7, 1.0)
--[[
// @Param: RTUN_PI_RATIO
// @DisplayName: Rover Quicktune PI ratio
// @Description: Ratio between P and I gains. Raise this to get a lower I gain, 0 to disable
// @Range: 0.5 1.0
// @Param: RTUN_SPD_I_RATIO
// @DisplayName: Rover Quicktune Speed FF to I ratio
// @Description: Ratio between speed FF and I gain. Raise this to get a higher I gain, 0 to leave I unchanged
// @Range: 0 2.0
// @User: Standard
--]]
local RTUN_PI_RATIO = bind_add_param('PI_RATIO', 8, 1.0)
local RTUN_SPD_I_RATIO = bind_add_param('SPD_I_RATIO', 8, 1.0)
--[[
// @Param: RTUN_AUTO_FILTER
@ -133,7 +129,7 @@ local RTUN_AUTO_FILTER = bind_add_param('AUTO_FILTER', 9, 1)
// @Units: s
// @User: Standard
--]]
local RTUN_AUTO_SAVE = bind_add_param('AUTO_SAVE', 10, 0)
local RTUN_AUTO_SAVE = bind_add_param('AUTO_SAVE', 10, 5)
--[[
// @Param: RTUN_RC_FUNC
@ -144,16 +140,6 @@ local RTUN_AUTO_SAVE = bind_add_param('AUTO_SAVE', 10, 0)
--]]
local RTUN_RC_FUNC = bind_add_param('RC_FUNC', 11, 300)
--[[
// @Param: RTUN_FF_GAINMARG
// @DisplayName: Rover Quicktune Steering Rate FeedForward gain margin
// @Description: Reduction in Steering Turn Rate FF gain from measured outputs and response. Raise this number to get a more conservative tune
// @Range: 0 80
// @Units: %
// @User: Standard
--]]
local RTUN_FF_GAINMARG = bind_add_param('FF_GAINMARG', 12, 10)
-- other vehicle parameters used by this script
local INS_GYRO_FILTER = bind_param("INS_GYRO_FILTER")
local GCS_PID_MASK = bind_param("GCS_PID_MASK")
@ -164,7 +150,7 @@ local RCIN_THROTTLE = rc:get_channel(RCMAP_THROTTLE:get())
-- definitions
local UPDATE_RATE_HZ = 40 -- this script updates at 40hz
local STAGE_DELAY = 4.0 -- gains increased every 4 seconds
local AXIS_CHANGE_DELAY = 4.0 -- delay of 4 seconds between axis to allow vehicle to settle
local PILOT_INPUT_DELAY = 4.0 -- gains are not updated for 4 seconds after pilot releases sticks
local FLTD_MUL = 0.5 -- ATC_STR_RAT_FLTD set to 0.5 * INS_GYRO_FILTER
local FLTT_MUL = 0.5 -- ATC_STR_RAT_FLTT set to 0.5 * INS_GYRO_FILTER
@ -179,19 +165,12 @@ function get_time()
end
-- local variables
local axis_names = { "ATC_STR_RAT", "ATC_SPEED", "PSC_VEL" } -- table of axis that may be tuned
local axis_names = { "ATC_STR_RAT", "ATC_SPEED" } -- table of axis that may be tuned
local param_suffixes = { "FF", "P", "I", "D", "FLTT", "FLTD", "FLTE" } -- table of parameters that may be tuned
local params_extra = {"CRUISE_SPEED", "CRUISE_THROTTLE"} -- table of extra parameters that may be changed
local params_skip = {"PSC_VEL_FLTT"} -- parameters that should be skipped from saving/restoring (because they do not exist)
local stages = { "D", "P", "FF" } -- table of tuning stages
local stage = stages[1] -- current tuning stage
local last_stage_change = get_time() -- time (in seconds) that stage last changed
local last_gain_report = get_time() -- time of last update to user on parameter gain actively being tuned
local last_axis_change = get_time() -- time (in seconds) that axis last changed
local last_pilot_input = get_time() -- time pilot last provided RC input
local tune_done_time = nil -- time that tuning completed (used for auto save feature)
local slew_parm = nil -- parameter name being slewed towards a target. params are sometimes slewed to reduce impact on controllers
local slew_delta = 0 -- gain change increment to be applied to parameter being actively tuned
local slew_steps = 0 -- max number of times to increment parameter being actively tuned
local axes_done = {} -- list of axes that have been tuned
local filters_done = {} -- table recording if filters have been set for each axis
local gcs_pid_mask_done = {} -- table recording if GCS_PID_MASK has been set for each axis
@ -213,28 +192,15 @@ local param_saved = {} -- table holding backup of each paramete
local param_changed = {} -- table holding whether each param's gain has been saved
local need_restore = false -- true if any param's gain has been changed
-- check for an item within a table
-- returns true if found, false if not found
function in_skip_table(pname)
for _, skip_item in ipairs(params_skip) do
if pname == skip_item then
return true
end
end
return false
end
-- initialise params, params_axis and param_changed tables
function init_params_tables()
-- add parameters to params dictionary
for _, axis in ipairs(axis_names) do
for _, suffix in ipairs(param_suffixes) do
local pname = axis .. "_" .. suffix
if not in_skip_table(pname) then
params[pname] = bind_param(pname)
params_axis[pname] = axis
param_changed[pname] = false
end
params[pname] = bind_param(pname)
params_axis[pname] = axis
param_changed[pname] = false
end
end
@ -254,7 +220,8 @@ function reset_axes_done()
gcs_pid_mask_done[axis] = false
end
tune_done_time = nil
stage = stages[1]
init_steering_ff()
init_speed_ff()
end
-- get all current param values into param_saved dictionary
@ -313,8 +280,6 @@ function setup_gcs_pid_mask(axis)
GCS_PID_MASK:set(1)
elseif axis == "ATC_SPEED" then
GCS_PID_MASK:set(2)
elseif axis == "PSC_VEL" then
GCS_PID_MASK:set(64)
else
gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("RTun: setup_gcs_pid_mask received unhandled aixs %s", axis))
end
@ -352,93 +317,32 @@ function get_slew_rate(axis)
if axis == "ATC_SPEED" then
return speed_srate
end
if axis == "PSC_VEL" then
local velocity_srate = AR_PosControl:get_srate()
return velocity_srate
end
gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("RTUN: get_slew_rate unsupported axis:%s", axis))
return 0.0
end
-- move to next stage of tune
function advance_stage(axis)
-- move to next axis of tune
function advance_axis(axis)
local now_sec = get_time()
if stage == "D" then
stage = "P"
elseif stage == "P" then
stage = "FF"
else
local prev_axis = get_current_axis()
axes_done[axis] = true
gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: %s tuning done", axis))
stage = "D"
-- check for tune completion
if prev_axis ~= nil and get_current_axis() == nil then
gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: Tuning DONE"))
tune_done_time = now_sec
end
local prev_axis = get_current_axis()
axes_done[axis] = true
-- check for tune completion
if prev_axis ~= nil and get_current_axis() == nil then
gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: Tuning DONE"))
tune_done_time = now_sec
end
last_stage_change = now_sec
last_axis_change = now_sec
end
-- change a gain
-- change a gain, log and update user
function adjust_gain(pname, value)
-- sanity check parameter shouldn't be skipped
if in_skip_table(pname) then
gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("RTun: attempted to update skipped param %s", pname))
return
end
local P = params[pname]
local old_value = P:get()
need_restore = true
param_changed[pname] = true
P:set(value)
if string.sub(pname, -2) == "_P" then
-- if we are updating a P value we may also update I
local ffname = string.gsub(pname, "_P", "_FF")
local FF = params[ffname]
if FF:get() > 0 then
-- if we have any FF on an axis then we don't couple I to P,
-- usually we want I = FF for a one sectond time constant for trim
return
end
-- if PI_RATIO is non-zero then update I
local PI_ratio = RTUN_PI_RATIO:get()
if PI_ratio > 0 then
local iname = string.gsub(pname, "_P", "_I")
local I = params[iname]
new_I_gain = value/PI_ratio
I:set(new_I_gain)
param_changed[iname] = true
write_log(iname)
end
end
end
-- return gain multipler for one loop
function get_gain_mul()
return math.exp(math.log(2.0)/(UPDATE_RATE_HZ*RTUN_DOUBLE_TIME:get()))
end
-- setup parameter slewing. slewing some changes over 2 seconds reduces shock to controllers
function setup_slew_gain(pname, gain)
slew_parm = pname
slew_steps = UPDATE_RATE_HZ / 2
slew_delta = (gain - params[pname]:get()) / slew_steps
end
-- update parameter slewing. slewing some changes over 2 seconds reduces shock to controllers
function update_slew_gain()
if slew_parm ~= nil then
local P = params[slew_parm]
adjust_gain(slew_parm, P:get()+slew_delta)
write_log(slew_parm)
slew_steps = slew_steps - 1
-- check if slewing is complete
if slew_steps == 0 then
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: %s %.3f", slew_parm, P:get()))
slew_parm = nil
end
end
write_log(pname)
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.3f -> %.3f", pname, old_value, value))
end
-- log parameter, current gain and current slew rate
@ -449,25 +353,6 @@ function write_log(pname)
logger:write("RTUN","SRate,Gain,Param", "ffN", slew_rate, param_gain, pname)
end
-- return gain limits on a parameter, or 0 for no limit
function gain_limit(pname)
if pname == "ATC_SPEED_P" then
return RTUN_SPD_P_MAX:get()
elseif pname == "ATC_SPEED_D" then
return RTUN_SPD_D_MAX:get()
end
return 0.0
end
-- check if parameter's gain has reached its limit
function reached_limit(pname, gain)
local limit = gain_limit(pname)
if limit > 0.0 and gain >= limit then
return true
end
return false
end
-- initialise steering ff tuning
function init_steering_ff()
ff_steering_sum = 0
@ -476,7 +361,9 @@ function init_steering_ff()
end
-- run steering turn rate controller feedforward calibration
function update_steering_ff(pname)
-- ff_pname is the FF parameter being tuned
-- returns true once the tuning has completed
function update_steering_ff(ff_pname)
-- get steering, turn rate, throttle and speed
local steering_out, _ = vehicle:get_steering_and_throttle()
local turn_rate_rads = ahrs:get_gyro():z()
@ -502,7 +389,7 @@ function update_steering_ff(pname)
ff_turn_rate_sum = ff_turn_rate_sum + math.abs(turn_rate_rads)
ff_turn_rate_count = ff_turn_rate_count + 1
if (update_user) then
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: %s %.0f%% complete", pname, complete_pct))
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: %s %.0f%% complete", ff_pname, complete_pct))
end
else
if update_user then
@ -516,28 +403,38 @@ function update_steering_ff(pname)
-- check for completion of two rotations of turns data and 10 seconds
if complete_pct >= 100 then
local old_gain = params[pname]:get()
local new_gain = (ff_steering_sum / ff_turn_rate_sum) * (1.0-(RTUN_FF_GAINMARG:get()*0.01))
adjust_gain(pname, new_gain)
write_log(pname)
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.3f -> %.3f", pname, old_gain, new_gain))
local FF_new_gain = (ff_steering_sum / ff_turn_rate_sum) * RTUN_STR_FFRATIO:get()
adjust_gain(ff_pname, FF_new_gain)
-- set P gain
if RTUN_STR_P_RATIO:get() > 0 then
local pname = string.gsub(ff_pname, "_FF", "_P")
adjust_gain(pname, FF_new_gain * RTUN_STR_P_RATIO:get())
end
-- set I gain
if RTUN_STR_I_RATIO:get() > 0 then
local iname = string.gsub(ff_pname, "_FF", "_I")
adjust_gain(iname, FF_new_gain * RTUN_STR_I_RATIO:get())
end
-- set I gain equal to FF
local iname = string.gsub(pname, "_FF", "_I")
local I = params[iname]
local I_old_gain = I:get()
I:set(new_gain)
param_changed[iname] = true
write_log(iname)
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.3f -> %.3f", iname, I_old_gain, new_gain))
return true
end
return false
end
-- initialise speed ff tuning
function init_speed_ff()
ff_throttle_sum = 0
ff_speed_sum = 0
ff_speed_count = 0
end
-- run speed controller feedforward calibration
function update_speed_ff(pname)
-- ff_pname is the FF parameter being tuned
-- returns true once the tuning has completed
function update_speed_ff(ff_pname)
-- get steering, turn rate, throttle and speed
local _, throttle_out = vehicle:get_steering_and_throttle()
local velocity_ned = ahrs:get_velocity_NED()
@ -564,13 +461,13 @@ function update_speed_ff(pname)
ff_speed_sum = ff_speed_sum + speed
ff_speed_count = ff_speed_count + 1
if (update_user) then
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: %s %.0f%% complete", pname, complete_pct))
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: %s %.0f%% complete", ff_pname, complete_pct))
end
else
if update_user then
if not throttle_ok then
gcs:send_text(MAV_SEVERITY.WARNING, string.format("RTun: increase throttle (%d < %d)", math.floor(throttle_out * 100), math.floor(SPEED_FF_THROTTLE_MIN * 100)))
elseif not turnrate_ok then
elseif not speed_ok then
gcs:send_text(MAV_SEVERITY.WARNING, string.format("RTun: increase speed (%3.1f < %3.1f)", speed, SPEED_FF_SPEED_MIN))
end
end
@ -578,26 +475,28 @@ function update_speed_ff(pname)
-- check for 10 seconds of data
if complete_pct >= 100 then
local cruise_speed_old = params["CRUISE_SPEED"]:get()
local cruise_speed_new = ff_speed_sum / ff_speed_count
local cruise_throttle_old = params["CRUISE_THROTTLE"]:get()
local cruise_throttle_new = (ff_throttle_sum / ff_speed_count) * 100
local cruise_throttle_new = (ff_throttle_sum / ff_speed_count) * 100 * RTUN_SPD_FFRATIO:get()
adjust_gain("CRUISE_SPEED", cruise_speed_new)
adjust_gain("CRUISE_THROTTLE", cruise_throttle_new)
write_log("CRUISE_SPEED")
write_log("CRUISE_THROTTLE")
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.2f -> %.2f", "CRUISE_SPEED", cruise_speed_old, cruise_speed_new))
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.1f -> %.1f", "CRUISE_THROTTLE", cruise_throttle_old, cruise_throttle_new))
-- set I gain equal to FF
local iname = string.gsub(pname, "_FF", "_I")
local I = params[iname]
local I_old_gain = I:get()
local I_new_gain = ff_throttle_sum / ff_speed_sum
I:set(I_new_gain)
param_changed[iname] = true
write_log(iname)
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.3f -> %.3f", iname, I_old_gain, I_new_gain))
-- calculate FF equivalent gain (used for setting P and I below)
local speed_ff_equivalent = (ff_throttle_sum / ff_speed_sum) * RTUN_SPD_FFRATIO:get();
-- set P gain
if RTUN_SPD_P_RATIO:get() > 0 then
local pname = string.gsub(ff_pname, "_FF", "_P")
local P_new_gain = speed_ff_equivalent * RTUN_SPD_P_RATIO:get()
adjust_gain(pname, P_new_gain)
end
-- set I gain
if RTUN_SPD_I_RATIO:get() > 0 then
local iname = string.gsub(ff_pname, "_FF", "_I")
local I_new_gain = speed_ff_equivalent * RTUN_SPD_I_RATIO:get()
adjust_gain(iname, I_new_gain)
end
return true
end
@ -663,11 +562,8 @@ function update()
return
end
-- update param gains for params being slewed towards a target
update_slew_gain()
-- return if we have just changed stages to give time for oscillations to subside
if get_time() - last_stage_change < STAGE_DELAY then
if get_time() - last_axis_change < AXIS_CHANGE_DELAY then
return
end
@ -710,70 +606,21 @@ function update()
end
-- get parameter currently being tuned
local srate = get_slew_rate(axis)
local pname = axis .. "_" .. stage
local param = params[pname]
local pname = axis .. "_FF"
if stage == "FF" then
-- feedforward tuning
local ff_done
if axis == "ATC_STR_RAT" then
ff_done = update_steering_ff(pname)
elseif axis == "ATC_SPEED" then
ff_done = update_speed_ff(pname)
elseif axis == "PSC_VEL" then
-- position controller feed-forward is not tuned
ff_done = true
else
gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("RTun: unsupported FF tuning %s", pname))
ff_done = true
end
if ff_done then
gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: %s tuning done", pname))
advance_stage(axis)
end
-- feedforward tuning
local ff_done
if axis == "ATC_STR_RAT" then
ff_done = update_steering_ff(pname)
elseif axis == "ATC_SPEED" then
ff_done = update_speed_ff(pname)
else
local oscillating = srate > RTUN_OSC_SMAX:get()
local limited = reached_limit(pname, param:get())
if limited or oscillating then
local reduction = (100.0-RTUN_PD_GAINMARG:get())*0.01
if not oscillating then
reduction = 1.0
end
local new_gain = param:get() * reduction
local limit = gain_limit(pname)
if limit > 0.0 and new_gain > limit then
gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: %s passed limit (>%.3f)", pname, limit))
new_gain = limit
end
local old_gain = param_saved[pname]
if new_gain < old_gain and string.sub(pname,-2) == '_D' then
-- we are lowering a D gain from the original gain. Also lower the P gain by the same amount
-- so that we don't trigger P oscillation. We don't drop P by more than a factor of 2
local ratio = math.max(new_gain / old_gain, 0.5)
local P_name = string.gsub(pname, "_D", "_P")
local old_P = params[P_name]:get()
local new_P = old_P * ratio
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusting %s %.3f -> %.3f", P_name, old_P, new_P))
adjust_gain(P_name, new_P)
write_log(P_name)
end
-- slew gain change over 2 seconds to ease impact on controller
setup_slew_gain(pname, new_gain)
gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: %s tuning done %.3f", pname, new_gain))
advance_stage(axis)
else
local new_gain = param:get()*get_gain_mul()
if new_gain <= 0.0001 then
new_gain = 0.001
end
adjust_gain(pname, new_gain)
write_log(pname)
if get_time() - last_gain_report > 3 then
last_gain_report = get_time()
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: %s %.3f sr:%.2f", pname, new_gain, srate))
end
end
gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("RTun: unsupported FF tuning %s", pname))
ff_done = true
end
if ff_done then
gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: %s tuning done", pname))
advance_axis(axis)
end
end

79
libraries/AP_Scripting/applets/rover-quicktune.md
View File

@ -1,6 +1,6 @@
# Rover QuickTune
Rover QuickTune tunes the steering (aka turn rate), speed and position controller velocity gains for rovers and boats
Rover QuickTune tunes the steering (aka turn rate) and speed controller gains for rovers and boats
The script is designed to be used in Circle mode and updates the following parameters
@ -15,14 +15,11 @@ ATC_SPEED_I
ATC_SPEED_D
CRUISE_SPEED
CRUISE_THROTTLE
PSC_VEL_P
PSC_VEL_I
PSC_VEL_D
# How To Use
Install this script in the autopilot's SD card's APM/scripts directory
Set SCR_ENABLE to 1 and reboot the autopilot
Set RTUN_ENABLE to 1.
Set RTUN_ENABLE to 1 (default)
Set RCx_OPTION = 300 where "x" refers to the transmitter's 2 or 3 position switch
use to start/stop/save tuning. E.g. if channel 6 is used set RC6_OPTION = 300
@ -30,7 +27,7 @@ use to start/stop/save tuning. E.g. if channel 6 is used set RC6_OPTION = 300
If necessary, the RTUN_RC_FUNC parameter can be set to another number (e.g. 302 for scripting3)
to avoid RCx_OPTION conflicts with other scripts.
If only a 2-position switch is available set RTUN_AUTO_SAVE to 1
By default the tune is saved a few seconds after completion. To control saving of the tune manually set RTUN_AUTO_SAVE to 0
Arm the vehicle and switch to Circle mode
Optionally set CIRC_SPEED (or WP_SPEED) to about half the vehicle's max speed
@ -40,23 +37,15 @@ Note the above parmaters only take effect when the vehicle is switched into Circ
Move the RC switch to the middle position to begin the tuning process.
Text messages should appear on the ground station's messages area showing progress
For P and D gain tuning, the relevant gain is slowly raised until the vehicle begins to oscillate after which the gain is reduced and the script moves onto the next gain.
For FF tuning, the steering or throttle output are compared to the response for at least 10 seconds.
During tuning the steering or throttle output are compared to the response for at least 10 seconds.
A message may appear stating the steering, turn rate, throttle or speed are too low in which case
the vehicle speed should be increased (Mission Planner's Action tab "Change Speed" button may be used)
or the radius should be reduced. The velocity FF is not tuned (nor does it need to be).
or the radius should be reduced.
By default the gains will be tuned in this order:
- ATC_STR_RAT_D
- ATC_STR_RAT_P and I
- ATC_STR_RAT_FF
- ATC_SPEED_D
- ATC_SPEED_P
- CRUISE_SPEED and CRUISE_THROTTLE
- PSC_VEL_D
- PSC_VEL_P and I
- ATC_STR_RAT_FF, then ATC_STR_RAT_P and I are set to ratios of the FF
- CRUISE_SPEED and CRUISE_THROTTLE, then ATC_SPEED_P and I are set to ratios of the FF
The script will also adjust filter settings:
@ -67,7 +56,7 @@ Save the tune by raising the RC switch to the high position
If the RC switch is moved high (ie. Save Tune) before the tune is completed the tune will pause, and any parameters completed will be saved and the current value of the one being actively tuned will remain active. You can resume tuning by returning the switch again to the middle position. If the RC switch is moved to the low position, the parameter currently being tuned will be reverted but any previously saved parameters will remain.
If you move the switch to the low position at any time in the tune before using the Tune Save switch position, then all parameters will be reverted to their original values. Parameters will also be reverted if you disarm before saving.
If you move the switch to the low position at any time in the tune before gains are saved, then all parameters will be reverted to their original values. Parameters will also be reverted if you disarm before saving.
If the pilot gives steering or throttle input during tuning then tuning is paused for 4 seconds. Tuning restarts once the pilot returns to the input to the neutral position.
@ -86,50 +75,38 @@ By default RCx_OPTIONS of 300 (scripting1) is used
## RTUN_AXES
The axes that will be tuned. The default is 7 meaning steering, speed and velocity
This parameter is a bitmask, so set 1 to tune just steering. 2 for speed. 4 for velocity
The axes that will be tuned. The default is 3 meaning steering and speed
This parameter is a bitmask, so set 1 to tune just steering. 2 for just speed
## RTUN_DOUBLE_TIME
## RTUN_STR_FFRATIO
How quickly a gain is raised while tuning. This is the number of seconds
for the gain to double. Most users will want to leave this at the default of 10 seconds.
Ratio between measured response and FF gain. Raise this to get a higher FF gain
The default of 0.9 is good for most users.
## RTUN_PD_GAINMARG
## RTUN_STR_P_RATIO
The percentage P and D gain margin to use. Once the oscillation point is found
the gain is reduced by this percentage. The default of 80% is good for most users.
Ratio between steering FF and P gains. Raise this to get a higher P gain, 0 to leave P unchanged
The default of 0.2 is good for most users.
## RTUN_FF_GAINMARG
## RTUN_STR_I_RATIO
The percentage FF gain margin to use. Once the output and response are used to calculate
the ideal feed-forward, the value is reduced by this percentage. The default of 20% is good for most users.
Ratio between steering FF and I gains. Raise this to get a higher I gain, 0 to leave I unchanged
The default of 0.2 is good for most users.
## RTUN_OSC_SMAX
## RTUN_SPD_FFRATIO
The Oscillation threshold in Hertz for detecting oscillation
The default of 5Hz is good for most vehicles but on very large vehicles
you may wish to lower this. For a vehicle of 50kg a value of 3 is likely to be good.
For a vehicle of 100kg a value of 1.5 is likely to be good.
Ratio between measured response and CRUISE_THROTTLE value. Raise this to get a higher CRUISE_THROTTLE value
The default of 1.0 is good for most users.
You can tell you have this set too high if you still have visible
oscillations after a parameter has completed tuning. In that case
halve this parameter and try again.
## RTUN_SPD_P_RATIO
## RTUN_SPD_P_MAX
Ratio between speed FF and P gain. Raise this to get a higher P gain, 0 to leave P unchanged
The default of 1.0 is good for most users.
The speed controller P gain max (aka ATC_SPEED_P)
## RTUN_SPD_I_RATIO
## RTUN_SPD_D_MAX
The speed controller D gain max (aka ATC_SPEED_D)
## RTUN_PI_RATIO
The ratio for P to I. This should normally be 1, but on some large vehicles a value of up to 3 can be
used if the I term in the PID is causing too much phase lag.
If RTUN_RP_PI_RATIO is less than 1 then the I value will not be
changed at all when P is changed.
Ratio between speed FF and I gain. Raise this to get a higher I gain, 0 to leave I unchanged
The default of 1.0 is good for most users.
## RTUN_AUTO_FILTER