AC_AutoTune: add safety checks for max allowed gains and freq range

libraries/AC_AutoTune/AC_AutoTune.cpp

@@ -736,9 +736,13 @@ void AC_AutoTune::next_tune_type(TuneType &curr_tune_type, bool reset)
     if (reset) {
         set_tune_sequence();
         tune_seq_curr = 0;
+    } else if (curr_tune_type == TUNE_COMPLETE) {
+        // leave tune_type as TUNE_COMPLETE to initiate next axis or exit autotune
+        return;
     } else {
         tune_seq_curr++;
     }
+
     curr_tune_type = tune_seq[tune_seq_curr];
 }
 

libraries/AC_AutoTune/AC_AutoTune.h

@@ -213,6 +213,18 @@ protected:
     // get_testing_step_timeout_ms accessor
     virtual uint32_t get_testing_step_timeout_ms() const = 0;
 
+    // get attitude for slow position hold in autotune mode
+    void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);
+
+    // type of gains to load
+    enum GainType {
+        GAIN_ORIGINAL   = 0,
+        GAIN_TEST       = 1,
+        GAIN_INTRA_TEST = 2,
+        GAIN_TUNED      = 3,
+    };
+    void load_gains(enum GainType gain_type);
+
     // copies of object pointers to make code a bit clearer
     AC_AttitudeControl *attitude_control;
     AC_PosControl *pos_control;
@@ -280,9 +292,6 @@ private:
     // directly updates attitude controller with targets
     void control_attitude();
 
-    // get attitude for slow position hold in autotune mode
-    void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);
-
     // convert latest level issue to string for reporting
     const char *level_issue_string() const;
 
@@ -310,15 +319,6 @@ private:
         FAILED = 3,               // tuning has failed, user is flying on original gains
     };
 
-    // type of gains to load
-    enum GainType {
-        GAIN_ORIGINAL   = 0,
-        GAIN_TEST       = 1,
-        GAIN_INTRA_TEST = 2,
-        GAIN_TUNED      = 3,
-    };
-    void load_gains(enum GainType gain_type);
-
     TuneMode mode;                       // see TuneMode for what modes are allowed
     bool     pilot_override;             // true = pilot is overriding controls so we suspend tuning temporarily
     bool     use_poshold;                // true = enable position hold

libraries/AC_AutoTune/AC_AutoTune_Heli.cpp

@@ -209,8 +209,36 @@ void AC_AutoTune_Heli::test_init()
 // run tests for each tune type
 void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign)
 {
-    
-    if (tune_type == TUNE_COMPLETE) { return; }
+    // 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 == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP || tune_type == SP_UP) && exceeded_freq_range(start_freq))){
+
+        load_gains(GAIN_INTRA_TEST);
+
+        attitude_control->use_sqrt_controller(true);
+
+        get_poshold_attitude(roll_cd, pitch_cd, desired_yaw_cd);
+
+        // 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)) {
+            gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Gains Failed, Skip Rate P/D Tuning");
+            counter = AUTOTUNE_SUCCESS_COUNT;
+            step = UPDATE_GAINS;
+        } else if ((tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP || tune_type == SP_UP) && exceeded_freq_range(start_freq)){
+            gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Exceeded frequency range");
+            AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
+            counter = AUTOTUNE_SUCCESS_COUNT;
+            step = UPDATE_GAINS;
+        } else if (tune_type == TUNE_COMPLETE) {
+            counter = AUTOTUNE_SUCCESS_COUNT;
+            step = UPDATE_GAINS;
+        }
+        return;
+    }
 
     switch (tune_type) {
     case RFF_UP:
@@ -1781,7 +1809,7 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
         if (!is_zero(sweep.ph180_freq)) {
             freq[frq_cnt] = sweep.ph180_freq - 0.5f * test_freq_incr;
         } else {
-            freq[frq_cnt] = 4.0f * M_PI;
+            freq[frq_cnt] = min_sweep_freq;
         }
         curr_test_freq = freq[frq_cnt];
         start_freq = curr_test_freq;
@@ -2096,3 +2124,13 @@ uint32_t AC_AutoTune_Heli::get_testing_step_timeout_ms() const
 {
     return AUTOTUNE_TESTING_STEP_TIMEOUT_MS;
 }
+
+    // exceeded_freq_range - ensures tuning remains inside frequency range
+bool AC_AutoTune_Heli::exceeded_freq_range(float frequency)
+{
+    bool ret = false;
+    if (frequency < min_sweep_freq || frequency > max_sweep_freq) {
+        ret = true;
+    }
+    return ret;
+}

libraries/AC_AutoTune/AC_AutoTune_Heli.h

@@ -170,7 +170,8 @@ private:
     // reset the sweep variables
     void reset_sweep_variables();
 
-    uint8_t method; //0: determine freq, 1: use max gain method, 2: use phase 180 method
+    // exceeded_freq_range - ensures tuning remains inside frequency range
+    bool exceeded_freq_range(float frequency);
 
     // updating rate FF variables
     // flag for completion of the initial direction for the feedforward test