diff --git a/libraries/AP_Scripting/applets/VTOL-quicktune.lua b/libraries/AP_Scripting/applets/VTOL-quicktune.lua index 29c5c3ae26..b36e79bcc6 100644 --- a/libraries/AP_Scripting/applets/VTOL-quicktune.lua +++ b/libraries/AP_Scripting/applets/VTOL-quicktune.lua @@ -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