mirror of https://github.com/rusefi/rusefi-1.git
PID auto tune unit test
This commit is contained in:
parent
5d86f610d5
commit
dcdbd433ab
|
@ -46,7 +46,7 @@ PID_AutoTune::PID_AutoTune() {
|
||||||
|
|
||||||
controlType = ZIEGLER_NICHOLS_PI;
|
controlType = ZIEGLER_NICHOLS_PI;
|
||||||
noiseBand = 0.5;
|
noiseBand = 0.5;
|
||||||
state = AUTOTUNER_OFF;
|
setState(AUTOTUNER_OFF);
|
||||||
oStep = 10.0;
|
oStep = 10.0;
|
||||||
SetLookbackSec(10);
|
SetLookbackSec(10);
|
||||||
}
|
}
|
||||||
|
@ -94,6 +94,20 @@ double PID_AutoTune::calculatePhaseLag(double inducedAmplitude)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void PID_AutoTune::setState(AutoTunerState state) {
|
||||||
|
this->state = state;
|
||||||
|
#if EFI_UNIT_TEST
|
||||||
|
printf("setState %d\r\n", state);
|
||||||
|
#endif /* EFI_UNIT_TEST */
|
||||||
|
}
|
||||||
|
|
||||||
|
void PID_AutoTune::setPeakType(Peak peakType) {
|
||||||
|
this->peakType = peakType;
|
||||||
|
#if EFI_UNIT_TEST
|
||||||
|
printf("peakType %d\r\n", peakType);
|
||||||
|
#endif /* EFI_UNIT_TEST */
|
||||||
|
}
|
||||||
|
|
||||||
bool PID_AutoTune::Runtime(Logging *logging)
|
bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
{
|
{
|
||||||
// check ready for new input
|
// check ready for new input
|
||||||
|
@ -102,7 +116,7 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
if (state == AUTOTUNER_OFF)
|
if (state == AUTOTUNER_OFF)
|
||||||
{
|
{
|
||||||
// initialize working variables the first time around
|
// initialize working variables the first time around
|
||||||
peakType = NOT_A_PEAK;
|
setPeakType(NOT_A_PEAK);
|
||||||
inputCount = 0;
|
inputCount = 0;
|
||||||
peakCount = 0;
|
peakCount = 0;
|
||||||
setpoint = input;
|
setpoint = input;
|
||||||
|
@ -122,11 +136,11 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
// move to new state
|
// move to new state
|
||||||
if (controlType == AMIGOF_PI)
|
if (controlType == AMIGOF_PI)
|
||||||
{
|
{
|
||||||
state = STEADY_STATE_AT_BASELINE;
|
setState(STEADY_STATE_AT_BASELINE);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
state = RELAY_STEP_UP;
|
setState(RELAY_STEP_UP);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -154,12 +168,12 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
// check input and change relay state if necessary
|
// check input and change relay state if necessary
|
||||||
if ((state == RELAY_STEP_UP) && (refVal > setpoint + workingNoiseBand))
|
if ((state == RELAY_STEP_UP) && (refVal > setpoint + workingNoiseBand))
|
||||||
{
|
{
|
||||||
state = RELAY_STEP_DOWN;
|
setState(RELAY_STEP_DOWN);
|
||||||
justChanged = true;
|
justChanged = true;
|
||||||
}
|
}
|
||||||
else if ((state == RELAY_STEP_DOWN) && (refVal < setpoint - workingNoiseBand))
|
else if ((state == RELAY_STEP_DOWN) && (refVal < setpoint - workingNoiseBand))
|
||||||
{
|
{
|
||||||
state = RELAY_STEP_UP;
|
setState(RELAY_STEP_UP);
|
||||||
justChanged = true;
|
justChanged = true;
|
||||||
}
|
}
|
||||||
if (justChanged)
|
if (justChanged)
|
||||||
|
@ -183,6 +197,10 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
Serial.println(asymmetry);
|
Serial.println(asymmetry);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if EFI_UNIT_TEST
|
||||||
|
printf("asymmetry=%f\r\n", asymmetry);
|
||||||
|
#endif /* EFI_UNIT_TEST */
|
||||||
|
|
||||||
if (asymmetry > AUTOTUNE_STEP_ASYMMETRY_TOLERANCE)
|
if (asymmetry > AUTOTUNE_STEP_ASYMMETRY_TOLERANCE)
|
||||||
{
|
{
|
||||||
// relay steps are asymmetric
|
// relay steps are asymmetric
|
||||||
|
@ -227,6 +245,10 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
Serial.println(relayBias);
|
Serial.println(relayBias);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if EFI_UNIT_TEST
|
||||||
|
printf("deltaRelayBias=%f relayBias=%f\r\n", deltaRelayBias, relayBias);
|
||||||
|
#endif /* EFI_UNIT_TEST */
|
||||||
|
|
||||||
// reset relay step counter
|
// reset relay step counter
|
||||||
// to give the process value oscillation
|
// to give the process value oscillation
|
||||||
// time to settle with the new relay bias value
|
// time to settle with the new relay bias value
|
||||||
|
@ -296,6 +318,10 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
Serial.println(state);
|
Serial.println(state);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if EFI_UNIT_TEST
|
||||||
|
printf("setpoint=%f refVal=%f\r\n", setpoint, refVal);
|
||||||
|
#endif /* EFI_UNIT_TEST */
|
||||||
|
|
||||||
// store initial inputs
|
// store initial inputs
|
||||||
// we don't want to trust the maxes or mins
|
// we don't want to trust the maxes or mins
|
||||||
// until the input array is full
|
// until the input array is full
|
||||||
|
@ -354,6 +380,7 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
}
|
}
|
||||||
avgInput /= (double)(inputCount + 1);
|
avgInput /= (double)(inputCount + 1);
|
||||||
|
|
||||||
|
bool stable = (iMax - iMin) <= 2.0 * workingNoiseBand;
|
||||||
#if defined (AUTOTUNE_DEBUG)
|
#if defined (AUTOTUNE_DEBUG)
|
||||||
Serial.print(F("iMax "));
|
Serial.print(F("iMax "));
|
||||||
Serial.println(iMax);
|
Serial.println(iMax);
|
||||||
|
@ -362,9 +389,19 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
Serial.print(F("avgInput "));
|
Serial.print(F("avgInput "));
|
||||||
Serial.println(avgInput);
|
Serial.println(avgInput);
|
||||||
Serial.print(F("stable "));
|
Serial.print(F("stable "));
|
||||||
Serial.println((iMax - iMin) <= 2.0 * workingNoiseBand);
|
Serial.println(stable);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
#if EFI_UNIT_TEST
|
||||||
|
printf("iMax=%f iMin=%f\r\n", iMax, iMin);
|
||||||
|
#endif /* EFI_UNIT_TEST */
|
||||||
|
|
||||||
|
#if EFI_UNIT_TEST
|
||||||
|
printf("avgInput=%f stable=%d\r\n", avgInput, stable);
|
||||||
|
#endif /* EFI_UNIT_TEST */
|
||||||
|
|
||||||
|
|
||||||
// if recent inputs are stable
|
// if recent inputs are stable
|
||||||
if ((iMax - iMin) <= 2.0 * workingNoiseBand)
|
if ((iMax - iMin) <= 2.0 * workingNoiseBand)
|
||||||
{
|
{
|
||||||
|
@ -375,7 +412,7 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
|
|
||||||
if (state == STEADY_STATE_AT_BASELINE)
|
if (state == STEADY_STATE_AT_BASELINE)
|
||||||
{
|
{
|
||||||
state = STEADY_STATE_AFTER_STEP_UP;
|
setState(STEADY_STATE_AFTER_STEP_UP);
|
||||||
lastPeaks[0] = avgInput;
|
lastPeaks[0] = avgInput;
|
||||||
inputCount = 0;
|
inputCount = 0;
|
||||||
#if EFI_UNIT_TEST
|
#if EFI_UNIT_TEST
|
||||||
|
@ -392,16 +429,20 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
Serial.println(K_process);
|
Serial.println(K_process);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if EFI_UNIT_TEST
|
||||||
|
printf("K_process=%f\r\n", K_process);
|
||||||
|
#endif /* EFI_UNIT_TEST */
|
||||||
|
|
||||||
// bad estimate of process gain
|
// bad estimate of process gain
|
||||||
if (K_process < 1e-10) // zero
|
if (K_process < 1e-10) // zero
|
||||||
{
|
{
|
||||||
state = AUTOTUNER_OFF;
|
setState(AUTOTUNER_OFF);
|
||||||
#if EFI_UNIT_TEST
|
#if EFI_UNIT_TEST
|
||||||
printf(":( 4\r\n");
|
printf(":( 4\r\n");
|
||||||
#endif /* EFI_UNIT_TEST */
|
#endif /* EFI_UNIT_TEST */
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
state = RELAY_STEP_DOWN;
|
setState(RELAY_STEP_DOWN);
|
||||||
|
|
||||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||||
sumInputSinceLastStep[0] = 0.0;
|
sumInputSinceLastStep[0] = 0.0;
|
||||||
|
@ -431,7 +472,7 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
{
|
{
|
||||||
justChanged = true;
|
justChanged = true;
|
||||||
}
|
}
|
||||||
peakType = MAXIMUM;
|
setPeakType(MAXIMUM);
|
||||||
}
|
}
|
||||||
else if (isMin)
|
else if (isMin)
|
||||||
{
|
{
|
||||||
|
@ -439,7 +480,7 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
{
|
{
|
||||||
justChanged = true;
|
justChanged = true;
|
||||||
}
|
}
|
||||||
peakType = MINIMUM;
|
setPeakType(MINIMUM);
|
||||||
}
|
}
|
||||||
|
|
||||||
// update peak times and values
|
// update peak times and values
|
||||||
|
@ -591,7 +632,7 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
// check convergence criterion for amplitude of induced oscillation
|
// check convergence criterion for amplitude of induced oscillation
|
||||||
if (((0.5 * (absMax - absMin) - inducedAmplitude) / inducedAmplitude) < AUTOTUNE_PEAK_AMPLITUDE_TOLERANCE)
|
if (((0.5 * (absMax - absMin) - inducedAmplitude) / inducedAmplitude) < AUTOTUNE_PEAK_AMPLITUDE_TOLERANCE)
|
||||||
{
|
{
|
||||||
state = CONVERGED;
|
setState(CONVERGED);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -609,7 +650,7 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
(peakCount >= 20)
|
(peakCount >= 20)
|
||||||
)
|
)
|
||||||
{
|
{
|
||||||
state = FAILED;
|
setState(FAILED);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (((byte) state & (CONVERGED | FAILED)) == 0)
|
if (((byte) state & (CONVERGED | FAILED)) == 0)
|
||||||
|
@ -686,7 +727,7 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
Ti = ((-3.05 + 1.72 * phaseLag) / pow(1.0 + (-6.10 + 3.44 * phaseLag) * kappa_phi, 2)) * Pu;
|
Ti = ((-3.05 + 1.72 * phaseLag) / pow(1.0 + (-6.10 + 3.44 * phaseLag) * kappa_phi, 2)) * Pu;
|
||||||
Td = 0.0;
|
Td = 0.0;
|
||||||
#if EFI_UNIT_TEST
|
#if EFI_UNIT_TEST
|
||||||
printf("Happy end 1!\r\n");
|
printf("Happy end AMIGOF_PI!\r\n");
|
||||||
#endif /* EFI_UNIT_TEST */
|
#endif /* EFI_UNIT_TEST */
|
||||||
// converged
|
// converged
|
||||||
return true;
|
return true;
|
||||||
|
@ -697,10 +738,25 @@ bool PID_AutoTune::Runtime(Logging *logging)
|
||||||
Td = tuningRule[controlType].PI_controller() ?
|
Td = tuningRule[controlType].PI_controller() ?
|
||||||
0.0 : Pu / (double) tuningRule[controlType].divisor(TD_DIVISOR);
|
0.0 : Pu / (double) tuningRule[controlType].divisor(TD_DIVISOR);
|
||||||
#if EFI_UNIT_TEST
|
#if EFI_UNIT_TEST
|
||||||
printf("Happy end 2!\r\n");
|
printf("Happy end!\r\n");
|
||||||
#endif /* EFI_UNIT_TEST */
|
#endif /* EFI_UNIT_TEST */
|
||||||
// converged
|
// converged
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
double PID_AutoTune::GetKp()
|
||||||
|
{
|
||||||
|
return Kp;
|
||||||
|
}
|
||||||
|
|
||||||
|
double PID_AutoTune::GetKi()
|
||||||
|
{
|
||||||
|
return Kp / Ti;
|
||||||
|
}
|
||||||
|
|
||||||
|
double PID_AutoTune::GetKd()
|
||||||
|
{
|
||||||
|
return Kp * Td;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -160,6 +160,10 @@ public:
|
||||||
double outputStart;
|
double outputStart;
|
||||||
|
|
||||||
unsigned long sampleTime;
|
unsigned long sampleTime;
|
||||||
|
byte nLookBack;
|
||||||
|
|
||||||
|
void setState(AutoTunerState state);
|
||||||
|
void setPeakType(Peak peakType);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
@ -171,7 +175,6 @@ private:
|
||||||
bool running; // todo: remove this
|
bool running; // todo: remove this
|
||||||
|
|
||||||
double noiseBand;
|
double noiseBand;
|
||||||
byte nLookBack;
|
|
||||||
byte controlType; // * selects autotune algorithm
|
byte controlType; // * selects autotune algorithm
|
||||||
|
|
||||||
enum AutoTunerState state; // * state of autotuner finite state machine
|
enum AutoTunerState state; // * state of autotuner finite state machine
|
||||||
|
|
|
@ -20,12 +20,17 @@ Logging logging;
|
||||||
|
|
||||||
static float zigZagOffset = 0;
|
static float zigZagOffset = 0;
|
||||||
|
|
||||||
static zigZagValue(int index) {
|
#define CYCLE 20
|
||||||
int i = index % 20;
|
|
||||||
if ( i <= 10) {
|
/**
|
||||||
return i * 10 + zigZagOffset;
|
* output linearly goes from 0 to 100 and back within each 'CYCLE' steps
|
||||||
|
*/
|
||||||
|
static float zigZagValue(int index) {
|
||||||
|
int i = index % CYCLE;
|
||||||
|
if ( i <= CYCLE / 2) {
|
||||||
|
return i * (100.0 / 2 / CYCLE) + zigZagOffset;
|
||||||
} else {
|
} else {
|
||||||
return (20 - i) * 10 + zigZagOffset;
|
return (CYCLE - i) * (100.0 / 2 / CYCLE) + zigZagOffset;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -35,7 +40,10 @@ void testPidAutoZigZag() {
|
||||||
mockTimeMs = 0;
|
mockTimeMs = 0;
|
||||||
|
|
||||||
PID_AutoTune at;
|
PID_AutoTune at;
|
||||||
|
at.SetLookbackSec(5);
|
||||||
at.sampleTime = 0; // not used in math only used to filter values out
|
at.sampleTime = 0; // not used in math only used to filter values out
|
||||||
|
assertEqualsM("nLookBack", 20, at.nLookBack);
|
||||||
|
|
||||||
|
|
||||||
at.outputStart = 50;
|
at.outputStart = 50;
|
||||||
|
|
||||||
|
@ -48,8 +56,9 @@ void testPidAutoZigZag() {
|
||||||
// assertEqualsLM("min@1", 0, at.absMin);
|
// assertEqualsLM("min@1", 0, at.absMin);
|
||||||
// assertEqualsLM("max@1", 10, at.absMax);
|
// assertEqualsLM("max@1", 10, at.absMax);
|
||||||
assertEqualsM("peakCount", 0, at.peakCount);
|
assertEqualsM("peakCount", 0, at.peakCount);
|
||||||
|
int startMockMs = mockTimeMs;
|
||||||
|
|
||||||
for (; mockTimeMs <= 11; mockTimeMs++) {
|
for (; mockTimeMs <= 10 + startMockMs; mockTimeMs++) {
|
||||||
at.input = zigZagValue(mockTimeMs);
|
at.input = zigZagValue(mockTimeMs);
|
||||||
at.Runtime(&logging);
|
at.Runtime(&logging);
|
||||||
|
|
||||||
|
@ -68,14 +77,14 @@ void testPidAutoZigZag() {
|
||||||
at.input = zigZagValue(mockTimeMs);
|
at.input = zigZagValue(mockTimeMs);
|
||||||
at.Runtime(&logging);
|
at.Runtime(&logging);
|
||||||
}
|
}
|
||||||
assertEqualsM("peakCount@41", 0, at.peakCount);
|
assertEqualsM("peakCount@41", 2, at.peakCount);
|
||||||
// assertEqualsM("Pu@41", 1, cisnan(at.Pu));
|
// assertEqualsM("Pu@41", 1, cisnan(at.Pu));
|
||||||
|
|
||||||
for (; mockTimeMs <= 60; mockTimeMs++) {
|
for (; mockTimeMs <= 60; mockTimeMs++) {
|
||||||
at.input = zigZagValue(mockTimeMs);
|
at.input = zigZagValue(mockTimeMs);
|
||||||
at.Runtime(&logging);
|
at.Runtime(&logging);
|
||||||
}
|
}
|
||||||
assertEqualsM("peakCount@60", 2, at.peakCount);
|
assertEqualsM("peakCount@60", 4, at.peakCount);
|
||||||
//assertEqualsM("Pu@60", 0.02, at.Pu);
|
//assertEqualsM("Pu@60", 0.02, at.Pu);
|
||||||
|
|
||||||
// zigZagOffset = 10;
|
// zigZagOffset = 10;
|
||||||
|
@ -84,7 +93,9 @@ void testPidAutoZigZag() {
|
||||||
at.input = zigZagValue(mockTimeMs);
|
at.input = zigZagValue(mockTimeMs);
|
||||||
at.Runtime(&logging);
|
at.Runtime(&logging);
|
||||||
}
|
}
|
||||||
assertEqualsM("peakCount@80", 4, at.peakCount);
|
assertEqualsM("peakCount@80", 6, at.peakCount);
|
||||||
|
assertEqualsM("ki", 27.7798, at.GetKi());
|
||||||
|
assertEqualsM("kd", 0.0, at.GetKd());
|
||||||
|
|
||||||
// todo: test the same code with noisy zig-zag function
|
// todo: test the same code with noisy zig-zag function
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue