APM_Control: implement new autotune system

use actuator/rate ratio with median filter for FF and Dmod/overshoot
for P,I,D

libraries/APM_Control/AP_AutoTune.cpp

@@ -43,28 +43,26 @@ extern const AP_HAL::HAL& hal;
 // time in milliseconds between autotune saves
 #define AUTOTUNE_SAVE_PERIOD 10000U
 
-// how much time we have to overshoot for to reduce gains
-#define AUTOTUNE_OVERSHOOT_TIME 100
-
-// how much time we have to undershoot for to increase gains
-#define AUTOTUNE_UNDERSHOOT_TIME 200
-
 // step size for increasing gains, percentage
-#define AUTOTUNE_INCREASE_STEP 5
+#define AUTOTUNE_INCREASE_FF_STEP 12
+#define AUTOTUNE_INCREASE_PD_STEP 5
+
+// step size for increasing gains when low impact, percentage
+#define AUTOTUNE_INCREASE_PD_LOW_STEP 30
 
 // step size for decreasing gains, percentage
-#define AUTOTUNE_DECREASE_STEP 8
+#define AUTOTUNE_DECREASE_FF_STEP 15
+#define AUTOTUNE_DECREASE_PD_STEP 20
 
-// min/max FF gains
+// limits on IMAX
-#define AUTOTUNE_MAX_FF 2.0f
+#define AUTOTUNE_MIN_IMAX 0.4
-#define AUTOTUNE_MIN_FF 0.05f
+#define AUTOTUNE_MAX_IMAX 0.9
 
-// tau ranges
+// ratio of I to P
-#define AUTOTUNE_MAX_TAU 0.7f
+#define AUTOTUNE_I_RATIO 0.75
-#define AUTOTUNE_MIN_TAU 0.2f
 
-#define AUTOTUNE_MIN_IMAX 2000
+// overshoot threshold
-#define AUTOTUNE_MAX_IMAX 4000
+#define AUTOTUNE_OVERSHOOT 1.1
 
 // constructor
 AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
@@ -72,10 +70,9 @@ AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
                          AC_PID &_rpid) :
     current(_gains),
     rpid(_rpid),
-    running(false),
     type(_type),
     aparm(parms),
-    saturated_surfaces(false)
+    ff_filter(2)
 {}
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@@ -93,20 +90,19 @@ AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
  */
 static const struct {
     float tau;
-    float Pratio;
     float rmax;
 } tuning_table[] = {
-    { 0.70f, 0.050f,  20 },   // level 1
+    { 1.00, 20 },   // level 1
-    { 0.65f, 0.055f,  30 },   // level 2
+    { 0.90, 30 },   // level 2
-    { 0.60f, 0.060f,  40 },   // level 3
+    { 0.80, 40 },   // level 3
-    { 0.55f, 0.065f,  50 },   // level 4
+    { 0.70, 50 },   // level 4
-    { 0.50f, 0.070f,  60 },   // level 5
+    { 0.60, 60 },   // level 5
-    { 0.45f, 0.075f,  75 },   // level 6
+    { 0.50, 75 },   // level 6
-    { 0.40f, 0.080f,  90 },   // level 7
+    { 0.25, 90 },   // level 7
-    { 0.30f, 0.085f, 120 },   // level 8
+    { 0.15, 120 },   // level 8
-    { 0.20f, 0.090f, 160 },   // level 9
+    { 0.10, 160 },   // level 9
-    { 0.10f, 0.095f, 210 },   // level 10
+    { 0.05, 210 },   // level 10
-    { 0.05f, 0.100f, 300 },   // (yes, it goes to 11)
+    { 0.03, 300 },   // (yes, it goes to 11)
 };
 
 /*
@@ -115,14 +111,12 @@ static const struct {
 void AP_AutoTune::start(void)
 {
     running = true;
-    state = DEMAND_UNSATURATED;
+    state = ATState::IDLE;
     uint32_t now = AP_HAL::millis();
 
-    state_enter_ms = now;
     last_save_ms = now;
 
-    last_save = current;
+    restore = last_save = get_gains(current);
-    restore = current;
 
     uint8_t level = aparm.autotune_level;
     if (level > ARRAY_SIZE(tuning_table)) {
@@ -132,13 +126,20 @@ void AP_AutoTune::start(void)
         level = 1;
     }
 
-    current.rmax.set(tuning_table[level-1].rmax);
+    current.rmax_pos.set(tuning_table[level-1].rmax);
+    current.rmax_neg.set(tuning_table[level-1].rmax);
     current.tau.set(tuning_table[level-1].tau);
-
+    rpid.kIMAX().set(constrain_float(rpid.kIMAX(), AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX));
-    current.imax = constrain_float(current.imax, AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX);
 
     next_save = current;
 
+    // use 2Hz filters on the actuator and rate to reduce impact of noise
+    actuator_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), 2);
+    rate_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), 2);
+
+    // scale slew limit to more agressively find oscillations during autotune
+    rpid.set_slew_limit_scale(1.5*45);
+
     Debug("START FF -> %.3f\n", rpid.ff().get());
 }
 
@@ -150,82 +151,213 @@ void AP_AutoTune::stop(void)
     if (running) {
         running = false;
         save_gains(restore);
+        rpid.set_slew_limit_scale(45);
     }
 }
 
 
+// @LoggerMessage: ATNP
+// @Description: Plane AutoTune
+// @Vehicles: Plane
+// @Field: TimeUS: Time since system startup
+// @Field: Axis: which axis is currently being tuned
+// @Field: State: tuning state
+// @Field: Sur: control surface deflection
+// @Field: Tar: target rate
+// @Field: FF0: FF value single sample
+// @Field: FF: FF value
+// @Field: P: P value
+// @Field: D: D value
+// @Field: Action: action taken
+
 /*
   one update cycle of the autotuner
  */
