From 274d526a693770cd6ad73544e2865245e948edd1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 30 Apr 2022 19:26:24 +1000
Subject: [PATCH] AP_Scripting: added quicktune.lua script

rapid tuning for VTOL control
---
 .../AP_Scripting/applets/VTOL-quicktune.lua   | 418 ++++++++++++++++++
 .../AP_Scripting/applets/VTOL-quicktune.md    | 118 +++++
 2 files changed, 536 insertions(+)
 create mode 100644 libraries/AP_Scripting/applets/VTOL-quicktune.lua
 create mode 100644 libraries/AP_Scripting/applets/VTOL-quicktune.md

diff --git a/libraries/AP_Scripting/applets/VTOL-quicktune.lua b/libraries/AP_Scripting/applets/VTOL-quicktune.lua
new file mode 100644
index 0000000000..706f4f2a83
--- /dev/null
+++ b/libraries/AP_Scripting/applets/VTOL-quicktune.lua
@@ -0,0 +1,418 @@
+--[[
+ fast tuning of VTOL gains for multirotors and quadplanes
+
+ This should be used in QLOITER mode for quadplanes and LOITER mode
+ for copters, although it will work in other VTOL modes
+
+--]]
+
+local PARAM_TABLE_KEY = 8
+local PARAM_TABLE_PREFIX = "QUIK_"
+
+local is_quadplane = false
+local atc_prefix = "ATC"
+
+-- bind a parameter to a variable
+function bind_param(name)
+   local p = Parameter()
+   assert(p:init(name), string.format('could not find %s parameter', name))
+   return p
+end
+
+-- add a parameter and bind it to a variable
+function bind_add_param(name, idx, default_value)
+   assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
+   return bind_param(PARAM_TABLE_PREFIX .. name)
+end
+
+-- setup quicktune specific parameters
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 7), 'could not add param table')
+
+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_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 INS_GYRO_FILTER  = bind_param("INS_GYRO_FILTER")
+
+local RCMAP_ROLL       = bind_param("RCMAP_ROLL")
+local RCMAP_PITCH      = bind_param("RCMAP_PITCH")
+local RCMAP_YAW        = bind_param("RCMAP_YAW")
+
+local RCIN_ROLL  = rc:get_channel(RCMAP_ROLL:get())
+local RCIN_PITCH = rc:get_channel(RCMAP_PITCH:get())
+local RCIN_YAW   = rc:get_channel(RCMAP_YAW:get())
+
+local UPDATE_RATE_HZ = 40
+local STAGE_DELAY = 4.0
+local PILOT_INPUT_DELAY = 4.0
+
+local YAW_FLTE_MAX = 2.0
+local FLTD_MUL = 0.5
+local FLTT_MUL = 0.5
+
+-- SMAX to set if none set at start
+local DEFAULT_SMAX = 50.0
+
+-- work out vehicle type
+if param:get("Q_A_RAT_RLL_SMAX") then
+   is_quadplane = true
+   atc_prefix = "Q_A"
+   gcs:send_text(0, "Quicktune for quadplane loaded")
+elseif param:get("ATC_RAT_RLL_SMAX") then
+   is_quadplane = false
+   gcs:send_text(0, "Quicktune for multicopter loaded")
+else
+   gcs:send_text(0, "Quicktune unknown vehicle")
+   return
+end
+
+-- get time in sections since boot
+function get_time()
+   return millis():tofloat() * 0.001
+end
+
+local axis_names = { "RLL", "PIT", "YAW" }
+local param_suffixes = { "FF", "P", "I", "D", "SMAX", "FLTT", "FLTD", "FLTE" }
+local stages = { "D", "P" }
+local stage = stages[1]
+local last_stage_change = get_time()
+local last_gain_report = get_time()
+local last_pilot_input = get_time()
+local slew_parm = nil
+local slew_target = 0
+local slew_delta = 0
+
+local axes_done = {}
+
+
+-- create params dictionary indexed by name, such as "RLL_P"
+local params = {}
+local param_saved = {}
+local param_changed = {}
+local need_restore = false
+
+for i, axis in ipairs(axis_names) do
+   for j, suffix in ipairs(param_suffixes) do
+      local pname = axis .. "_" .. suffix
+      params[pname] = bind_param(atc_prefix .. "_" .. "RAT_" .. pname)
+      param_changed[pname] = false
+   end
+end
+
+-- reset to start
+function reset_axes_done()
+   for i, axis in ipairs(axis_names) do
+      axes_done[axis] = false
+   end
+   stage = stages[1]
+end
+
+-- get all current param values into param_saved dictionary
+function get_all_params()
+   for pname in pairs(params) do
+      param_saved[pname] = params[pname]:get()
+   end
+end
+
+-- restore all param values from param_saved dictionary
+function restore_all_params()
+   for pname in pairs(params) do
+      local changed = param_changed[pname] and 1 or 0
+      if param_changed[pname] then
+         params[pname]:set(param_saved[pname])
+         param_changed[pname] = false
+      end
+   end
+end
+
+-- save all param values to storage
+function save_all_params()
+   for pname in pairs(params) do
+      if param_changed[pname] then
+         params[pname]:set_and_save(params[pname]:get())
+         param_saved[pname] = params[pname]:get()
+         param_changed[pname] = false
+      end
+   end
+end
+
+-- setup a default SMAX if zero
+function setup_SMAX()
+   for i, axis in ipairs(axis_names) do
+      local smax = axis .. "_SMAX"
+      if params[smax]:get() <= 0 then
+         adjust_gain(smax, DEFAULT_SMAX)
+      end
+   end
+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
+      end
+   end
+end
+
+-- check for pilot input to pause tune
+function have_pilot_input()
+   if (math.abs(RCIN_ROLL:norm_input_dz()) > 0 or
+       math.abs(RCIN_PITCH:norm_input_dz()) > 0 or
+       math.abs(RCIN_YAW:norm_input_dz()) > 0) then
+      return true
+   end
+   return false
+end
+
+reset_axes_done()
+get_all_params()
+
+-- 3 position switch
+local quick_switch = nil
+
+-- get the axis name we are working on, or nil for all done
+function get_current_axis()
+   local axes = QUIK_AXES:get()
+   for i = 1, #axis_names do
+      local mask = (1 << (i-1))
+      local axis_name = axis_names[i]
+      if (mask & axes) ~= 0 and axes_done[axis_name] == false then
+         return axis_names[i]
+      end
+   end
+   return nil
+end
+
+-- get slew rate for an axis
+function get_slew_rate(axis)
+   local roll_srate, pitch_srate, yaw_srate = AC_AttitudeControl:get_rpy_srate()
+   if axis == "RLL" then
+      return roll_srate
+   end
+   if axis == "PIT" then
+      return pitch_srate
+   end
+   if axis == "YAW" then
+      return yaw_srate
+   end
+   return 0.0
+end
+
+-- move to next stage of tune
+function advance_stage(axis)
+   if stage == "D" then
+      stage = "P"
+   else
+      axes_done[axis] = true
+      stage = "D"
+   end
+end
+
+-- get param name, such as RLL_P, used as index into param dictionaries
+function get_pname(axis, stage)
+   return axis .. "_" .. stage
+end
+
+-- change a gain
+function adjust_gain(pname, value)
+   local P = params[pname]
+   need_restore = true
+   param_changed[pname] = true
+   P:set(value)
+   if string.sub(pname, -2) == "_P" then
+      -- also change I gain
+      local iname = string.gsub(pname, "_P", "_I")
+      local ffname = string.gsub(pname, "_P", "_FF")
+      local I = params[iname]
+      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
+      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)
+      end
+   end
+end
+
+-- return gain multipler for one loop
+function get_gain_mul()
+   return math.exp(math.log(2.0)/(UPDATE_RATE_HZ*QUIK_DOUBLE_TIME:get()))
+end
+
+function setup_slew_gain(pname, gain)
+   slew_parm = pname
+   slew_target = gain
+   slew_steps = UPDATE_RATE_HZ/2
+   slew_delta = (gain - params[pname]:get()) / slew_steps
+end
+
+function update_slew_gain()
+   if slew_parm ~= nil then
+      local P = params[slew_parm]
+      local axis = string.sub(slew_parm, 1, 3)
+      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()))
+         slew_parm = nil
+      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 suffix == "_P" then
+         return QUIK_YAW_P_MAX:get()
+      end
+      if suffix == "_D" then
+         return QUIK_YAW_D_MAX:get()
+      end
+   end
+   return 0.0
+end
+
+function reached_limit(pname, gain)
+   local limit = gain_limit(pname)
+   if limit > 0.0 and gain >= limit then
+      return true
+   end
+   return false
+end
+
+-- main update function
+function update()
+   if quick_switch == nil then
+      quick_switch = rc:find_channel_for_option(300)
+   end
+   if quick_switch == nil or QUIK_ENABLE:get() < 1 then
+      return
+   end
+
+   if have_pilot_input() then
+      last_pilot_input = get_time()
+   end
+
+   local sw_pos = quick_switch:get_aux_switch_pos()
+   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"))
+      end
+      reset_axes_done()
+      return
+   end
+   if sw_pos == 2 then
+      -- save all params
+      if need_restore then
+         need_restore = false
+         save_all_params()
+         gcs:send_text(0, string.format("Tuning: saved"))
+      end
+   end
+   if sw_pos ~= 1 then
+      return
+   end
+
+   if get_time() - last_stage_change < STAGE_DELAY then
+      update_slew_gain()
+      return
+   end
+
+   axis = get_current_axis()
+   if axis == nil then
+      -- nothing left to do
+      return
+   end
+
+   if not need_restore then
+      -- we are just starting tuning, get current values
+      gcs:send_text(0, 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
+
+   local srate = get_slew_rate(axis)
+   local pname = get_pname(axis, stage)
+   local P = params[pname]
+   local oscillating = srate > QUIK_OSC_SMAX:get()
+   local limited = reached_limit(pname, P:get())
+   if limited or oscillating then
+      local reduction = (100.0-QUIK_GAIN_MARGIN:get())*0.01
+      if not oscillating then
+         reduction = 1.0
+      end
+      local new_gain = P:get() * reduction
+      local limit = gain_limit(pname)
+      if limit > 0.0 and new_gain > limit then
+         new_gain = limit
+      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))
+      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
+         new_gain = 0.001
+      end
+      adjust_gain(pname, new_gain)
+      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))
+      end
+   end
+end
+
+-- wrapper around update(). This calls update() at 10Hz,
+-- and if update faults then an error is displayed, but the script is not
+-- stopped
+function protected_wrapper()
+  local success, err = pcall(update)
+  if not success then
+     gcs:send_text(0, "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
+     return
+  end
+  return protected_wrapper, 1000/UPDATE_RATE_HZ
+end
+
+-- start running update loop
+return protected_wrapper()
diff --git a/libraries/AP_Scripting/applets/VTOL-quicktune.md b/libraries/AP_Scripting/applets/VTOL-quicktune.md
new file mode 100644
index 0000000000..b08ad94c42
--- /dev/null
+++ b/libraries/AP_Scripting/applets/VTOL-quicktune.md
@@ -0,0 +1,118 @@
+# VTOL QuickTune
+
+This script implements a fast VTOL tuning system for multicopters and
+quadplanes. This script can be used to automate the process of
+producing a good "manual tune" for the VTOL rate control parameters.
+
+The script is designed to be used in QLOITER mode for quadplanes or
+LOITER mode in multicopters, although it can also be used in other
+VTOL modes.
+
+# Parameters
+
+The script adds 7 parameters to control it's behaviour. The parameters
+are:
+
+## QUIK_ENABLE
+
+this must be set to 1 to enable the script
+
+## QUIK_AXES
+
+This is the set of axes that the tune will run on. The default is 7,
+which means roll, pitch and yaw. It is a bitmask, so if you want just
+roll and pitch then set this to 3. For just yaw you would set it to 4.
+
+## QUIK_DOUBLE_TIME
+
+This controls how quickly a gain is raised while tuning. It is a time
+in seconds for the gain to double. Most users will want to leave this
+at the default of 10 seconds.
+
+## QUIK_GAIN_MARGIN
+
+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.
+
+## QUIK_OSC_SMAX
+
+This is the oscillation threshold in Hertz for detecting oscillation
+when a gain is raised. 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.
+
+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.
+
+## QUIK_YAW_P_MAX
+
+This sets a limit on the YAW_P rate gain. The yaw axis on most
+multirotor style vehicles needs to have a much lower limit on the P
+gain than the oscillation limit to ensure that enough control remains
+for roll, pitch and thrust. A maximum of 0.5 is good for most VTOL
+vehicles.
+
+## QUIK_YAW_D_MAX
+
+This sets a limit on the YAW_D rate gain. The yaw axis on most
+multirotor style vehicles needs to have a much lower limit on the D
+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.
+
+# Operation
+
+First you should setup harmonic notch filtering using the guide in the
+ArduPilot wiki. This tuning system relies on you already having
+reduced gyro noise using the harmonic notch filter. It will fail if
+your noise is too high.
+
+Install the lua script in the APM/SCRIPTS directory on the flight
+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.
+
+You should then takeoff and put the vehicle into QLOITER mode (for
+quadplanes) or LOITER mode (for multicopters) and have it in a steady
+hover in low wind.
+
+Then move the control switch you setup with option 300 to the middle
+position. This will start the tuning process. You will see text
+messages on the ground station showing the progress of the tune. As
+the aircraft reaches the oscillation limit of each parameter it will
+start a small oscillation, then it will reduce that gain by the
+configured QUIK_GAIN_MARGIN percentage and then move onto the next
+parameter.
+
+With default settings the parameters to be tuned will be:
+
+ - RLL_D
+ - RLL_P
+ - PIT_D
+ - PIT_P
+ - YAW_D
+ - YAW_P
+
+The script will also adjust filter settings using the following rules:
+
+ - the FLTD and FLTT settings will be set to half of the INS_GYRO_FILTER value
+ - the YAW_FLTE filter will be set to a maximum of 2Hz
+ - 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.
+
+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 the pilot gives roll, pitch or yaw input while tuning then the tune
+is paused until 4 seconds after the pilot input stops.