AP_Scripting: added QUIK_ANGLE_MAX for lua quicktune

this brings the same protection to the lua script as was added for the
C++ implementation
This commit is contained in:
Andrew Tridgell 2024-12-17 08:35:44 +11:00
parent 9503744e6a
commit 613b835d67
1 changed files with 34 additions and 1 deletions

View File

@ -19,6 +19,7 @@
local MAV_SEVERITY_INFO = 6
local MAV_SEVERITY_NOTICE = 5
local MAV_SEVERITY_CRITICAL = 2
local MAV_SEVERITY_EMERGENCY = 0
local PARAM_TABLE_KEY = 8
@ -40,7 +41,7 @@ function bind_add_param(name, idx, default_value)
end
-- setup quicktune specific parameters
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 14), 'could not add param table')
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 15), 'could not add param table')
--[[
// @Param: QUIK_ENABLE
@ -170,6 +171,15 @@ local QUIK_MAX_REDUCE = bind_add_param('MAX_REDUCE', 13, 20)
--]]
local QUIK_OPTIONS = bind_add_param('OPTIONS', 14, 0)
--[[
// @Param: QUIK_ANGLE_MAX
// @DisplayName: maximum angle error for tune abort
// @Description: If while tuning the angle error goes over this limit then the tune will aborts to prevent a bad oscillation in the case of the tuning algorithm failing. If you get an error "Tuning: attitude error ABORTING" and you think it is a false positive then you can either raise this parameter or you can try increasing the QUIK_DOUBLE_TIME to do the tune more slowly. A value of zero disables this check.
// @Units: deg
// @User: Standard
--]]
local QUIK_ANGLE_MAX = bind_add_param('ANGLE_MAX', 15, 10)
local OPTIONS_TWO_POSITION = (1<<0)
local INS_GYRO_FILTER = bind_param("INS_GYRO_FILTER")
@ -222,6 +232,7 @@ local tune_done_time = nil
local slew_parm = nil
local slew_target = 0
local slew_delta = 0
local aborted = false
local axes_done = {}
local filters_done = {}
@ -511,6 +522,14 @@ function update()
sw_pos_tune = 2
sw_pos_save = -1
end
if aborted then
if sw_pos == 0 then
aborted = false
else
-- after an abort require low switch position to reset
return
end
end
if sw_pos == sw_pos_tune 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()
@ -523,6 +542,7 @@ function update()
restore_all_params()
gcs:send_text(MAV_SEVERITY_EMERGENCY, string.format("Tuning: reverted"))
tune_done_time = nil
aborted = true
end
reset_axes_done()
return
@ -539,6 +559,19 @@ function update()
return
end
if QUIK_ANGLE_MAX:get() > 0 and need_restore then
local att_error = AC_AttitudeControl:get_att_error_angle_deg()
if att_error > QUIK_ANGLE_MAX:get() then
need_restore = false
restore_all_params()
gcs:send_text(MAV_SEVERITY_CRITICAL, string.format("Tuning: attitude error %.1fdeg - ABORTING", att_error))
tune_done_time = nil
aborted = true
reset_axes_done()
return
end
end
if get_time() - last_stage_change < STAGE_DELAY then
update_slew_gain()
return