-void AP_AutoTune::update(float desired_rate, float achieved_rate, float servo_out)
+void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler)
 {
     if (!running) {
         return;
     }
     check_save();
-
     // see what state we are in
-    ATState new_state;
+    ATState new_state = state;
-    float abs_desired_rate = fabsf(desired_rate);
+    const float desired_rate = pinfo.target;
-    uint32_t now = AP_HAL::millis();
+    // filter actuator without I term
+    const float actuator = actuator_filter.apply(pinfo.FF + pinfo.P + pinfo.D);
+    const float actual_rate = rate_filter.apply(pinfo.actual);
 
-    if (fabsf(servo_out) >= 45) {
+    max_actuator = MAX(max_actuator, actuator);
-        // we have saturated the servo demand (not including
+    min_actuator = MIN(min_actuator, actuator);
-        // integrator), we cannot get any information that would allow
+    max_rate = MAX(max_rate, actual_rate);
-        // us to increase the gain
+    min_rate = MIN(min_rate, actual_rate);
-        saturated_surfaces = true;
+    max_target = MAX(max_target, desired_rate);
-    }
+    min_target = MIN(min_target, desired_rate);
+    max_P = MAX(max_P, fabsf(pinfo.P));
+    max_D = MAX(max_D, fabsf(pinfo.D));
+    min_Dmod = MIN(min_Dmod, pinfo.Dmod);
 
-    if (abs_desired_rate < 0.8f * current.rmax) {
+    int16_t att_limit_cd;
-        // we are not demanding max rate
+    if (type == AUTOTUNE_ROLL) {
-        new_state = DEMAND_UNSATURATED;
+        att_limit_cd = aparm.roll_limit_cd;
-    } else if (fabsf(achieved_rate) > abs_desired_rate) {
-        new_state = desired_rate > 0 ? DEMAND_OVER_POS : DEMAND_OVER_NEG;
     } else {
-        new_state = desired_rate > 0 ? DEMAND_UNDER_POS : DEMAND_UNDER_NEG;
+        att_limit_cd = MIN(abs(aparm.pitch_limit_max_cd),abs(aparm.pitch_limit_min_cd));
     }
-    if (new_state != state) {
+    const float rate_threshold1 = 0.75 * MIN(att_limit_cd * 0.01 / current.tau.get(), current.rmax_pos);
-        check_state_exit(now - state_enter_ms);
+    const float rate_threshold2 = 0.25 * rate_threshold1;
+
+    switch (state) {
+    case ATState::IDLE:
+        if (desired_rate > rate_threshold1) {
+            new_state = ATState::DEMAND_POS;
+        } else if (desired_rate < -rate_threshold1) {
+            new_state = ATState::DEMAND_NEG;
+        }
+        break;
+    case ATState::DEMAND_POS:
+        if (desired_rate < rate_threshold2) {
+            new_state = ATState::IDLE;
+        }
+        break;
+    case ATState::DEMAND_NEG:
+        if (desired_rate > -rate_threshold2) {
+            new_state = ATState::IDLE;
+        }
+        break;
+    }
+
+    AP::logger().Write(
+        type==AUTOTUNE_ROLL?"ATNR":"ATNP",
+        "TimeUS,Axis,State,Sur,Tar,FF0,FF,P,D,Action",
+        "s--dk-----",
+        "F--000000-",
+        "QBBffffffB",
+        AP_HAL::micros64(),
+        unsigned(type),
+        unsigned(new_state),
+        actuator,
+        desired_rate,
+        FF0,
+        current.FF,
+        current.P,
+        current.D,
+        unsigned(action));
+
+    if (new_state == state) {
+        return;
+    }
+
+    const uint32_t now = AP_HAL::millis();
+
+    if (new_state != ATState::IDLE) {
+        // starting an event
+        min_actuator = max_actuator = min_rate = max_rate = 0;
+        state_enter_ms = now;
+        state = new_state;
+        return;
+    }
+
+    if ((state == ATState::DEMAND_POS && max_rate < 0.01 * current.rmax_pos) ||
+        (state == ATState::DEMAND_NEG && min_rate > -0.01 * current.rmax_neg)) {
+        // we didn't get enough rate
+        state = ATState::IDLE;
+        action = Action::LOW_RATE;
+        min_Dmod = 1;
+        max_P = max_D = 0;
+        return;
+    }
+
+    if (now - state_enter_ms < 100) {
+        // not long enough sample
+        state = ATState::IDLE;
+        action = Action::SHORT;
+        min_Dmod = 1;
+        max_P = max_D = 0;
+        return;
+    }
+
+    // we've finished an event. calculate the single-event FF value
+    if (state == ATState::DEMAND_POS) {
+        FF0 = max_actuator / (max_rate * scaler);
+    } else {
+        FF0 = min_actuator / (min_rate * scaler);
+    }
+
+    // apply median filter
+    float FF = ff_filter.apply(FF0);
+
+    const float old_FF = rpid.ff();
+
+    // limit size of change in FF
+    FF = constrain_float(FF,
+                         old_FF*(1-AUTOTUNE_DECREASE_FF_STEP*0.01),
+                         old_FF*(1+AUTOTUNE_INCREASE_FF_STEP*0.01));
+
+    // did the P or D components go over 15% of total actuator?
+    const float abs_actuator = MAX(max_actuator, fabsf(min_actuator));
+    const float PD_high = 0.15 * abs_actuator;
+    bool PD_significant = (max_P > PD_high || max_D > PD_high);
+
+    // see if we overshot
+    bool overshot = (state == ATState::DEMAND_POS)?
+        (max_rate > max_target*AUTOTUNE_OVERSHOOT):
+        (min_rate < min_target*AUTOTUNE_OVERSHOOT);
+
+    // adjust P and D
+    float D = rpid.kD();
+    float P = rpid.kP();
+
+    D = MAX(D, 0.0005);
+    P = MAX(P, 0.01);
+
+    // if the slew limiter kicked in or
+    if (min_Dmod < 1.0 || (overshot && PD_significant)) {
+        // we're overshooting or oscillating, decrease gains. We
+        // assume the gain that needs to be reduced is the one that
+        // peaked at a higher value
+        if (max_P < max_D) {
+            D *= (100 - AUTOTUNE_DECREASE_PD_STEP)*0.01;
+        } else {
+            P *= (100 - AUTOTUNE_DECREASE_PD_STEP)*0.01;
+        }
+        action = Action::LOWER_PD;
+    } else {
+        const float low_PD = 0.05 * MAX(max_actuator, fabsf(min_actuator));
+        // not oscillating or overshooting, increase the gains
+        if (max_P < low_PD) {
+            // P is very small, increase rapidly
+            P *= (100 + AUTOTUNE_INCREASE_PD_LOW_STEP)*0.01;
+        } else {
+            P *= (100 + AUTOTUNE_INCREASE_PD_STEP)*0.01;
+        }
+        if (max_D < low_PD) {
+            // D is very small, increase rapidly
+            D *= (100 + AUTOTUNE_INCREASE_PD_LOW_STEP)*0.01;
+        } else {
+            D *= (100 + AUTOTUNE_INCREASE_PD_STEP)*0.01;
+        }
+        action = Action::RAISE_PD;
+    }
+
+
+    rpid.ff().set(FF);
+    rpid.kP().set(P);
+    rpid.kD().set(D);
+    rpid.kI().set(P*AUTOTUNE_I_RATIO);
+    current.FF = FF;
+    current.P = P;
+    current.I = rpid.kI().get();
+    current.D = D;
+
+    Debug("FPID=(%.3f, %.3f, %.3f, %.3f)\n",
+          rpid.ff().get(),
+          rpid.kP().get(),
+          rpid.kI().get(),
+          rpid.kD().get());
+
+    min_Dmod = 1;
+    max_P = max_D = 0;
     state = new_state;
     state_enter_ms = now;
-        saturated_surfaces = false;
-    }
-    if (state != DEMAND_UNSATURATED) {
-        write_log(servo_out, desired_rate, achieved_rate);
-    }
-}
-
-/*
-  called when we change state to see if we should change gains
- */
-void AP_AutoTune::check_state_exit(uint32_t state_time_ms)
-{
-    switch (state) {
-    case DEMAND_UNSATURATED:
-        break;
-    case DEMAND_UNDER_POS:
-    case DEMAND_UNDER_NEG:
-        // we increase P if we have not saturated the surfaces during
-        // this state, and we have
-        if (state_time_ms >= AUTOTUNE_UNDERSHOOT_TIME && !saturated_surfaces) {
-            rpid.ff().set(rpid.ff() * (100+AUTOTUNE_INCREASE_STEP) * 0.01f);
-            if (rpid.ff() > AUTOTUNE_MAX_FF) {
-                rpid.ff().set(AUTOTUNE_MAX_FF);
-            }
-            Debug("UNDER FF -> %.3f\n", rpid.ff().get());
-        }
-        break;
-    case DEMAND_OVER_POS:
-    case DEMAND_OVER_NEG:
-        if (state_time_ms >= AUTOTUNE_OVERSHOOT_TIME) {
-            rpid.ff().set(rpid.ff() * (100-AUTOTUNE_DECREASE_STEP) * 0.01f);
-            if (rpid.ff() < AUTOTUNE_MIN_FF) {
-                rpid.ff().set(AUTOTUNE_MIN_FF);
-            }
-            Debug("OVER FF -> %.3f\n", rpid.ff().get());
-        }
-        break;
-    }
 }
 
 /*
@@ -241,67 +373,44 @@ void AP_AutoTune::check_save(void)
     // the last save period. This means the pilot has
     // AUTOTUNE_SAVE_PERIOD milliseconds to decide they don't like the
     // gains and switch out of autotune
-    ATGains tmp = current;
+    ATGains tmp = get_gains(current);
 
     save_gains(next_save);
-    Debug("SAVE FF -> %.3f\n", rpid.ff().get());
+    last_save = next_save;
 
     // restore our current gains
-    current = tmp;
+    set_gains(tmp);
 
     // if the pilot exits autotune they get these saved values
     restore = next_save;
 
     // the next values to save will be the ones we are flying now
-    next_save = current;
+    next_save = tmp;
     last_save_ms = AP_HAL::millis();
 }
 
-/*
-  log a parameter change from autotune
- */
-void AP_AutoTune::log_param_change(float v, const char *suffix)
-{
-    AP_Logger *logger = AP_Logger::get_singleton();
-    if (!logger->logging_started()) {
-        return;
-    }
-    char key[AP_MAX_NAME_SIZE+1];
-    if (type == AUTOTUNE_ROLL) {
-        strncpy(key, "RLL2SRV_", 9);
-        strncpy(&key[8], suffix, AP_MAX_NAME_SIZE-8);
-    } else {
-        strncpy(key, "PTCH2SRV_", 10);
-        strncpy(&key[9], suffix, AP_MAX_NAME_SIZE-9);
-    }
-    key[AP_MAX_NAME_SIZE] = 0;
-    logger->Write_Parameter(key, v);
-}
-
 /*
   set a float and save a float if it has changed by more than
   0.1%. This reduces the number of insignificant EEPROM writes
  */
-void AP_AutoTune::save_float_if_changed(AP_Float &v, float value, const char *suffix)
+void AP_AutoTune::save_float_if_changed(AP_Float &v, float value)
 {
     float old_value = v.get();
     v.set(value);
     if (value <= 0 || fabsf((value-old_value)/value) > 0.001f) {
         v.save();
-        log_param_change(v.get(), suffix);
     }
 }
 
 /*
   set a int16 and save if changed
  */
-void AP_AutoTune::save_int16_if_changed(AP_Int16 &v, int16_t value, const char *suffix)
+void AP_AutoTune::save_int16_if_changed(AP_Int16 &v, int16_t value)
 {
     int16_t old_value = v.get();
     v.set(value);
     if (old_value != v.get()) {
         v.save();
-        log_param_change(v.get(), suffix);
     }
 }
 
@@ -311,35 +420,43 @@ void AP_AutoTune::save_int16_if_changed(AP_Int16 &v, int16_t value, const char *
  */
 void AP_AutoTune::save_gains(const ATGains &v)
 {
+    ATGains tmp = current;
     current = last_save;
-    save_float_if_changed(current.tau, v.tau, "TCONST");
+    save_float_if_changed(current.tau, v.tau);
-#if 0
+    save_int16_if_changed(current.rmax_pos, v.rmax_pos);
-    save_float_if_changed(rpid.kP(), v.P, "P");
+    save_int16_if_changed(current.rmax_neg, v.rmax_neg);
-    save_float_if_changed(rpid.kI(), v.I, "I");
+    save_float_if_changed(rpid.ff(), v.FF);
-    save_float_if_changed(rpid.kD(), v.D, "D");
+    save_float_if_changed(rpid.kP(), v.P);
-    save_float_if_changed(rpid.ff(), v.FF, "FF");
+    save_float_if_changed(rpid.kI(), v.I);
-    save_int16_if_changed(current.rmax, v.rmax, "RMAX");
+    save_float_if_changed(rpid.kD(), v.D);
-    save_int16_if_changed(current.imax, v.imax, "IMAX");
+    save_float_if_changed(rpid.kIMAX(), v.IMAX);
-#endif
+    last_save = get_gains(current);
-    last_save = current;
+    current = tmp;
 }
 
-void AP_AutoTune::write_log(float servo, float demanded, float achieved)
+/*
+  get gains with PID components
+ */
+AP_AutoTune::ATGains AP_AutoTune::get_gains(const ATGains &v)
 {
-    AP_Logger *logger = AP_Logger::get_singleton();
+    ATGains ret = v;
-    if (!logger->logging_started()) {
+    ret.FF = rpid.ff();
-        return;
+    ret.P = rpid.kP();
-    }
+    ret.I = rpid.kI();
-
+    ret.D = rpid.kD();
-    struct log_ATRP pkt = {
+    ret.IMAX = rpid.kIMAX();
-        LOG_PACKET_HEADER_INIT(LOG_ATRP_MSG),
+    return ret;
-        time_us    : AP_HAL::micros64(),
+}
-        type       : static_cast<uint8_t>(type),
+
-    	state      : (uint8_t)state,
+/*
-        servo      : (int16_t)(servo*100),
+  set gains with PID components
-        demanded   : demanded,
+ */
-        achieved   : achieved,
+void AP_AutoTune::set_gains(const ATGains &v)
-        P          : rpid.kP().get()
+{
-    };
+    current = v;
-    logger->WriteBlock(&pkt, sizeof(pkt));
+    rpid.ff().set(v.FF);
+    rpid.kP().set(v.P);
+    rpid.kI().set(v.I);
+    rpid.kD().set(v.D);
+    rpid.kIMAX().set(v.IMAX);
 }

libraries/APM_Control/AP_AutoTune.h

@@ -10,8 +10,9 @@ class AP_AutoTune {
 public:
     struct ATGains {
         AP_Float tau;
-        AP_Int16 rmax;
+        AP_Int16 rmax_pos;
-        AP_Int16 imax;
+        AP_Int16 rmax_neg;
+        float FF, P, I, D, IMAX;
     };
 
     enum ATType {
@@ -42,11 +43,11 @@ public:
     void stop(void);
 
     // update called whenever autotune mode is active. This is
-    // typically at 50Hz
+    // called at the main loop rate
-    void update(float desired_rate, float achieved_rate, float servo_out);
+    void update(AP_Logger::PID_Info &pid_info, float scaler);
 
     // are we running?
-    bool running:1;
+    bool running;
     
 private:
     // the current gains
@@ -58,9 +59,6 @@ private:
 
 	const AP_Vehicle::FixedWing &aparm;
 
-    // did we saturate surfaces?
-    bool saturated_surfaces:1;
-
     // values to restore if we leave autotune mode
     ATGains restore; 
 
@@ -71,25 +69,50 @@ private:
     ATGains next_save;
 
     // time when we last saved
-    uint32_t last_save_ms = 0;
+    uint32_t last_save_ms;
 
     // the demanded/achieved state
-    enum ATState {DEMAND_UNSATURATED,
+    enum class ATState {IDLE,
-                  DEMAND_UNDER_POS, 
+                        DEMAND_POS,
-                  DEMAND_OVER_POS,
+                        DEMAND_NEG};
-                  DEMAND_UNDER_NEG,
+    ATState state;
-                  DEMAND_OVER_NEG} state = DEMAND_UNSATURATED;
+
+    // the demanded/achieved state
+    enum class Action {NONE,
+                       LOW_RATE,
+                       SHORT,
+                       RAISE_PD,
+                       LOWER_PD};
+    Action action;
 
     // when we entered the current state
-    uint32_t state_enter_ms = 0;
+    uint32_t state_enter_ms;
 
     void check_save(void);
     void check_state_exit(uint32_t state_time_ms);
     void save_gains(const ATGains &v);
 
-    void write_log(float servo, float demanded, float achieved);
+    void save_float_if_changed(AP_Float &v, float value);
+    void save_int16_if_changed(AP_Int16 &v, int16_t value);
 
-    void log_param_change(float v, const char *suffix);
+    // get gains with PID components
-    void save_float_if_changed(AP_Float &v, float value, const char *suffix);
+    ATGains get_gains(const ATGains &current);
-    void save_int16_if_changed(AP_Int16 &v, int16_t value, const char *suffix);
+    void set_gains(const ATGains &v);
+
+    // 5 point mode filter for FF estimate
+    ModeFilterFloat_Size5 ff_filter;
+
+    LowPassFilterFloat actuator_filter;
+    LowPassFilterFloat rate_filter;
+
+    float max_actuator;
+    float min_actuator;
+    float max_rate;
+    float min_rate;
+    float max_target;
+    float min_target;
+    float max_P;
+    float max_D;
+    float min_Dmod;
+    float FF0;
 };

libraries/APM_Control/AP_PitchController.cpp

@@ -41,7 +41,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
 	// @Units: deg/s
 	// @Increment: 1
 	// @User: Advanced
-    AP_GROUPINFO("2SRV_RMAX_UP",     4, AP_PitchController, gains.rmax,   0.0f),
+    AP_GROUPINFO("2SRV_RMAX_UP",     4, AP_PitchController, gains.rmax_pos,   0.0f),
 
     // @Param: 2SRV_RMAX_DN
 	// @DisplayName: Pitch down max rate
@@ -50,7 +50,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
 	// @Units: deg/s
 	// @Increment: 1
 	// @User: Advanced
-    AP_GROUPINFO("2SRV_RMAX_DN",     5, AP_PitchController, _max_rate_neg,   0.0f),
+    AP_GROUPINFO("2SRV_RMAX_DN",     5, AP_PitchController, gains.rmax_neg,   0.0f),
 
     // @Param: 2SRV_RLL
 	// @DisplayName: Roll compensation
@@ -60,33 +60,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("2SRV_RLL",      6, AP_PitchController, _roll_ff,        1.0f),
 
-    // @Param: 2SRV_IMAX
+    // index 7, 8 reserved for old IMAX, FF
-	// @DisplayName: Integrator limit
-	// @Description: Limit of pitch integrator gain in centi-degrees of servo travel. Servos are assumed to have +/- 4500 centi-degrees of travel, so a value of 3000 allows trim of up to 2/3 of servo travel range.
-	// @Range: 0 4500
-	// @Increment: 1
-	// @User: Advanced
-    AP_GROUPINFO("2SRV_IMAX",      7, AP_PitchController, gains.imax,     3000),
-
-    // index 8 reserved for old FF
-
-    // @Param: 2SRV_SRMAX
-    // @DisplayName: Servo slew rate limit
-    // @Description: Sets an upper limit on the servo slew rate produced by the D-gain (pitch rate feedback). If the amplitude of the control action produced by the pitch rate feedback exceeds this value, then the D-gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive D-gain. The limit should be set to no more than 25% of the servo's specified slew rate to allow for inertia and aerodynamic load effects. Note: The D-gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
-    // @Units: deg/s
-    // @Range: 0 500
-    // @Increment: 10.0
-    // @User: Advanced
-    AP_GROUPINFO("2SRV_SRMAX", 9, AP_PitchController, _slew_rate_max, 150.0f),
-
-    // @Param: 2SRV_SRTAU
-    // @DisplayName: Servo slew rate decay time constant
-    // @Description: This sets the time constant used to recover the D gain after it has been reduced due to excessive servo slew rate.
-    // @Units: s
-    // @Range: 0.5 5.0
-    // @Increment: 0.1
-    // @User: Advanced
-    AP_GROUPINFO("2SRV_SRTAU", 10, AP_PitchController, _slew_rate_tau, 1.0f),
 
     // @Param: _RATE_P
     // @DisplayName: Pitch axis rate controller P gain
@@ -154,19 +128,19 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
     // @Increment: 0.5
     // @User: Advanced
 
-    // @Param: _RATE_STAU
-    // @DisplayName: Pitch slew rate decay time constant
-    // @Description: This sets the time constant used to recover the P+D gain after it has been reduced due to excessive slew rate.
-    // @Units: s
-    // @Range: 0.5 5.0
-    // @Increment: 0.1
-    // @User: Advanced
-
     AP_SUBGROUPINFO(rate_pid, "_RATE_", 11, AP_PitchController, AC_PID),
     
     AP_GROUPEND
 };
 
+AP_PitchController::AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms)
+    : aparm(parms)
+    , _ahrs(ahrs)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+    rate_pid.set_slew_limit_scale(45);
+}
+
 /*
   AC_PID based rate controller
 */
