AP_AutoTune.cpp

Go to the documentation of this file.

/*
   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "AP_AutoTune.h"

#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>

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

// step size for decreasing gains, percentage
#define AUTOTUNE_DECREASE_STEP 8

// min/max P gains
#define AUTOTUNE_MAX_P 5.0f
#define AUTOTUNE_MIN_P 0.3f

// tau ranges
#define AUTOTUNE_MAX_TAU 0.7f
#define AUTOTUNE_MIN_TAU 0.2f

#define AUTOTUNE_MIN_IMAX 2000
#define AUTOTUNE_MAX_IMAX 4000

// constructor
AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
                         const AP_Vehicle::FixedWing &parms,
                         DataFlash_Class &_dataflash) :
    running(false),
    current(_gains),
    type(_type),
    aparm(parms),
    dataflash(_dataflash),
    saturated_surfaces(false)
{}

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <stdio.h>
# define Debug(fmt, args ...)  do {::printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
 # define Debug(fmt, args ...)
#endif

/*
  auto-tuning table. This table gives the starting values for key
  tuning parameters based on a user chosen AUTOTUNE_LEVEL parameter
  from 1 to 10. Level 1 is a very soft tune. Level 10 is a very
  aggressive tune.
 */
static const struct {
    float tau;
    float Dratio;
    float rmax;
} tuning_table[] = {
    { 0.70f, 0.050f,  20 },   // level 1
    { 0.65f, 0.055f,  30 },   // level 2
    { 0.60f, 0.060f,  40 },   // level 3
    { 0.55f, 0.065f,  50 },   // level 4
    { 0.50f, 0.070f,  60 },   // level 5
    { 0.45f, 0.075f,  75 },   // level 6
    { 0.40f, 0.080f,  90 },   // level 7
    { 0.30f, 0.085f, 120 },   // level 8
    { 0.20f, 0.090f, 160 },   // level 9
    { 0.10f, 0.095f, 210 },   // level 10
    { 0.05f, 0.100f, 300 },   // (yes, it goes to 11)
};

/*
  start an autotune session
*/
void AP_AutoTune::start(void)
{
    running = true;
    state = DEMAND_UNSATURATED;
    uint32_t now = AP_HAL::millis();

    state_enter_ms = now;
    last_save_ms = now;

    last_save = current;
    restore = current;

    uint8_t level = aparm.autotune_level;
    if (level > ARRAY_SIZE(tuning_table)) {
        level = ARRAY_SIZE(tuning_table);
    }
    if (level < 1) {
        level = 1;
    }

    current.rmax.set(tuning_table[level-1].rmax);
    // D gain is scaled to a fixed ratio of P gain
    current.D.set(tuning_table[level-1].Dratio * current.P);
    current.tau.set(tuning_table[level-1].tau);

    current.imax = constrain_float(current.imax, AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX);

    // force a fixed ratio of I to D gain on the rate feedback path
    current.I = 0.5f * current.D / current.tau;

    next_save = current;

    Debug("START P -> %.3f\n", current.P.get());
}

/*
  called when we change state to see if we should change gains
 */
void AP_AutoTune::stop(void)
{
    if (running) {
        running = false;
        save_gains(restore);
    }
}


/*
  one update cycle of the autotuner
 */
void AP_AutoTune::update(float desired_rate, float achieved_rate, float servo_out)
{
    if (!running) {
        return;
    }
    check_save();

    // see what state we are in
    ATState new_state;
    float abs_desired_rate = fabsf(desired_rate);
    uint32_t now = AP_HAL::millis();

    if (fabsf(servo_out) >= 45) {
        // we have saturated the servo demand (not including
        // integrator), we cannot get any information that would allow
        // us to increase the gain
        saturated_surfaces = true;
    }

    if (abs_desired_rate < 0.8f * current.rmax) {
        // we are not demanding max rate
        new_state = DEMAND_UNSATURATED;
    } 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;
    }
    if (new_state != state) {
        check_state_exit(now - state_enter_ms);
        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) {
            current.P.set(current.P * (100+AUTOTUNE_INCREASE_STEP) * 0.01f);
            if (current.P > AUTOTUNE_MAX_P) {
                current.P = AUTOTUNE_MAX_P;
            }
            Debug("UNDER P -> %.3f\n", current.P.get());
        }
        current.D.set(tuning_table[aparm.autotune_level-1].Dratio * current.P);
        break;
    case DEMAND_OVER_POS:
    case DEMAND_OVER_NEG:
        if (state_time_ms >= AUTOTUNE_OVERSHOOT_TIME) {
            current.P.set(current.P * (100-AUTOTUNE_DECREASE_STEP) * 0.01f);
            if (current.P < AUTOTUNE_MIN_P) {
                current.P = AUTOTUNE_MIN_P;
            }
            Debug("OVER P -> %.3f\n", current.P.get());
        }
        current.D.set(tuning_table[aparm.autotune_level-1].Dratio * current.P);
        break;
    }
}

/*
  see if we should save new values
 */
void AP_AutoTune::check_save(void)
{
    if (AP_HAL::millis() - last_save_ms < AUTOTUNE_SAVE_PERIOD) {
        return;
    }

    // save the next_save values, which are the autotune value from
    // 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;

    save_gains(next_save);
    Debug("SAVE P -> %.3f\n", current.P.get());

    // restore our current gains
    current = 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;
    last_save_ms = AP_HAL::millis();
}

/*
  log a parameter change from autotune
 */
void AP_AutoTune::log_param_change(float v, const char *suffix)
{
    if (!dataflash.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;
    dataflash.Log_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)
{
    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)
{
    int16_t old_value = v.get();
    v.set(value);
    if (old_value != v.get()) {
        v.save();
        log_param_change(v.get(), suffix);
    }
}


/*
  save a set of gains
 */
void AP_AutoTune::save_gains(const ATGains &v)
{
    current = last_save;
    save_float_if_changed(current.tau, v.tau, "TCONST");
    save_float_if_changed(current.P, v.P, "P");
    save_float_if_changed(current.I, v.I, "I");
    save_float_if_changed(current.D, v.D, "D");
    save_int16_if_changed(current.rmax, v.rmax, "RMAX");
    save_int16_if_changed(current.imax, v.imax, "IMAX");
    last_save = current;
}

void AP_AutoTune::write_log(float servo, float demanded, float achieved)
{
    if (!dataflash.logging_started()) {
        return;
    }

    struct log_ATRP pkt = {
        LOG_PACKET_HEADER_INIT(LOG_ATRP_MSG),
        time_us    : AP_HAL::micros64(),
        type       : static_cast<uint8_t>(type),
        state      : (uint8_t)state,
        servo      : (int16_t)(servo*100),
        demanded   : demanded,
        achieved   : achieved,
        P          : current.P.get()
    };
    dataflash.WriteBlock(&pkt, sizeof(pkt));
}