Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Scripting: sync quicktune with master

Browse Source
This commit is contained in:
Andrew Tridgell 2022-06-17 08:02:42 +10:00 committed by Randy Mackay
parent 54e6aa7d34
commit 9535e3de41
2 changed files with 154 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

154
libraries/AP_Scripting/applets/VTOL-quicktune.lua
View File

@ -6,6 +6,17 @@
--]]
--[[
- TODO:
- disable weathervaning while tuning?
- possibly lower P XY gains during tuning?
- bail out on a large angle error?
--]]
local MAV_SEVERITY_INFO = 6
local MAV_SEVERITY_NOTICE = 5
local MAV_SEVERITY_EMERGENCY = 0
local PARAM_TABLE_KEY = 8
local PARAM_TABLE_PREFIX = "QUIK_"
@ -26,15 +37,19 @@ function bind_add_param(name, idx, default_value)
end
-- setup quicktune specific parameters
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 7), 'could not add param table')
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 11), 'could not add param table')
local QUIK_ENABLE = bind_add_param('ENABLE', 1, 0)
local QUIK_ENABLE = bind_add_param('ENABLE', 1, 0)
local QUIK_AXES = bind_add_param('AXES', 2, 7)
local QUIK_DOUBLE_TIME = bind_add_param('DOUBLE_TIME', 3, 10)
local QUIK_GAIN_MARGIN = bind_add_param('GAIN_MARGIN', 4, 70)
local QUIK_GAIN_MARGIN = bind_add_param('GAIN_MARGIN', 4, 60)
local QUIK_OSC_SMAX = bind_add_param('OSC_SMAX', 5, 5)
local QUIK_YAW_P_MAX = bind_add_param('YAW_P_MAX', 6, 0.5)
local QUIK_YAW_D_MAX = bind_add_param('YAW_D_MAX', 7, 0.01)
local QUIK_RP_PI_RATIO = bind_add_param('RP_PI_RATIO', 8, 1.0)
local QUIK_Y_PI_RATIO = bind_add_param('Y_PI_RATIO', 9, 10)
local QUIK_AUTO_FILTER = bind_add_param('AUTO_FILTER', 10, 1)
local QUIK_AUTO_SAVE = bind_add_param('AUTO_SAVE', 11, 0)
local INS_GYRO_FILTER = bind_param("INS_GYRO_FILTER")
@ -61,16 +76,16 @@ local DEFAULT_SMAX = 50.0
if param:get("Q_A_RAT_RLL_SMAX") then
is_quadplane = true
atc_prefix = "Q_A"
gcs:send_text(0, "Quicktune for quadplane loaded")
gcs:send_text(MAV_SEVERITY_EMERGENCY, "Quicktune for quadplane loaded")
elseif param:get("ATC_RAT_RLL_SMAX") then
is_quadplane = false
gcs:send_text(0, "Quicktune for multicopter loaded")
gcs:send_text(MAV_SEVERITY_EMERGENCY, "Quicktune for multicopter loaded")
else
gcs:send_text(0, "Quicktune unknown vehicle")
gcs:send_text(MAV_SEVERITY_EMERGENCY, "Quicktune unknown vehicle")
return
end
-- get time in sections since boot
-- get time in seconds since boot
function get_time()
return millis():tofloat() * 0.001
end
@ -82,11 +97,14 @@ local stage = stages[1]
local last_stage_change = get_time()
local last_gain_report = get_time()
local last_pilot_input = get_time()
local last_notice = get_time()
local tune_done_time = nil
local slew_parm = nil
local slew_target = 0
local slew_delta = 0
local axes_done = {}
local filters_done = {}
-- create params dictionary indexed by name, such as "RLL_P"
@ -107,6 +125,7 @@ end
function reset_axes_done()
for i, axis in ipairs(axis_names) do
axes_done[axis] = false
filters_done[axis] = false
end
stage = stages[1]
end
@ -151,20 +170,19 @@ function setup_SMAX()
end
-- setup filter frequencies
function setup_filters()
for i, axis in ipairs(axis_names) do
local fltd = axis .. "_FLTD"
local fltt = axis .. "_FLTT"
local flte = axis .. "_FLTE"
adjust_gain(fltt, INS_GYRO_FILTER:get() * FLTT_MUL)
adjust_gain(fltd, INS_GYRO_FILTER:get() * FLTD_MUL)
if axis == "YAW" then
local FLTE = params[flte]
if FLTE:get() <= 0.0 or FLTE:get() > YAW_FLTE_MAX then
adjust_gain(flte, YAW_FLTE_MAX)
end
function setup_filters(axis)
local fltd = axis .. "_FLTD"
local fltt = axis .. "_FLTT"
local flte = axis .. "_FLTE"
adjust_gain(fltt, INS_GYRO_FILTER:get() * FLTT_MUL)
adjust_gain(fltd, INS_GYRO_FILTER:get() * FLTD_MUL)
if axis == "YAW" then
local FLTE = params[flte]
if FLTE:get() <= 0.0 or FLTE:get() > YAW_FLTE_MAX then
adjust_gain(flte, YAW_FLTE_MAX)
end
end
filters_done[axis] = true
end
-- check for pilot input to pause tune
@ -183,6 +201,18 @@ get_all_params()
-- 3 position switch
local quick_switch = nil
function axis_enabled(axis)
local axes = QUIK_AXES:get()
for i = 1, #axis_names do
local mask = (1 << (i-1))
local axis_name = axis_names[i]
if axis == axis_name and (mask & axes) ~= 0 then
return true
end
end
return false
end
-- get the axis name we are working on, or nil for all done
function get_current_axis()
local axes = QUIK_AXES:get()
@ -217,6 +247,7 @@ function advance_stage(axis)
stage = "P"
else
axes_done[axis] = true
gcs:send_text(5, string.format("Tuning: %s done", axis))
stage = "D"
end
end
@ -226,6 +257,16 @@ function get_pname(axis, stage)
return axis .. "_" .. stage
end
-- get axis name from parameter name
function param_axis(pname)
return string.sub(pname, 1, 3)
end
-- get parameter suffix from parameter name
function param_suffix(pname)
return string.sub(pname, 4)
end
-- change a gain
function adjust_gain(pname, value)
local P = params[pname]
@ -244,12 +285,14 @@ function adjust_gain(pname, value)
return
end
param_changed[iname] = true
if string.sub(pname, 1, 3) == "YAW" then
-- for yaw, I is 1/10 of P
I:set(value*0.1)
else
-- otherwise I == P
I:set(value)
-- work out ratio of P to I that we want
local PI_ratio = QUIK_RP_PI_RATIO:get()
if param_axis(pname) == "YAW" then
PI_ratio = QUIK_Y_PI_RATIO:get()
end
if PI_ratio >= 1 then
I:set(value/PI_ratio)
end
end
end
@ -269,22 +312,26 @@ end
function update_slew_gain()
if slew_parm ~= nil then
local P = params[slew_parm]
local axis = string.sub(slew_parm, 1, 3)
local axis = param_axis(slew_parm)
local ax_stage = string.sub(slew_parm, -1)
adjust_gain(slew_parm, P:get()+slew_delta)
slew_steps = slew_steps - 1
logger.write('QUIK','SRate,Gain,Param', 'ffn', get_slew_rate(axis), P:get(), axis .. ax_stage)
if slew_steps == 0 then
gcs:send_text(0, string.format("%s %.4f", slew_parm, P:get()))
gcs:send_text(MAV_SEVERITY_INFO, string.format("%s %.4f", slew_parm, P:get()))
slew_parm = nil
if get_current_axis() == nil then
gcs:send_text(MAV_SEVERITY_NOTICE, string.format("Tuning: DONE"))
tune_done_time = get_time()
end
end
end
end
-- return gain limits on a parameter, or 0 for no limit
function gain_limit(pname)
if string.sub(pname, 1, 3) == "YAW" then
local suffix = string.sub(pname, -2)
if param_axis(pname) == "YAW" then
local suffix = param_suffix(pname)
if suffix == "_P" then
return QUIK_YAW_P_MAX:get()
end
@ -304,6 +351,7 @@ function reached_limit(pname, gain)
end
-- main update function
local last_warning = get_time()
function update()
if quick_switch == nil then
quick_switch = rc:find_channel_for_option(300)
@ -317,12 +365,18 @@ function update()
end
local sw_pos = quick_switch:get_aux_switch_pos()
if sw_pos == 1 and (not arming:is_armed() or not vehicle:get_likely_flying()) and get_time() > last_warning + 5 then
gcs:send_text(MAV_SEVERITY_EMERGENCY, string.format("Tuning: Must be flying to tune"))
last_warning = get_time()
return
end
if sw_pos == 0 or not arming:is_armed() or not vehicle:get_likely_flying() then
-- abort, revert parameters
if need_restore then
need_restore = false
restore_all_params()
gcs:send_text(0, string.format("Tuning: reverted"))
gcs:send_text(MAV_SEVERITY_EMERGENCY, string.format("Tuning: reverted"))
tune_done_time = nil
end
reset_axes_done()
return
@ -332,7 +386,7 @@ function update()
if need_restore then
need_restore = false
save_all_params()
gcs:send_text(0, string.format("Tuning: saved"))
gcs:send_text(MAV_SEVERITY_NOTICE, string.format("Tuning: saved"))
end
end
if sw_pos ~= 1 then
@ -346,22 +400,34 @@ function update()
axis = get_current_axis()
if axis == nil then
-- nothing left to do
-- nothing left to do, check autosave time
if tune_done_time ~= nil and QUIK_AUTO_SAVE:get() > 0 then
if get_time() - tune_done_time > QUIK_AUTO_SAVE:get() then
need_restore = false
save_all_params()
gcs:send_text(MAV_SEVERITY_NOTICE, string.format("Tuning: saved"))
tune_done_time = nil
end
end
return
end
if not need_restore then
-- we are just starting tuning, get current values
gcs:send_text(0, string.format("Tuning: starting tune"))
gcs:send_text(MAV_SEVERITY_NOTICE, string.format("Tuning: starting tune"))
get_all_params()
setup_SMAX()
setup_filters()
end
if get_time() - last_pilot_input < PILOT_INPUT_DELAY then
return
end
if not filters_done[axis] then
gcs:send_text(MAV_SEVERITY_INFO, string.format("Starting %s tune", axis))
setup_filters(axis)
end
local srate = get_slew_rate(axis)
local pname = get_pname(axis, stage)
local P = params[pname]
@ -377,14 +443,22 @@ function update()
if limit > 0.0 and new_gain > limit then
new_gain = limit
end
local old_gain = param_saved[pname]
if new_gain < old_gain and string.sub(pname,-2) == '_D' and param_axis(pname) ~= 'YAW' 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("adjusting %s %.3f -> %.3f", P_name, old_P, new_P))
adjust_gain(P_name, new_P)
end
setup_slew_gain(pname, new_gain)
logger.write('QUIK','SRate,Gain,Param', 'ffn', srate, P:get(), axis .. stage)
gcs:send_text(0, string.format("Tuning: %s done", pname))
gcs:send_text(6, string.format("Tuning: %s done", pname))
advance_stage(axis)
last_stage_change = get_time()
if get_current_axis() == nil then
gcs:send_text(0, string.format("Tuning: DONE"))
end
else
local new_gain = P:get()*get_gain_mul()
if new_gain <= 0.0001 then
@ -394,7 +468,7 @@ function update()
logger.write('QUIK','SRate,Gain,Param', 'ffn', srate, P:get(), axis .. stage)
if get_time() - last_gain_report > 3 then
last_gain_report = get_time()
gcs:send_text(0, string.format("%s %.4f sr:%.2f", pname, new_gain, srate))
gcs:send_text(MAV_SEVERITY_INFO, string.format("%s %.4f sr:%.2f", pname, new_gain, srate))
end
end
end
@ -405,7 +479,7 @@ end
function protected_wrapper()
local success, err = pcall(update)
if not success then
gcs:send_text(0, "Internal Error: " .. err)
gcs:send_text(MAV_SEVERITY_EMERGENCY, "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

49
libraries/AP_Scripting/applets/VTOL-quicktune.md
View File

@ -33,7 +33,7 @@ at the default of 10 seconds.
This is the percentage gain margin to use. Once the oscillation point
for a gain is found the gain is reduced by this percentage. The
default of 70% is good for most users.
default of 60% is good for most users.
## QUIK_OSC_SMAX
@ -63,6 +63,37 @@ gain than the oscillation limit to ensure that enough control remains
for roll, pitch and thrust. A maximum of 0.01 is good for most VTOL
vehicles.
## QUIK_RP_PI_RATIO
This is the ratio for P to I for roll and pitch axes. 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 QUIK_RP_PI_RATIO is less than 1 then the I value will not be
changed at all when P is changed.
## QUIK_Y_PI_RATIO
This is the ratio for P to I for the yax axis. This should
normally be 10, but a different value may be needed on some vehicle
types.
If QUIK_Y_PI_RATIO is less than 1 then the I value will not be
changed at all when P is changed.
## QUIK_AUTO_FILTER
This enables automatic setting of the PID filters based on the
INS_GYRO_FILTER value. Set to zero to disable this feature.
## QUIK_AUTO_SAVE
This enables automatic saving of the tune if this number of seconds
pass after the end of the tune without reverting the tune. Setting
this to a non-zero value allows you to use quicktune with a 2-position
switch, with the switch settings as low and mid positions. A zero
value disables auto-save and you need to have a 3 position switch.
# Operation
First you should setup harmonic notch filtering using the guide in the
@ -75,9 +106,10 @@ controllers microSD card, then set SCR_ENABLE to 1. Reboot, and
refresh parameters. Then set QUIK_ENABLE to 1.
You will then need to setup a 3 position switch on an available RC
input channel for controlling the tune. If for example channel 6 is
available with a 3 position switch then you should set RC6_OPTION=300
to association the tuning control with that switch.
input channel for controlling the tune (or 2 position if you set
QUIK_AUTO_SAVE). If for example channel 6 is available with a 3
position switch then you should set RC6_OPTION=300 to association the
tuning control with that switch.
You should then takeoff and put the vehicle into QLOITER mode (for
quadplanes) or LOITER mode (for multicopters) and have it in a steady
@ -107,12 +139,11 @@ The script will also adjust filter settings using the following rules:
- if no SMAX is set for a rate controller than the SMAX will be set to 50Hz
Once the tuning is finished you will see a "Tuning: done" message. You
can save the tune by moving the switch to the high position. You
should do this to save before you land and disarm.
can save the tune by moving the switch to the high position (Tune Save). You
should do this to save before you land and disarm. If you save 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, or if 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
then all parameters will be reverted to their original
values. Parameters will also be reverted if you disarm.
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 the pilot gives roll, pitch or yaw input while tuning then the tune
is paused until 4 seconds after the pilot input stops.