@@ -174,7 +148,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
 {
     const float dt = AP::scheduler().get_loop_period_s();
     const float eas2tas = _ahrs.get_EAS2TAS();
-    bool limit_I = fabsf(last_ac_out) >= 45;
+    bool limit_I = fabsf(_last_out) >= 45;
     float rate_y = _ahrs.get_gyro().y;
     float old_I = rate_pid.get_i();
 
@@ -244,7 +218,12 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
     }
 
     // remember the last output to trigger the I limit
-    last_ac_out = out;
+    _last_out = out;
+
+    if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {
+        // let autotune have a go at the values 
+        autotune->update(pinfo, scaler);
+    }
     
     // output is scaled to notional centidegrees of deflection
     return constrain_int32(out * 100, -4500, 4500);
@@ -341,10 +320,10 @@ int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool
 	// as the rates will be tuned when upright, and it is common that
 	// much higher rates are needed inverted	
 	if (!inverted) {
-		if (_max_rate_neg && desired_rate < -_max_rate_neg) {
+        if (gains.rmax_neg && desired_rate < -gains.rmax_neg) {
-			desired_rate = -_max_rate_neg;
+            desired_rate = -gains.rmax_neg;
-		} else if (gains.rmax && desired_rate > gains.rmax) {
+        } else if (gains.rmax_pos && desired_rate > gains.rmax_pos) {
-			desired_rate = gains.rmax;
+            desired_rate = gains.rmax_pos;
 		}
 	}
 	
@@ -376,10 +355,12 @@ void AP_PitchController::convert_pid()
     }
 
     float old_ff=0, old_p=1.0, old_i=0.3, old_d=0.08;
