Browse Source

AP_Scripting: pause quicktune during pilot input

thanks to Marco for the suggestion
apm_2208
Andrew Tridgell 3 years ago
parent
commit
c2af9a9154
  1. 29
      libraries/AP_Scripting/applets/VTOL-quicktune.lua
  2. 2
      libraries/AP_Scripting/applets/VTOL-quicktune.md

29
libraries/AP_Scripting/applets/VTOL-quicktune.lua

@ -38,8 +38,17 @@ local QUIK_YAW_D_MAX = bind_add_param('YAW_D_MAX', 7, 0.01) @@ -38,8 +38,17 @@ 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
@ -72,6 +81,7 @@ local stages = { "D", "P" } @@ -72,6 +81,7 @@ 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
@ -157,6 +167,16 @@ function setup_filters() @@ -157,6 +167,16 @@ function setup_filters()
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()
@ -294,6 +314,11 @@ function update() @@ -294,6 +314,11 @@ function update()
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
@ -336,6 +361,10 @@ function update() @@ -336,6 +361,10 @@ function update()
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]

2
libraries/AP_Scripting/applets/VTOL-quicktune.md

@ -114,3 +114,5 @@ If you move the switch to the low position at any time in the tune @@ -114,3 +114,5 @@ 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.

Loading…
Cancel
Save