AC_AutoTune: tradheli autotune, sqashed commits together

libraries/AC_AutoTune/AC_AutoTune.h

@@ -22,6 +22,7 @@
 #include <AC_AttitudeControl/AC_AttitudeControl.h>
 #include <AC_AttitudeControl/AC_PosControl.h>
 #include <AP_Math/AP_Math.h>
+#include "AC_AutoTune_FreqResp.h"
 
 #define AUTOTUNE_AXIS_BITMASK_ROLL            1
 #define AUTOTUNE_AXIS_BITMASK_PITCH           2
@@ -39,8 +40,6 @@
 
 #define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000
 
-#define AUTOTUNE_DWELL_CYCLES                10
-
 class AC_AutoTune
 {
 public:
@@ -175,6 +174,12 @@ protected:
     // returns true if rate P gain of zero is acceptable for this vehicle
     virtual bool allow_zero_rate_p() = 0;
 
+    // returns true if max tested accel is used for parameter
+    virtual bool set_accel_to_max_test_value() = 0;
+
+    // returns true if pilot is allowed to make inputs during test
+    virtual bool allow_pilot_rp_input() = 0;
+
     // get minimum rate P (for any axis)
     virtual float get_rp_min() const = 0;
 
@@ -184,11 +189,15 @@ protected:
     // get minimum rate Yaw filter value
     virtual float get_yaw_rate_filt_min() const = 0;
 
+    // reverse direction for twitch test
+    virtual bool twitch_reverse_direction() = 0;
+
     // get attitude for slow position hold in autotune mode
     void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);
 
     virtual void Log_AutoTune() = 0;
     virtual void Log_AutoTuneDetails() = 0;
+    virtual void Log_AutoTuneSweep() = 0;
 
     // send message with high level status (e.g. Started, Stopped)
     void update_gcs(uint8_t message_id) const;
@@ -252,6 +261,8 @@ protected:
     TuneType tune_seq[6];         // holds sequence of tune_types to be performed
     uint8_t tune_seq_curr;        // current tune sequence step
 
+    virtual void set_tune_sequence() = 0;
+
     // type of gains to load
     enum GainType {
         GAIN_ORIGINAL   = 0,
@@ -343,18 +354,17 @@ protected:
 
     // Feedforward test used to determine Rate FF gain
     void rate_ff_test_init();
-    void rate_ff_test_run(float max_angle_cds, float target_rate_cds);
+    void rate_ff_test_run(float max_angle_cds, float target_rate_cds, float dir_sign);
 
     // dwell test used to perform frequency dwells for rate gains
     void dwell_test_init(float filt_freq);
-    void dwell_test_run(uint8_t freq_resp_input, float dwell_freq, float &dwell_gain, float &dwell_phase);
+    void dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase);
 
     // dwell test used to perform frequency dwells for angle gains
     void angle_dwell_test_init(float filt_freq);
-    void angle_dwell_test_run(float dwell_freq, float &dwell_gain, float &dwell_phase);
+    void angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase);
 
-    // determines the gain and phase for a dwell
+    float waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax);
-    void determine_gain(float tgt_rate, float meas_rate, float freq, float &gain, float &phase, bool &cycles_complete, bool funct_reset);
 
     uint8_t  ff_test_phase;                         // phase of feedforward test
     float    test_command_filt;                     // filtered commanded output
@@ -370,12 +380,35 @@ protected:
     uint8_t  freq_cnt;
     uint8_t  freq_cnt_max;
     float    curr_test_freq;
-    bool     dwell_complete;
+    float    curr_test_gain;
+    float    curr_test_phase;
     Vector3f start_angles;
+    uint32_t settle_time;
+    uint32_t phase_out_time;
+    float    waveform_freq_rads;  //current frequency for chirp waveform
+    float    start_freq;  //start freq for dwell test
+    float    stop_freq;   //ending freq for dwell test
+    float    trim_pff_out;  // trim output of the PID rate controller for P, I and FF terms
+    float    trim_meas_rate;  // trim measured gyro rate
 
     LowPassFilterFloat  command_filt;               // filtered command
     LowPassFilterFloat  target_rate_filt;            // filtered target rotation rate in radians/second
 
+    // sweep_data tracks the overall characteristics in the response to the frequency sweep
+    struct sweep_data {
+        float    maxgain_freq;
+        float    maxgain_gain;
+        float    maxgain_phase;
+        float    ph180_freq;
+        float    ph180_gain;
+        float    ph180_phase;
+        float    ph270_freq;
+        float    ph270_gain;
+        float    ph270_phase;
+        uint8_t  progress;  // set based on phase of frequency response.  0 - start; 1 - reached 180 deg; 2 - reached 270 deg;
+    };
+    sweep_data sweep;
+
     struct max_gain_data {
         float freq;
         float phase;
@@ -385,4 +418,8 @@ protected:
 
     max_gain_data max_rate_p;
     max_gain_data max_rate_d;
+
+AC_AutoTune_FreqResp freqresp_rate;
+AC_AutoTune_FreqResp freqresp_angle;
+
 };

libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp

@@ -0,0 +1,450 @@
+/*
+This function receives time history data during a dwell test or frequency sweep and determines the gain and phase of the response to the input.
+Once the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles and the cycles_complete flag
+is set.  This function must be reset using the reset flag prior to the next dwell.
+*/
+
+#include <AP_HAL/AP_HAL.h>
+#include "AC_AutoTune_FreqResp.h"
+
+float AC_AutoTune_FreqResp::update()
+{
+    float dummy = 0.0f;
+    return dummy;
+}
+
+void AC_AutoTune_FreqResp::init(InputType input_type)
+{
+    excitation = input_type;
+    max_target_cnt = 0;
+    min_target_cnt = 0;
+    max_meas_cnt = 0;
+    min_meas_cnt = 0;
+    input_start_time_ms = 0;
+    new_tgt_time_ms = 0;
+    new_meas_time_ms = 0;
+    new_target = false;
+    new_meas = false;
+    curr_test_freq = 0.0f;
+    curr_test_gain = 0.0f;
+    curr_test_phase = 0.0f;
+    max_accel = 0.0f;
+    max_meas_rate = 0.0f;
+    max_command = 0.0f;
+    meas_peak_info_buffer->clear();
+    tgt_peak_info_buffer->clear();
+    cycle_complete = false;
+}
+
+// determine_gain - this function receives time history data during a dwell test input and determines the gain and phase of the response to the input.
+// Once the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles and the cycles_complete flag
+// is set.  This function must be reset using the reset flag prior to the next dwell.
+void AC_AutoTune_FreqResp::determine_gain(float tgt_rate, float meas_rate, float tgt_freq)
+{
+    uint32_t now = AP_HAL::millis();
+    uint32_t half_cycle_time_ms = 0;
+    uint32_t cycle_time_ms = 0;
+
+    if (cycle_complete) {
+        return;
+    }
+
+    if (!is_zero(tgt_freq)) {
+        half_cycle_time_ms = (uint32_t)(300 * 6.28 / tgt_freq);
+        cycle_time_ms = (uint32_t)(1000 * 6.28 / tgt_freq);
+    }
+
+    if (input_start_time_ms == 0) {
+        input_start_time_ms = now;
+    }
+
+    // cycles are complete! determine gain and phase and exit
+    if (max_meas_cnt > AUTOTUNE_DWELL_CYCLES + 1 && max_target_cnt > AUTOTUNE_DWELL_CYCLES + 1 && excitation == DWELL) {
+        float delta_time = 0.0f;
+        float sum_gain = 0.0f;
+        uint8_t cnt = 0;
+        uint8_t gcnt = 0;
+        uint16_t meas_cnt, tgt_cnt;
+        float meas_ampl = 0.0f;
+        float tgt_ampl = 0.0f;
+        uint32_t meas_time = 0;
+        uint32_t tgt_time = 0;
+        for (int i = 0;  i < AUTOTUNE_DWELL_CYCLES; i++) {
+            meas_cnt=0;
+            tgt_cnt=0;
+            pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time);
+            pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time);
+            push_to_meas_buffer(0, 0.0f, 0);
+            push_to_tgt_buffer(0, 0.0f, 0);
+            // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: tgt_cnt=%f meas_cnt=%f", (double)(tgt_cnt), (double)(meas_cnt));
+
+            if (meas_cnt == tgt_cnt && meas_cnt != 0) {
+                if (tgt_ampl > 0.0f) {
+                    sum_gain += meas_ampl / tgt_ampl;
+                    gcnt++;
+                }
+                float d_time = (float)(meas_time - tgt_time);
+                if (d_time < 2.0f * (float)cycle_time_ms) {
+                    delta_time += d_time;
+                    cnt++;
+                }
+            } else if (meas_cnt > tgt_cnt) {
+                pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time);
+                push_to_tgt_buffer(0, 0.0f, 0);
+            } else if (meas_cnt < tgt_cnt) {
+                pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time);
+                push_to_meas_buffer(0, 0.0f, 0);
+            }                
+
+        }
+        if (gcnt > 0) {
+            curr_test_gain = sum_gain / gcnt;
+        }
+        if (cnt > 0) {
+            delta_time = delta_time / cnt;
+        }
+        curr_test_phase = tgt_freq * delta_time * 0.001f * 360.0f / 6.28f;
+        if (curr_test_phase > 360.0f) {
+            curr_test_phase = curr_test_phase - 360.0f;
+        }
+        curr_test_freq = tgt_freq;
+        cycle_complete = true;
+        // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cycles completed");
+        return;
+    }
+
+    // Indicates when the target(input) is positive or negative half of the cycle to notify when the max or min should be sought
+    if (!is_positive(prev_target) && is_positive(tgt_rate) && !new_target && now > new_tgt_time_ms) {
+        new_target = true;
+        new_tgt_time_ms = now + half_cycle_time_ms;
+        // reset max_target
+        max_target = 0.0f;
+        max_target_cnt++;
+        temp_min_target = min_target;
+        if (min_target_cnt > 0) {
+            sweep_tgt.max_time_m1 = temp_max_tgt_time;
+            temp_max_tgt_time = max_tgt_time;
+            sweep_tgt.count_m1 = min_target_cnt - 1;
+            sweep_tgt.amplitude_m1 = temp_tgt_ampl;
+            temp_tgt_ampl = temp_max_target - temp_min_target;
+            push_to_tgt_buffer(min_target_cnt,temp_tgt_ampl,temp_max_tgt_time);
+        }
+                // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt));
+
+    } else if (is_positive(prev_target) && !is_positive(tgt_rate) && new_target && now > new_tgt_time_ms && max_target_cnt > 0) {
+        new_target = false;
+        new_tgt_time_ms = now + half_cycle_time_ms;
+        min_target_cnt++;
+        temp_max_target = max_target;
+        min_target = 0.0f;
+                // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt));
+    }
+
+    // Indicates when the measured value (output) is positive or negative half of the cycle to notify when the max or min should be sought
+    if (!is_positive(prev_meas) && is_positive(meas_rate) && !new_meas && now > new_meas_time_ms && max_target_cnt > 0) {
+        new_meas = true;
+        new_meas_time_ms = now + half_cycle_time_ms;
+        // reset max_meas
+        max_meas = 0.0f;
+        max_meas_cnt++;
+        temp_min_meas = min_meas;
+        if (min_meas_cnt > 0 && min_target_cnt > 0) {
+            sweep_meas.max_time_m1 = temp_max_meas_time;
+            temp_max_meas_time = max_meas_time;
+            sweep_meas.count_m1 = min_meas_cnt - 1;
+            sweep_meas.amplitude_m1 = temp_meas_ampl;
+            temp_meas_ampl = temp_max_meas - temp_min_meas;
+            push_to_meas_buffer(min_meas_cnt,temp_meas_ampl,temp_max_meas_time);
+
+            if (excitation == SWEEP) {
+                float tgt_period = 0.001f * (temp_max_tgt_time - sweep_tgt.max_time_m1);
+                if (!is_zero(tgt_period)) {
+                    curr_test_freq = 6.28f / tgt_period;
+                } else {
+                    curr_test_freq = 0.0f;
+                }
+                if (!is_zero(sweep_tgt.amplitude_m1)) {
+                    curr_test_gain = sweep_meas.amplitude_m1/sweep_tgt.amplitude_m1;
+                } else {
+                    curr_test_gain = 0.0f;
+                }
+                curr_test_phase = curr_test_freq * (float)(sweep_meas.max_time_m1 - sweep_tgt.max_time_m1) * 0.001f * 360.0f / 6.28f;
+                // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f sweepgain=%f", (double)(curr_test_freq), (double)(curr_test_gain));
+                cycle_complete = true;
+            }
+        } 
+                // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt));
+    } else if (is_positive(prev_meas) && !is_positive(meas_rate) && new_meas && now > new_meas_time_ms && max_meas_cnt > 0) {
+        new_meas = false;
+        new_meas_time_ms = now + half_cycle_time_ms;
+        min_meas_cnt++;
+        temp_max_meas = max_meas;
+        min_meas = 0.0f;
+                // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt));
+    }
+
+    if (tgt_rate > max_target && new_target) {
+        max_target = tgt_rate;
+        max_tgt_time = now;
+    }
+
+    if (tgt_rate < min_target && !new_target) {
+        min_target = tgt_rate;
+    }
+
+    if (meas_rate > max_meas && new_meas) {
+        max_meas = meas_rate;
+        max_meas_time = now;
+    }
+
+    if (meas_rate < min_meas && !new_meas) {
+        min_meas = meas_rate;
+    }
+
+    prev_target = tgt_rate;
+    prev_meas = meas_rate;
+}
+
+// determine_gain_angle - this function receives time history data during a dwell test input and determines the gain and phase of the response to the input.
+// Once the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles and the cycles_complete flag
+// is set.  This function must be reset using the reset flag prior to the next dwell.
+void AC_AutoTune_FreqResp::determine_gain_angle(float command, float tgt_angle, float meas_angle, float tgt_freq)
+{
+
+    uint32_t now = AP_HAL::millis();
+    float dt = 0.0025;
+    uint32_t half_cycle_time_ms = 0;
+    uint32_t cycle_time_ms = 0;
+
+    if (cycle_complete) {
+        return;
+    }
+
+    if (!is_zero(tgt_freq)) {
+        half_cycle_time_ms = (uint32_t)(300 * 6.28 / tgt_freq);
+        cycle_time_ms = (uint32_t)(1000 * 6.28 / tgt_freq);
+    }
+
+    if (input_start_time_ms == 0) {
+        input_start_time_ms = now;
+        prev_tgt_angle = tgt_angle;
+        prev_meas_angle = meas_angle;
+        prev_target = 0.0f;
+        prev_meas = 0.0f;
+    }
+
+    target_rate = (tgt_angle - prev_tgt_angle) / dt;
+    measured_rate = (meas_angle - prev_meas_angle) / dt;
+
+    // cycles are complete! determine gain and phase and exit
+    if (max_meas_cnt > AUTOTUNE_DWELL_CYCLES + 1 && max_target_cnt > AUTOTUNE_DWELL_CYCLES + 1 && excitation == DWELL) {
+        float delta_time = 0.0f;
+        float sum_gain = 0.0f;
+        uint8_t cnt = 0;
+        uint8_t gcnt = 0;
+        uint16_t meas_cnt, tgt_cnt;
+        float meas_ampl = 0.0f;
+        float tgt_ampl = 0.0f;
+        uint32_t meas_time = 0;
+        uint32_t tgt_time = 0;
+        for (int i = 0;  i < AUTOTUNE_DWELL_CYCLES; i++) {
+            meas_cnt=0;
+            tgt_cnt=0;
+            pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time);
+            pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time);
+            push_to_meas_buffer(0, 0.0f, 0);
+            push_to_tgt_buffer(0, 0.0f, 0);
+            // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: tgt_cnt=%f meas_cnt=%f", (double)(tgt_cnt), (double)(meas_cnt));
+
+            if (meas_cnt == tgt_cnt && meas_cnt != 0) {
+                if (tgt_ampl > 0.0f) {
+                    sum_gain += meas_ampl / tgt_ampl;
+                    gcnt++;
+                }
+                float d_time = (float)(meas_time - tgt_time);
+                if (d_time < 2.0f * (float)cycle_time_ms) {
+                    delta_time += d_time;
+                    cnt++;
+                }
+            } else if (meas_cnt > tgt_cnt) {
+                pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time);
+                push_to_tgt_buffer(0, 0.0f, 0);
+            } else if (meas_cnt < tgt_cnt) {
+                pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time);
+                push_to_meas_buffer(0, 0.0f, 0);
+            }                
+
+        }
+        if (gcnt > 0) {
+            curr_test_gain = sum_gain / gcnt;
+        }
+        if (cnt > 0) {
+            delta_time = delta_time / cnt;
+        }
+        curr_test_phase = tgt_freq * delta_time * 0.001f * 360.0f / 6.28f;
+        if (curr_test_phase > 360.0f) {
+            curr_test_phase = curr_test_phase - 360.0f;
+        }
+        float dwell_max_accel = tgt_freq * max_meas_rate * 5730.0f;
+        if (!is_zero(max_command)) {
+            // normalize accel for input size
+            dwell_max_accel = dwell_max_accel / (2.0f * max_command);
+        }
+        if (dwell_max_accel > max_accel) {
+            max_accel = dwell_max_accel;
+        }
+        curr_test_freq = tgt_freq;
+        cycle_complete = true;
+        // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cycles completed");
+        return;
+    }
+
+    // Indicates when the target(input) is positive or negative half of the cycle to notify when the max or min should be sought
+    if (is_positive(prev_target) && !is_positive(target_rate) && !new_target && now > new_tgt_time_ms) {
+        new_target = true;
+        new_tgt_time_ms = now + half_cycle_time_ms;
+        // reset max_target
+        max_target = 0.0f;
+        max_target_cnt++;
+        temp_min_target = min_target;
+        if (min_target_cnt > 0) {
+            sweep_tgt.max_time_m1 = temp_max_tgt_time;
+            temp_max_tgt_time = max_tgt_time;
+            sweep_tgt.count_m1 = min_target_cnt - 1;
+            sweep_tgt.amplitude_m1 = temp_tgt_ampl;
+            temp_tgt_ampl = temp_max_target - temp_min_target;
+            push_to_tgt_buffer(min_target_cnt,temp_tgt_ampl,temp_max_tgt_time);
+        }
+                // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_tgt_cnt=%f", (double)(max_target_cnt));
+
+    } else if (!is_positive(prev_target) && is_positive(target_rate) && new_target && now > new_tgt_time_ms && max_target_cnt > 0) {
+        new_target = false;
+        new_tgt_time_ms = now + half_cycle_time_ms;
+        min_target_cnt++;
+        temp_max_target = max_target;
+        min_target = 0.0f;
+                // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt));
+    }
+
+    // Indicates when the measured value (output) is positive or negative half of the cycle to notify when the max or min should be sought
+    if (is_positive(prev_meas) && !is_positive(measured_rate) && !new_meas && now > new_meas_time_ms && max_target_cnt > 0) {
+        new_meas = true;
+        new_meas_time_ms = now + half_cycle_time_ms;
+        // reset max_meas
+        max_meas = 0.0f;
+        max_meas_cnt++;
+        temp_min_meas = min_meas;
+        if (min_meas_cnt > 0 && min_target_cnt > 0) {
+            sweep_meas.max_time_m1 = temp_max_meas_time;
+            temp_max_meas_time = max_meas_time;
+            sweep_meas.count_m1 = min_meas_cnt - 1;
+            sweep_meas.amplitude_m1 = temp_meas_ampl;
+            temp_meas_ampl = temp_max_meas - temp_min_meas;
+            push_to_meas_buffer(min_meas_cnt,temp_meas_ampl,temp_max_meas_time);
+
+            if (excitation == SWEEP) {
+                float tgt_period = 0.001f * (temp_max_tgt_time - sweep_tgt.max_time_m1);
+                if (!is_zero(tgt_period)) {
+                    curr_test_freq = 6.28f / tgt_period;
+                } else {
+                    curr_test_freq = 0.0f;
+                }
+                if (!is_zero(sweep_tgt.amplitude_m1)) {
+                    curr_test_gain = sweep_meas.amplitude_m1/sweep_tgt.amplitude_m1;
+                } else {
+                    curr_test_gain = 0.0f;
+                }
+                curr_test_phase = curr_test_freq * (float)(sweep_meas.max_time_m1 - sweep_tgt.max_time_m1) * 0.001f * 360.0f / 6.28f;
+               //gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase));
+                cycle_complete = true;
+            }
+        } 
+                // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_meas_cnt=%f", (double)(max_meas_cnt));
+    } else if (!is_positive(prev_meas) && is_positive(measured_rate) && new_meas && now > new_meas_time_ms && max_meas_cnt > 0) {
+        new_meas = false;
+        new_meas_time_ms = now + half_cycle_time_ms;
+        min_meas_cnt++;
+        temp_max_meas = max_meas;
+        min_meas = 0.0f;
+                // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt));
+    }
+
+    if (tgt_angle > max_target && new_target) {
+        max_target = tgt_angle;
+        max_tgt_time = now;
+    }
+
+    if (tgt_angle < min_target && !new_target) {
+        min_target = tgt_angle;
+    }
+
+    if (meas_angle > max_meas && new_meas) {
+        max_meas = meas_angle;
+        max_meas_time = now;
+    }
+
+    if (meas_angle < min_meas && !new_meas) {
+        min_meas = meas_angle;
+    }
+
+    if (now > (uint32_t)(input_start_time_ms + 7.0f * cycle_time_ms) && now < (uint32_t)(input_start_time_ms + 9.0f * cycle_time_ms)) {
+        if (measured_rate > max_meas_rate) {
+            max_meas_rate = measured_rate;
+        }
+        if (command > max_command) {
+            max_command = command;
+        }
+    }
+
+    prev_target = target_rate;
+    prev_meas = measured_rate;
+    prev_tgt_angle = tgt_angle;
+    prev_meas_angle = meas_angle;
+}
+
+// push to peak info buffer
+void AC_AutoTune_FreqResp::push_to_meas_buffer(uint16_t count, float amplitude, uint32_t time_ms)
+{
+    peak_info sample;
+    sample.curr_count = count;
+    sample.amplitude = amplitude;
+    sample.time_ms = time_ms;
+    meas_peak_info_buffer->push(sample);
+}
+
+// pull from peak info buffer
+void AC_AutoTune_FreqResp::pull_from_meas_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms)
+{
+    peak_info sample;
+    if (!meas_peak_info_buffer->pop(sample)) {
+        // no sample
+        return;
+    }
+    count = sample.curr_count;
+    amplitude = sample.amplitude;
+    time_ms = sample.time_ms;
+}
+
+// push to peak info buffer
+void AC_AutoTune_FreqResp::push_to_tgt_buffer(uint16_t count, float amplitude, uint32_t time_ms)
+{
+    peak_info sample;
+    sample.curr_count = count;
+    sample.amplitude = amplitude;
+    sample.time_ms = time_ms;
+    tgt_peak_info_buffer->push(sample);
+
+}
+
+// pull from peak info buffer
+void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms)
+{
+    peak_info sample;
+    if (!tgt_peak_info_buffer->pop(sample)) {
+        // no sample
+        return;
+    }
+    count = sample.curr_count;
+    amplitude = sample.amplitude;
+    time_ms = sample.time_ms;
+}

libraries/AC_AutoTune/AC_AutoTune_FreqResp.h

@@ -0,0 +1,89 @@
+#pragma once
+
+/*
+ Gain and phase determination algorithm
+*/
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+
+#define AUTOTUNE_DWELL_CYCLES                6
+
+class AC_AutoTune_FreqResp {
+public:
+    // Constructor
+    AC_AutoTune_FreqResp()
+{
+    // initialize test variables
+    meas_peak_info_buffer = new ObjectBuffer<peak_info>(AUTOTUNE_DWELL_CYCLES);
+    tgt_peak_info_buffer = new ObjectBuffer<peak_info>(AUTOTUNE_DWELL_CYCLES);
+}
+
+//    CLASS_NO_COPY(AC_PI);
+
+    // update calculations
+    float update();
+
+    enum InputType {
+        DWELL = 0,                 
+        SWEEP = 1, 
+    };
+
+    void init(InputType input_type);
+
+    // determines the gain and phase for a dwell
+    void determine_gain(float tgt_rate, float meas_rate, float tgt_freq);
+
+    // determines the gain and phase for a dwell
+    void determine_gain_angle(float command, float tgt_angle, float meas_angle, float tgt_freq);
+
+    // enable external query if cycle is complete and phase and gain data are available
+    bool is_cycle_complete() { return cycle_complete;}
+
+    void reset_cycle_complete() { cycle_complete = false; }
+
+    // frequency response accessors
+    float get_freq() { return curr_test_freq; }
+    float get_gain() { return curr_test_gain; }
+    float get_phase() { return curr_test_phase; }
+    float get_accel_max() { return max_accel; }
+
+private:
+    float max_target, max_meas, prev_target, prev_meas, prev_tgt_angle, prev_meas_angle;
+    float min_target, min_meas, temp_meas_ampl, temp_tgt_ampl;
+    float temp_max_target, temp_min_target, target_rate, measured_rate, max_meas_rate, max_command;
+    float temp_max_meas, temp_min_meas;
+    uint32_t temp_max_tgt_time, temp_max_meas_time;
+    uint32_t max_tgt_time, max_meas_time, new_tgt_time_ms, new_meas_time_ms, input_start_time_ms;
+    uint16_t min_target_cnt, max_target_cnt, max_meas_cnt, min_meas_cnt;
+    bool new_target = false;
+    bool new_meas = false;
+    bool cycle_complete = false;
+    float curr_test_freq, curr_test_gain, curr_test_phase;
+    float max_accel;
+    InputType excitation;
+
+    // sweep_peak_finding_data tracks the peak data
+    struct sweep_peak_finding_data {
+        uint16_t count_m1;
+        float amplitude_m1;
+        float max_time_m1;
+    };
+    sweep_peak_finding_data sweep_meas;
+    sweep_peak_finding_data sweep_tgt;
+
+    //store determine gain data in ring buffer
+    struct peak_info {
+        uint16_t curr_count;
+        float amplitude;
+        uint32_t time_ms; 
+
+    };
+    ObjectBuffer<peak_info> *meas_peak_info_buffer;
+    ObjectBuffer<peak_info> *tgt_peak_info_buffer;
+    void push_to_meas_buffer(uint16_t count, float amplitude, uint32_t time_ms);
+    void pull_from_meas_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms);
+    void push_to_tgt_buffer(uint16_t count, float amplitude, uint32_t time_ms);
+    void pull_from_tgt_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms);
+
+};

libraries/AC_AutoTune/AC_AutoTune_Heli.h

@@ -24,20 +24,14 @@ class AC_AutoTune_Heli : public AC_AutoTune
 {
 public:
     // constructor
-    AC_AutoTune_Heli()
+    AC_AutoTune_Heli();
-    {
+
-        //            tune_seq[0] = 4;  // RFF_UP
-        //            tune_seq[1] = 8;   // MAX_GAINS
-        //            tune_seq[2] = 0; // RD_UP
-        //            tune_seq[3] = 2; // RP_UP
-        //            tune_seq[2] = 6; // SP_UP
-        //            tune_seq[3] = 9; // tune complete
-        tune_seq[0] = SP_UP;
-        tune_seq[1] = TUNE_COMPLETE;
-    };
     // save gained, called on disarm
     void save_tuning_gains() override;
 
+    // var_info for holding Parameter information
+    static const struct AP_Param::GroupInfo var_info[];
+
 protected:
 
     void load_test_gains() override;
@@ -90,25 +84,52 @@ protected:
     // get minimum rate Yaw filter value
     float get_yaw_rate_filt_min() const override;
 
+    // reverse direction for twitch test
+    bool twitch_reverse_direction() override { return positive_direction; }
+
     void Log_AutoTune() override;
     void Log_AutoTuneDetails() override;
-    void Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp);
+    void Log_AutoTuneSweep() override;
-    void Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads);
+    void Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp, float max_accel);
-
+    void Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads, float tgt_ang_rad, float ang_rad);
+    void Log_Write_AutoTuneSweep(float freq, float gain, float phase);
     // returns true if rate P gain of zero is acceptable for this vehicle
     bool allow_zero_rate_p() override { return true; }
 
+    // returns true if max tested accel is used for parameter
+    bool set_accel_to_max_test_value() override { return false; }
+
+    // returns true if pilot is allowed to make inputs during test
+    bool allow_pilot_rp_input() override
+    {
+        if (!use_poshold && tune_type == SP_UP) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+
     // send intermittant updates to user on status of tune
     void do_gcs_announcements() override;
 
+    void set_tune_sequence() override;
+
+    AP_Int8  seq_bitmask;
+    AP_Float min_sweep_freq;
+    AP_Float max_sweep_freq;
+    AP_Float max_resp_gain;
+
 private:
     // updating_rate_ff_up - adjust FF to ensure the target is reached
     // FF is adjusted until rate requested is acheived
     void updating_rate_ff_up(float &tune_ff, float rate_target, float meas_rate, float meas_command);
-    void updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, float gain_incr, float max_gain);
+    void updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p);
     void updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_d);
     void updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt);
+    void updating_angle_p_up_yaw(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt);
    // updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur
     void updating_max_gains(float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d);
 
+    uint8_t method; //0: determine freq, 1: use max gain method, 2: use phase 180 method
+
 };

libraries/AC_AutoTune/AC_AutoTune_Multi.cpp

@@ -36,15 +36,31 @@
 
 #include "AC_AutoTune_Multi.h"
 
+const AP_Param::GroupInfo AC_AutoTune_Multi::var_info[] = {
+    AP_NESTEDGROUPINFO(AC_AutoTune, 0),
+
+    // @Param: AGGR
+    // @DisplayName: Autotune aggressiveness
+    // @Description: Autotune aggressiveness. Defines the bounce back used to detect size of the D term.
+    // @Range: 0.05 0.10
+    // @User: Standard
+    AP_GROUPINFO("AGGR", 1, AC_AutoTune_Multi, aggressiveness, 0.1f),
+
+    // @Param: MIN_D
+    // @DisplayName: AutoTune minimum D
+    // @Description: Defines the minimum D gain
+    // @Range: 0.001 0.006
+    // @User: Standard
+    AP_GROUPINFO("MIN_D", 2, AC_AutoTune_Multi, min_d,  0.001f),
+
+    AP_GROUPEND
+};
+
 // constructor
 AC_AutoTune_Multi::AC_AutoTune_Multi()
 {
-    tune_seq[0] = RD_UP;
+    tune_seq[0] = TUNE_COMPLETE;
-    tune_seq[1] = RD_DOWN;
+    AP_Param::setup_object_defaults(this, var_info);
-    tune_seq[2] = RP_UP;
-    tune_seq[3] = SP_DOWN;
-    tune_seq[4] = SP_UP;
-    tune_seq[5] = TUNE_COMPLETE;
 }
 
 void AC_AutoTune_Multi::do_gcs_announcements()

