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
parent 5bac860a4f
commit ce7f04d44b
2 changed files with 149 additions and 325 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

353
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 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 ATC_SPEED_D
CRUISE_SPEED CRUISE_SPEED
CRUISE_THROTTLE CRUISE_THROTTLE
PSC_VEL_P
PSC_VEL_I
PSC_VEL_D
See the accompanying rover-quiktune.md file for instructions on how to use 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 -- global definitions
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} 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_PREFIX = "RTUN_"
local PARAM_TABLE_SIZE = 11
-- bind a parameter to a variable -- bind a parameter to a variable
function bind_param(name) function bind_param(name)
@ -41,7 +39,7 @@ function bind_add_param(name, idx, default_value)
end end
-- setup quicktune specific parameters -- 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 // @Param: RTUN_ENABLE
@ -56,66 +54,64 @@ local RTUN_ENABLE = bind_add_param('ENABLE', 1, 1)
// @Param: RTUN_AXES // @Param: RTUN_AXES
// @DisplayName: Rover Quicktune axes // @DisplayName: Rover Quicktune axes
// @Description: axes to tune // @Description: axes to tune
// @Bitmask: 0:Steering,1:Speed,2:Velocity // @Bitmask: 0:Steering,1:Speed
// @User: Standard // @User: Standard
--]] --]]
local RTUN_AXES = bind_add_param('AXES', 2, 7) local RTUN_AXES = bind_add_param('AXES', 2, 3)
--[[ --[[
// @Param: RTUN_DOUBLE_TIME // @Param: RTUN_STR_FFRATIO
// @DisplayName: Rover Quicktune doubling time // @DisplayName: Rover Quicktune Steering Rate FeedForward ratio
// @Description: Time to double a tuning parameter. Raise this for a slower tune. // @Description: Ratio between measured response and FF gain. Raise this to get a higher FF gain
// @Range: 5 20 // @Range: 0 1.0
// @Units: s
// @User: Standard // @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 // @Param: RTUN_STR_P_RATIO
// @DisplayName: Rover Quicktune P and D gain margin // @DisplayName: Rover Quicktune Steering FF to P ratio
// @Description: Reduction in P and D gain after oscillation detected. Raise this number to get a more conservative tune // @Description: Ratio between steering FF and P gains. Raise this to get a higher P gain, 0 to leave P unchanged
// @Range: 20 90 // @Range: 0 2.0
// @Units: %
// @User: Standard // @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 // @Param: RTUN_STR_I_RATIO
// @DisplayName: Rover Quicktune oscillation rate threshold // @DisplayName: Rover Quicktune Steering FF to I ratio
// @Description: Threshold for oscillation detection. A lower value will lead to a more conservative tune. // @Description: Ratio between steering FF and I gains. Raise this to get a higher I gain, 0 to leave I unchanged
// @Range: 1 10 // @Range: 0 2.0
// @User: Standard // @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 // @Param: RTUN_SPD_FFRATIO
// @DisplayName: Rover Quicktune Speed P max // @DisplayName: Rover Quicktune Speed FeedForward (equivalent) ratio
// @Description: Maximum value for speed P gain // @Description: Ratio between measured response and CRUISE_THROTTLE value. Raise this to get a higher CRUISE_THROTTLE value
// @Range: 0.1 10 // @Range: 0 1.0
// @User: Standard // @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 // @Param: RTUN_SPD_P_RATIO
// @DisplayName: Rover Quicktune Speed D max // @DisplayName: Rover Quicktune Speed FF to P ratio
// @Description: Maximum value for speed D gain // @Description: Ratio between speed FF and P gain. Raise this to get a higher P gain, 0 to leave P unchanged
// @Range: 0.001 1 // @Range: 0 2.0
// @User: Standard // @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 // @Param: RTUN_SPD_I_RATIO
// @DisplayName: Rover Quicktune PI ratio // @DisplayName: Rover Quicktune Speed FF to I ratio
// @Description: Ratio between P and I gains. Raise this to get a lower I gain, 0 to disable // @Description: Ratio between speed FF and I gain. Raise this to get a higher I gain, 0 to leave I unchanged
// @Range: 0.5 1.0 // @Range: 0 2.0
// @User: Standard // @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 // @Param: RTUN_AUTO_FILTER
@ -133,7 +129,7 @@ local RTUN_AUTO_FILTER = bind_add_param('AUTO_FILTER', 9, 1)
// @Units: s // @Units: s
// @User: Standard // @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 // @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) 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 -- other vehicle parameters used by this script
local INS_GYRO_FILTER = bind_param("INS_GYRO_FILTER") local INS_GYRO_FILTER = bind_param("INS_GYRO_FILTER")
local GCS_PID_MASK = bind_param("GCS_PID_MASK") local GCS_PID_MASK = bind_param("GCS_PID_MASK")
@ -164,7 +150,7 @@ local RCIN_THROTTLE = rc:get_channel(RCMAP_THROTTLE:get())
-- definitions -- definitions
local UPDATE_RATE_HZ = 40 -- this script updates at 40hz 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 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 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 local FLTT_MUL = 0.5 -- ATC_STR_RAT_FLTT set to 0.5 * INS_GYRO_FILTER
@ -179,19 +165,12 @@ function get_time()
end end
-- local variables -- 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 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_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 last_axis_change = get_time() -- time (in seconds) that axis last changed
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_pilot_input = get_time() -- time pilot last provided RC input 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 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 axes_done = {} -- list of axes that have been tuned
local filters_done = {} -- table recording if filters have been set for each axis 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 local gcs_pid_mask_done = {} -- table recording if GCS_PID_MASK has been set for each axis
@ -213,30 +192,17 @@ local param_saved = {} -- table holding backup of each paramete
local param_changed = {} -- table holding whether each param's gain has been saved 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 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 -- initialise params, params_axis and param_changed tables
function init_params_tables() function init_params_tables()
-- add parameters to params dictionary -- add parameters to params dictionary
for _, axis in ipairs(axis_names) do for _, axis in ipairs(axis_names) do
for _, suffix in ipairs(param_suffixes) do for _, suffix in ipairs(param_suffixes) do
local pname = axis .. "_" .. suffix local pname = axis .. "_" .. suffix
if not in_skip_table(pname) then
params[pname] = bind_param(pname) params[pname] = bind_param(pname)
params_axis[pname] = axis params_axis[pname] = axis
param_changed[pname] = false param_changed[pname] = false
end end
end end
end
-- add extra parameters to param dictionary -- add extra parameters to param dictionary
for _, extra_param_name in ipairs(params_extra) do for _, extra_param_name in ipairs(params_extra) do
@ -254,7 +220,8 @@ function reset_axes_done()
gcs_pid_mask_done[axis] = false gcs_pid_mask_done[axis] = false
end end
tune_done_time = nil tune_done_time = nil
stage = stages[1] init_steering_ff()
init_speed_ff()
end end
-- get all current param values into param_saved dictionary -- get all current param values into param_saved dictionary
@ -313,8 +280,6 @@ function setup_gcs_pid_mask(axis)
GCS_PID_MASK:set(1) GCS_PID_MASK:set(1)
elseif axis == "ATC_SPEED" then elseif axis == "ATC_SPEED" then
GCS_PID_MASK:set(2) GCS_PID_MASK:set(2)
elseif axis == "PSC_VEL" then
GCS_PID_MASK:set(64)
else else
gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("RTun: setup_gcs_pid_mask received unhandled aixs %s", axis)) gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("RTun: setup_gcs_pid_mask received unhandled aixs %s", axis))
end end
@ -352,93 +317,32 @@ function get_slew_rate(axis)
if axis == "ATC_SPEED" then if axis == "ATC_SPEED" then
return speed_srate return speed_srate
end 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)) gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("RTUN: get_slew_rate unsupported axis:%s", axis))
return 0.0 return 0.0
end end
-- move to next stage of tune -- move to next axis of tune
function advance_stage(axis) function advance_axis(axis)
local now_sec = get_time() local now_sec = get_time()
if stage == "D" then
stage = "P"
elseif stage == "P" then
stage = "FF"
else
local prev_axis = get_current_axis() local prev_axis = get_current_axis()
axes_done[axis] = true axes_done[axis] = true
gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: %s tuning done", axis))
stage = "D"
-- check for tune completion -- check for tune completion
if prev_axis ~= nil and get_current_axis() == nil then if prev_axis ~= nil and get_current_axis() == nil then
gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: Tuning DONE")) gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: Tuning DONE"))
tune_done_time = now_sec tune_done_time = now_sec
end end
end last_axis_change = now_sec
last_stage_change = now_sec
end end
-- change a gain -- change a gain, log and update user
function adjust_gain(pname, value) 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 P = params[pname]
local old_value = P:get()
need_restore = true need_restore = true
param_changed[pname] = true param_changed[pname] = true
P:set(value) P:set(value)
if string.sub(pname, -2) == "_P" then write_log(pname)
-- if we are updating a P value we may also update I gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.3f -> %.3f", pname, old_value, value))
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
end end
-- log parameter, current gain and current slew rate -- 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) logger:write("RTUN","SRate,Gain,Param", "ffN", slew_rate, param_gain, pname)
end 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 -- initialise steering ff tuning
function init_steering_ff() function init_steering_ff()
ff_steering_sum = 0 ff_steering_sum = 0
@ -476,7 +361,9 @@ function init_steering_ff()
end end
-- run steering turn rate controller feedforward calibration -- 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 -- get steering, turn rate, throttle and speed
local steering_out, _ = vehicle:get_steering_and_throttle() local steering_out, _ = vehicle:get_steering_and_throttle()
local turn_rate_rads = ahrs:get_gyro():z() 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_sum = ff_turn_rate_sum + math.abs(turn_rate_rads)
ff_turn_rate_count = ff_turn_rate_count + 1 ff_turn_rate_count = ff_turn_rate_count + 1
if (update_user) then 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 end
else else
if update_user then 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 -- check for completion of two rotations of turns data and 10 seconds
if complete_pct >= 100 then if complete_pct >= 100 then
local old_gain = params[pname]:get() local FF_new_gain = (ff_steering_sum / ff_turn_rate_sum) * RTUN_STR_FFRATIO:get()
local new_gain = (ff_steering_sum / ff_turn_rate_sum) * (1.0-(RTUN_FF_GAINMARG:get()*0.01)) adjust_gain(ff_pname, FF_new_gain)
adjust_gain(pname, new_gain)
write_log(pname) -- set P gain
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.3f -> %.3f", pname, old_gain, new_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 return true
end end
return false return false
end 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 -- 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 -- get steering, turn rate, throttle and speed
local _, throttle_out = vehicle:get_steering_and_throttle() local _, throttle_out = vehicle:get_steering_and_throttle()
local velocity_ned = ahrs:get_velocity_NED() 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_sum = ff_speed_sum + speed
ff_speed_count = ff_speed_count + 1 ff_speed_count = ff_speed_count + 1
if (update_user) then 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 end
else else
if update_user then if update_user then
if not throttle_ok 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))) 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)) gcs:send_text(MAV_SEVERITY.WARNING, string.format("RTun: increase speed (%3.1f < %3.1f)", speed, SPEED_FF_SPEED_MIN))
end end
end end
@ -578,26 +475,28 @@ function update_speed_ff(pname)
-- check for 10 seconds of data -- check for 10 seconds of data
if complete_pct >= 100 then 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_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 * RTUN_SPD_FFRATIO:get()
local cruise_throttle_new = (ff_throttle_sum / ff_speed_count) * 100
adjust_gain("CRUISE_SPEED", cruise_speed_new) adjust_gain("CRUISE_SPEED", cruise_speed_new)
adjust_gain("CRUISE_THROTTLE", cruise_throttle_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 -- calculate FF equivalent gain (used for setting P and I below)
local iname = string.gsub(pname, "_FF", "_I") local speed_ff_equivalent = (ff_throttle_sum / ff_speed_sum) * RTUN_SPD_FFRATIO:get();
local I = params[iname]
local I_old_gain = I:get() -- set P gain
local I_new_gain = ff_throttle_sum / ff_speed_sum if RTUN_SPD_P_RATIO:get() > 0 then
I:set(I_new_gain) local pname = string.gsub(ff_pname, "_FF", "_P")
param_changed[iname] = true local P_new_gain = speed_ff_equivalent * RTUN_SPD_P_RATIO:get()
write_log(iname) adjust_gain(pname, P_new_gain)
gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.3f -> %.3f", iname, I_old_gain, I_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 return true
end end
@ -663,11 +562,8 @@ function update()
return return
end 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 -- 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 return
end end
@ -710,70 +606,21 @@ function update()
end end
-- get parameter currently being tuned -- get parameter currently being tuned
local srate = get_slew_rate(axis) local pname = axis .. "_FF"
local pname = axis .. "_" .. stage
local param = params[pname]
if stage == "FF" then
-- feedforward tuning -- feedforward tuning
local ff_done local ff_done
if axis == "ATC_STR_RAT" then if axis == "ATC_STR_RAT" then
ff_done = update_steering_ff(pname) ff_done = update_steering_ff(pname)
elseif axis == "ATC_SPEED" then elseif axis == "ATC_SPEED" then
ff_done = update_speed_ff(pname) ff_done = update_speed_ff(pname)
elseif axis == "PSC_VEL" then
-- position controller feed-forward is not tuned
ff_done = true
else else
gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("RTun: unsupported FF tuning %s", pname)) gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("RTun: unsupported FF tuning %s", pname))
ff_done = true ff_done = true
end end
if ff_done then if ff_done then
gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: %s tuning done", pname)) gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: %s tuning done", pname))
advance_stage(axis) advance_axis(axis)
end
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
end end
end end

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

@ -1,6 +1,6 @@
# Rover QuickTune # 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 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 ATC_SPEED_D
CRUISE_SPEED CRUISE_SPEED
CRUISE_THROTTLE CRUISE_THROTTLE
PSC_VEL_P
PSC_VEL_I
PSC_VEL_D
# How To Use # How To Use
Install this script in the autopilot's SD card's APM/scripts directory Install this script in the autopilot's SD card's APM/scripts directory
Set SCR_ENABLE to 1 and reboot the autopilot 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 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 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) 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. 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 Arm the vehicle and switch to Circle mode
Optionally set CIRC_SPEED (or WP_SPEED) to about half the vehicle's max speed 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. 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 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. During tuning the steering or throttle output are compared to the response for at least 10 seconds.
For FF 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 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) 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: By default the gains will be tuned in this order:
- ATC_STR_RAT_D - ATC_STR_RAT_FF, then ATC_STR_RAT_P and I are set to ratios of the FF
- ATC_STR_RAT_P and I - CRUISE_SPEED and CRUISE_THROTTLE, then ATC_SPEED_P and I are set to ratios of the FF
- ATC_STR_RAT_FF
- ATC_SPEED_D
- ATC_SPEED_P
- CRUISE_SPEED and CRUISE_THROTTLE
- PSC_VEL_D
- PSC_VEL_P and I
The script will also adjust filter settings: 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 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. 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 ## RTUN_AXES
The axes that will be tuned. The default is 7 meaning steering, speed and 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 speed. 4 for velocity 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 Ratio between measured response and FF gain. Raise this to get a higher FF gain
for the gain to double. Most users will want to leave this at the default of 10 seconds. 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 Ratio between steering FF and P gains. Raise this to get a higher P gain, 0 to leave P unchanged
the gain is reduced by this percentage. The default of 80% is good for most users. 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 Ratio between steering FF and I gains. Raise this to get a higher I gain, 0 to leave I unchanged
the ideal feed-forward, the value is reduced by this percentage. The default of 20% is good for most users. The default of 0.2 is good for most users.
## RTUN_OSC_SMAX ## RTUN_SPD_FFRATIO
The Oscillation threshold in Hertz for detecting oscillation Ratio between measured response and CRUISE_THROTTLE value. Raise this to get a higher CRUISE_THROTTLE value
The default of 5Hz is good for most vehicles but on very large vehicles The default of 1.0 is good for most users.
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.
You can tell you have this set too high if you still have visible ## RTUN_SPD_P_RATIO
oscillations after a parameter has completed tuning. In that case
halve this parameter and try again.
## 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 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.
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.
## RTUN_AUTO_FILTER ## RTUN_AUTO_FILTER