Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM_Control: Modify D and I gain scaling

This commit is contained in:
Paul Riseborough 2014-04-16 06:58:02 +10:00 committed by Andrew Tridgell
parent 7f00bd7f5d
commit ccc7d36493
1 changed files with 24 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/APM_Control/AP_AutoTune.cpp
View File

@ -91,20 +91,20 @@ AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
*/
static const struct {
float tau;
float D;
float Dratio;
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)
{ 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)
};
/*
@ -131,13 +131,14 @@ void AP_AutoTune::start(void)
}
current.rmax.set(pgm_read_float(&tuning_table[level-1].rmax));
current.D.set( pgm_read_float(&tuning_table[level-1].D));
// D gain is scaled to a fixed ratio of P gain
current.D.set( pgm_read_float(&tuning_table[level-1].Dratio) * current.P);
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
current.I = current.D / current.tau;
// 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;
@ -214,6 +215,7 @@ void AP_AutoTune::check_state_exit(uint32_t state_time_ms)
}
Debug("UNDER P -> %.3f\n", current.P.get());
}
current.D.set( pgm_read_float(&tuning_table[aparm.autotune_level-1].Dratio) * current.P);
break;
case DEMAND_OVER_POS:
case DEMAND_OVER_NEG:
@ -224,6 +226,7 @@ void AP_AutoTune::check_state_exit(uint32_t state_time_ms)
}
Debug("OVER P -> %.3f\n", current.P.get());
}
current.D.set( pgm_read_float(&tuning_table[aparm.autotune_level-1].Dratio) * current.P);
break;
}
}