libraries/AC_AutoTune/AC_AutoTune_Multi.h

@@ -30,6 +30,9 @@ public:
     // save gained, called on disarm
     void save_tuning_gains() override;
 
+    // var_info for holding Parameter information
+    static const struct AP_Param::GroupInfo var_info[];
+
 protected:
     // load gains
     void load_test_gains() override;
@@ -77,6 +80,12 @@ protected:
     // returns true if rate P gain of zero is acceptable for this vehicle
     bool allow_zero_rate_p() override { return false; }
 
+    // returns true if pilot is allowed to make inputs during test
+    bool allow_pilot_rp_input() override { return false; }
+
+    // returns true if max tested accel is used for parameter
+    bool set_accel_to_max_test_value() override { return true; }
+
     // get minimum rate P (for any axis)
     float get_rp_min() const override;
 
@@ -86,8 +95,12 @@ protected:
     // get minimum yaw rate filter value
     float get_yaw_rate_filt_min() const override;
 
+    // reverse direction for twitch test
+    bool twitch_reverse_direction() override { return !positive_direction; }
+
     void Log_AutoTune() override;
     void Log_AutoTuneDetails() override;
+    void Log_AutoTuneSweep() override {};
     void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt);
     void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds);
 
@@ -111,4 +124,14 @@ private:
     // updating_angle_p_up - increase P to ensure the target is reached
     // P is increased until we achieve our target within a reasonable time
     void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max);
+
+    void set_tune_sequence() override {
+        tune_seq[0] = RD_UP;
+        tune_seq[1] = RD_DOWN;
+        tune_seq[2] = RP_UP;
+        tune_seq[3] = SP_DOWN;
+        tune_seq[4] = SP_UP;
+        tune_seq[5] = TUNE_COMPLETE;
+    }
+
 };