+    int16_t old_imax = 3000;
     bool have_old = AP_Param::get_param_by_index(this, 1, AP_PARAM_FLOAT, &old_p);
     have_old |= AP_Param::get_param_by_index(this, 3, AP_PARAM_FLOAT, &old_i);
     have_old |= AP_Param::get_param_by_index(this, 2, AP_PARAM_FLOAT, &old_d);
     have_old |= AP_Param::get_param_by_index(this, 8, AP_PARAM_FLOAT, &old_ff);
+    have_old |= AP_Param::get_param_by_index(this, 7, AP_PARAM_FLOAT, &old_imax);
     if (!have_old) {
         // none of the old gains were set
         return;
@@ -390,5 +371,34 @@ void AP_PitchController::convert_pid()
     rate_pid.kI().set_and_save_ifchanged(old_i * gains.tau);
     rate_pid.kP().set_and_save_ifchanged(old_d);
     rate_pid.kD().set_and_save_ifchanged(0);
-    rate_pid.kIMAX().set_and_save_ifchanged(gains.imax/4500.0);
+    rate_pid.kIMAX().set_and_save_ifchanged(old_imax/4500.0);
+}
+
+/*
+  start an autotune
+ */
+void AP_PitchController::autotune_start(void)
+{
+    if (autotune == nullptr) {
+        autotune = new AP_AutoTune(gains, AP_AutoTune::AUTOTUNE_PITCH, aparm, rate_pid);
+        if (autotune == nullptr) {
+            if (!failed_autotune_alloc) {
+                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed pitch allocation");
+            }
+            failed_autotune_alloc = true;
+        }
+    }
+    if (autotune != nullptr) {
+        autotune->start();
+    }
+}
+
+/*
+  restore autotune gains
+ */
+void AP_PitchController::autotune_restore(void)
+{
+    if (autotune != nullptr) {
+        autotune->stop();
+    }
 }

libraries/APM_Control/AP_PitchController.h

@@ -7,19 +7,10 @@
 #include <AP_Logger/AP_Logger.h>
 #include <AP_Math/AP_Math.h>
 #include <AC_PID/AC_PID.h>
-#include <Filter/SlewLimiter.h>
 
 class AP_PitchController {
 public:
-    AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms)
+    AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms);
-        : aparm(parms)
-        , autotune(gains, AP_AutoTune::AUTOTUNE_PITCH, parms, rate_pid)
-        , _ahrs(ahrs)
-    {
-        AP_Param::setup_object_defaults(this, var_info);
-        _slew_rate_filter.set_cutoff_frequency(10.0f);
-        _slew_rate_filter.reset(0.0f);
-    }
 
     /* Do not allow copies */
     AP_PitchController(const AP_PitchController &other) = delete;
@@ -39,8 +30,8 @@ public:
         rate_pid.set_integrator(rate_pid.get_i() * 0.995);
     }
     
-    void autotune_start(void) { autotune.start(); }
+    void autotune_start(void);
-    void autotune_restore(void) { autotune.stop(); }
+    void autotune_restore(void);
 
     const       AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
 
@@ -56,12 +47,13 @@ public:
 private:
     const AP_Vehicle::FixedWing &aparm;
     AP_AutoTune::ATGains gains;
-    AP_AutoTune autotune;
+    AP_AutoTune *autotune;
+    bool failed_autotune_alloc;
 	AP_Int16 _max_rate_neg;
 	AP_Float _roll_ff;
 	uint32_t _last_t;
     float _last_out;
-    AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 10, 10, 10, 0.02, 150, 1};
+    AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 10, 7, 4, 0.02, 100, 1};
 
     AP_Logger::PID_Info _pid_info;
 
@@ -69,13 +61,4 @@ private:
     float   _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
 
     AP_AHRS &_ahrs;
-
-    // D gain limit cycle control
-    LowPassFilterFloat _slew_rate_filter;   // LPF applied to the derivative of the control action generated by the angular rate feedback
-    float _slew_rate_amplitude;             // Amplitude of the servo slew rate produced by the angular rate feedback (deg/sec)
-
-    AP_Float _slew_rate_max;                // Maximum permitted angular rate control feedback servo slew rate (deg/sec)
-    AP_Float _slew_rate_tau;                // Time constant used to recover gain after a slew rate exceedance (sec)
-
-    float last_ac_out;
 };

libraries/APM_Control/AP_RollController.cpp

@@ -41,35 +41,9 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
 	// @Units: deg/s
 	// @Increment: 1
 	// @User: Advanced
-    AP_GROUPINFO("2SRV_RMAX",   4, AP_RollController, gains.rmax,       0),
+    AP_GROUPINFO("2SRV_RMAX",   4, AP_RollController, gains.rmax_pos,       0),
 
-    // @Param: 2SRV_IMAX
+    // index 5, 6 reserved for old IMAX, FF
-	// @DisplayName: Integrator limit
-	// @Description: Limit of roll integrator gain in centi-degrees of servo travel. Servos are assumed to have +/- 4500 centi-degrees of travel, so a value of 3000 allows trim of up to 2/3 of servo travel range.
-	// @Range: 0 4500
-	// @Increment: 1
-	// @User: Advanced
-    AP_GROUPINFO("2SRV_IMAX",      5, AP_RollController, gains.imax,        3000),
-
-    // index 6 reserved for old FF
-
-    // @Param: 2SRV_SRMAX
-    // @DisplayName: Servo slew rate limit
-    // @Description: Sets an upper limit on the servo slew rate produced by the D-gain (roll rate feedback). If the amplitude of the control action produced by the roll rate feedback exceeds this value, then the D-gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive D-gain. The parameter should be set to no more than 25% of the servo's specified slew rate to allow for inertia and aerodynamic load effects. Note: The D-gain will not be reduced to less than 10% of the nominal value. A valule of zero will disable this feature.
-    // @Units: deg/s
-    // @Range: 0 500
-    // @Increment: 10.0
-    // @User: Advanced
-    AP_GROUPINFO("2SRV_SRMAX", 7, AP_RollController, _slew_rate_max, 150.0f),
-
-    // @Param: 2SRV_SRTAU
-    // @DisplayName: Servo slew rate decay time constant
-    // @Description: This sets the time constant used to recover the D-gain after it has been reduced due to excessive servo slew rate.
-    // @Units: s
-    // @Range: 0.5 5.0
-    // @Increment: 0.1
-    // @User: Advanced
-    AP_GROUPINFO("2SRV_SRTAU", 8, AP_RollController, _slew_rate_tau, 1.0f),
 
     // @Param: _RATE_P
     // @DisplayName: Roll axis rate controller P gain
@@ -137,19 +111,20 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
     // @Increment: 0.5
     // @User: Advanced
 
-    // @Param: _RATE_STAU
-    // @DisplayName: Roll slew rate decay time constant
-    // @Description: This sets the time constant used to recover the P+D gain after it has been reduced due to excessive slew rate.
-    // @Units: s
-    // @Range: 0.5 5.0
-    // @Increment: 0.1
-    // @User: Advanced
-
     AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_RollController, AC_PID),
     
     AP_GROUPEND
 };
 
