From ae7293ef6825d81a0dff734e9a1d624fc47f6653 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 13 Apr 2014 22:11:57 +1000 Subject: [PATCH] APM_Control: added support for AUTOTUNE_LEVEL 10 levels of tune, for what type of tune the user wants --- libraries/APM_Control/AP_AutoTune.cpp | 75 ++++++++++++-------- libraries/APM_Control/AP_AutoTune.h | 4 +- libraries/APM_Control/AP_PitchController.cpp | 6 +- libraries/APM_Control/AP_PitchController.h | 2 +- libraries/APM_Control/AP_RollController.h | 2 +- 5 files changed, 52 insertions(+), 37 deletions(-) diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp index 49210aed32..e6e4e2220c 100644 --- a/libraries/APM_Control/AP_AutoTune.cpp +++ b/libraries/APM_Control/AP_AutoTune.cpp @@ -49,28 +49,14 @@ extern const AP_HAL::HAL& hal; // how much time we have to undershoot for to increase gains #define AUTOTUNE_UNDERSHOOT_TIME 200 -// step size for increasing gains -#define AUTOTUNE_INCREASE_STEP 0.05f +// step size for increasing gains, percentage +#define AUTOTUNE_INCREASE_STEP 5 -// step size for decreasing gains -#define AUTOTUNE_DECREASE_STEP 0.1f - -// rate ranges -#define AUTOTUNE_MAX_RATE 90 -#define AUTOTUNE_DEF_RATE 60 -#define AUTOTUNE_MIN_RATE 30 - -// I ranges -#define AUTOTUNE_MAX_I 0.3 -#define AUTOTUNE_DEF_I 0.1 -#define AUTOTUNE_MIN_I 0.05 - -// D ranges -#define AUTOTUNE_MAX_D 0.15 -#define AUTOTUNE_MIN_D 0.05 +// step size for decreasing gains, percentage +#define AUTOTUNE_DECREASE_STEP 8 // min/max P gains -#define AUTOTUNE_MAX_P 4.0f +#define AUTOTUNE_MAX_P 5.0f #define AUTOTUNE_MIN_P 0.3f // tau ranges @@ -81,9 +67,12 @@ extern const AP_HAL::HAL& hal; #define AUTOTUNE_MAX_IMAX 4000 // constructor -AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type, DataFlash_Class &_dataflash) : +AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type, + const AP_Vehicle::FixedWing &parms, + DataFlash_Class &_dataflash) : current(_gains), type(_type), + aparm(parms), dataflash(_dataflash) {} @@ -94,6 +83,30 @@ AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type, DataFlash_Class &_datafl # 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 + agressive tune. + */ +static const struct { + float tau; + float D; + float rmax; +} tuning_table[] PROGMEM = { + { 0.70f, 0.02f, 20 }, // level 1 + { 0.65f, 0.03f, 30 }, // level 2 + { 0.60f, 0.04f, 40 }, // level 3 + { 0.55f, 0.05f, 50 }, // level 4 + { 0.50f, 0.05f, 60 }, // level 5 + { 0.45f, 0.07f, 75 }, // level 6 + { 0.40f, 0.10f, 90 }, // level 7 + { 0.30f, 0.15f, 120 }, // level 8 + { 0.20f, 0.20f, 160 }, // level 9 + { 0.10f, 0.30f, 210 }, // level 10 + { 0.05f, 0.60f, 300 }, // (yes, it goes to 11) +}; + /* start an autotune session */ @@ -109,16 +122,18 @@ void AP_AutoTune::start(void) last_save = current; restore = current; - /* - set some reasonable values for I and D if the user hasn't set - them at all - */ - if (current.rmax < AUTOTUNE_MIN_RATE || current.rmax > AUTOTUNE_MAX_RATE) { - current.rmax.set(AUTOTUNE_DEF_RATE); + uint8_t level = aparm.autotune_level; + if (level > sizeof(tuning_table)/sizeof(tuning_table[0])) { + level = sizeof(tuning_table)/sizeof(tuning_table[0]); + } + if (level < 1) { + level = 1; } - current.D = constrain_float(current.D, AUTOTUNE_MIN_D, AUTOTUNE_MAX_D); - current.tau = constrain_float(current.tau, AUTOTUNE_MIN_TAU, AUTOTUNE_MAX_TAU); + current.rmax.set(pgm_read_float(&tuning_table[level-1].rmax)); + current.D.set( pgm_read_float(&tuning_table[level-1].D)); + current.tau.set( pgm_read_float(&tuning_table[level-1].tau)); + current.imax = constrain_float(current.imax, AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX); // force a 45 degree phase @@ -193,7 +208,7 @@ void AP_AutoTune::check_state_exit(uint32_t state_time_ms) // 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 += AUTOTUNE_INCREASE_STEP; + current.P.set(current.P * (100+AUTOTUNE_INCREASE_STEP) * 0.01f); if (current.P > AUTOTUNE_MAX_P) { current.P = AUTOTUNE_MAX_P; } @@ -203,7 +218,7 @@ void AP_AutoTune::check_state_exit(uint32_t state_time_ms) case DEMAND_OVER_POS: case DEMAND_OVER_NEG: if (state_time_ms >= AUTOTUNE_OVERSHOOT_TIME) { - current.P -= AUTOTUNE_DECREASE_STEP; + current.P.set(current.P * (100-AUTOTUNE_DECREASE_STEP) * 0.01f); if (current.P < AUTOTUNE_MIN_P) { current.P = AUTOTUNE_MIN_P; } diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h index 510a03601d..a977fcb143 100644 --- a/libraries/APM_Control/AP_AutoTune.h +++ b/libraries/APM_Control/AP_AutoTune.h @@ -24,7 +24,7 @@ public: }; // constructor - AP_AutoTune(ATGains &_gains, ATType type, DataFlash_Class &_dataflash); + AP_AutoTune(ATGains &_gains, ATType type, const AP_Vehicle::FixedWing &parms, DataFlash_Class &_dataflash); // called when autotune mode is entered void start(void); @@ -47,6 +47,8 @@ private: // what type of autotune is this ATType type; + const AP_Vehicle::FixedWing &aparm; + DataFlash_Class &dataflash; // did we saturate surfaces? diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 33ffac292e..ba4a5c22bb 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -170,10 +170,8 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool // contributed autotune.update(desired_rate, achieved_rate, _last_out); - // limit pitch down rate to rate up - if (_max_rate_neg == 0 || _max_rate_neg > gains.rmax) { - _max_rate_neg = gains.rmax; - } + // set down rate to rate up when auto-tuning + _max_rate_neg.set_and_save_ifchanged(gains.rmax); } _last_out += _integrator; diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 5dec794819..f76d31a0b8 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -14,7 +14,7 @@ class AP_PitchController { public: AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, DataFlash_Class &_dataflash) : aparm(parms), - autotune(gains, AP_AutoTune::AUTOTUNE_PITCH, _dataflash), + autotune(gains, AP_AutoTune::AUTOTUNE_PITCH, parms, _dataflash), _ahrs(ahrs) { AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index 261bdf46fb..610bc21811 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -14,7 +14,7 @@ class AP_RollController { public: AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, DataFlash_Class &_dataflash) : aparm(parms), - autotune(gains, AP_AutoTune::AUTOTUNE_ROLL, _dataflash), + autotune(gains, AP_AutoTune::AUTOTUNE_ROLL, parms, _dataflash), _ahrs(ahrs) { AP_Param::setup_object_defaults(this, var_info);