From 6e3f1918c29d787b6ddad67b652f78b833e3593d Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Wed, 13 Apr 2022 23:41:58 -0400 Subject: [PATCH] AC_AutoTune: fix gain determination fail logic --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 8c8a0ad5b3..6020e96a61 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -211,8 +211,7 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) { // if tune complete or beyond frequency range or no max allowed gains then exit testing if (tune_type == TUNE_COMPLETE || - (tune_type == RP_UP && max_rate_p.max_allowed <= 0.0f) || - (tune_type == RD_UP && max_rate_d.max_allowed <= 0.0f) || + ((tune_type == RP_UP || tune_type == RD_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) || ((tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP || tune_type == SP_UP) && exceeded_freq_range(start_freq))){ load_gains(GAIN_ORIGINAL); @@ -224,7 +223,7 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) // hold level attitude attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw_cd, true); - if ((tune_type == RP_UP && max_rate_p.max_allowed <= 0.0f) || (tune_type == RD_UP && max_rate_d.max_allowed <= 0.0f)) { + if ((tune_type == RP_UP || tune_type == RD_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Gain Determination Failed"); mode = FAILED; AP::logger().Write_Event(LogEvent::AUTOTUNE_FAILED);