+// constructor
+AP_RollController::AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms)
+        : aparm(parms)
+        , _ahrs(ahrs)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+    rate_pid.set_slew_limit_scale(45);
+}
+
 
 /*
   AC_PID based rate controller
@@ -158,7 +133,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
 {
     const float dt = AP::scheduler().get_loop_period_s();
     const float eas2tas = _ahrs.get_EAS2TAS();
-    bool limit_I = fabsf(last_ac_out) >= 45;
+    bool limit_I = fabsf(_last_out) >= 45;
     float rate_x = _ahrs.get_gyro().x;
     float aspeed;
     float old_I = rate_pid.get_i();
@@ -212,7 +187,12 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
     float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D;
 
     // remember the last output to trigger the I limit
-    last_ac_out = out;
+    _last_out = out;
+
+    if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {
+        // let autotune have a go at the values 
+        autotune->update(pinfo, scaler);
+    }
     
     // output is scaled to notional centidegrees of deflection
     return constrain_int32(out * 100, -4500, 4500);
@@ -249,10 +229,10 @@ int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool d
 	float desired_rate = angle_err * 0.01f / gains.tau;
 
     // Limit the demanded roll rate
-    if (gains.rmax && desired_rate < -gains.rmax) {
+    if (gains.rmax_pos && desired_rate < -gains.rmax_pos) {
-        desired_rate = - gains.rmax;
+        desired_rate = - gains.rmax_pos;
-    } else if (gains.rmax && desired_rate > gains.rmax) {
+    } else if (gains.rmax_pos && desired_rate > gains.rmax_pos) {
-        desired_rate = gains.rmax;
+        desired_rate = gains.rmax_pos;
     }
 
     return _get_rate_out(desired_rate, scaler, disable_integrator);
@@ -275,10 +255,12 @@ void AP_RollController::convert_pid()
         return;
     }
     float old_ff=0, old_p=1.0, old_i=0.3, old_d=0.08;
+    int16_t old_imax=3000;
     bool have_old = AP_Param::get_param_by_index(this, 1, AP_PARAM_FLOAT, &old_p);
     have_old |= AP_Param::get_param_by_index(this, 3, AP_PARAM_FLOAT, &old_i);
     have_old |= AP_Param::get_param_by_index(this, 2, AP_PARAM_FLOAT, &old_d);
     have_old |= AP_Param::get_param_by_index(this, 6, AP_PARAM_FLOAT, &old_ff);
+    have_old |= AP_Param::get_param_by_index(this, 5, AP_PARAM_INT16, &old_imax);
     if (!have_old) {
         // none of the old gains were set
         return;
@@ -289,5 +271,34 @@ void AP_RollController::convert_pid()
     rate_pid.kI().set_and_save_ifchanged(old_i * gains.tau);
     rate_pid.kP().set_and_save_ifchanged(old_d);
     rate_pid.kD().set_and_save_ifchanged(0);
-    rate_pid.kIMAX().set_and_save_ifchanged(gains.imax/4500.0);
+    rate_pid.kIMAX().set_and_save_ifchanged(old_imax/4500.0);
+}
+
+/*
+  start an autotune
+ */
+void AP_RollController::autotune_start(void)
+{
+    if (autotune == nullptr) {
+        autotune = new AP_AutoTune(gains, AP_AutoTune::AUTOTUNE_ROLL, aparm, rate_pid);
+        if (autotune == nullptr) {
+            if (!failed_autotune_alloc) {
+                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed roll allocation");
+            }
+            failed_autotune_alloc = true;
+        }
+    }
+    if (autotune != nullptr) {
+        autotune->start();
+    }
+}
+
+/*
+  restore autotune gains
+ */
+void AP_RollController::autotune_restore(void)
+{
+    if (autotune != nullptr) {
+        autotune->stop();
+    }
 }

libraries/APM_Control/AP_RollController.h

@@ -10,15 +10,7 @@
 
 class AP_RollController {
 public:
-    AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms)
+    AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms);
-        : aparm(parms)
-        , autotune(gains, AP_AutoTune::AUTOTUNE_ROLL, parms, rate_pid)
-        , _ahrs(ahrs)
-    {
-        AP_Param::setup_object_defaults(this, var_info);
-        _slew_rate_filter.set_cutoff_frequency(10.0f);
-        _slew_rate_filter.reset(0.0f);
-    }
 
     /* Do not allow copies */
     AP_RollController(const AP_RollController &other) = delete;
@@ -38,8 +30,8 @@ public:
         rate_pid.set_integrator(rate_pid.get_i() * 0.995);
     }
     
-    void autotune_start(void) { autotune.start(); }
+    void autotune_start(void);
-    void autotune_restore(void) { autotune.stop(); }
+    void autotune_restore(void);
 
     const       AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
 
@@ -62,21 +54,15 @@ public:
 private:
     const AP_Vehicle::FixedWing &aparm;
     AP_AutoTune::ATGains gains;
-    AP_AutoTune autotune;
+    AP_AutoTune *autotune;
+    bool failed_autotune_alloc;
 	uint32_t _last_t;
 	float _last_out;
-    AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 10, 10, 10, 0.02, 150, 1};
+    AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 10, 7, 4, 0.02, 100, 1};
-    float last_ac_out;
 
     AP_Logger::PID_Info _pid_info;
 
     int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator);
 
     AP_AHRS &_ahrs;
-
-    // D gain limit cycle control
-    LowPassFilterFloat _slew_rate_filter;   // LPF applied to the derivative of the control action generated by the angular rate feedback
-    float _slew_rate_amplitude;             // Amplitude of the servo slew rate produced by the angular rate feedback (deg/sec)
-    AP_Float _slew_rate_max;                // Maximum permitted angular rate control feedback servo slew rate (deg/sec)
-    AP_Float _slew_rate_tau;                // Time constant used to recover gain after a slew rate exceedance (sec)
 };