From dcdbd433abe02c066e319154526fc2b011411186 Mon Sep 17 00:00:00 2001 From: rusefi Date: Thu, 22 Nov 2018 18:43:41 -0500 Subject: [PATCH] PID auto tune unit test --- firmware/controllers/math/pid_auto_tune.cpp | 88 +++++++++++++++++---- firmware/controllers/math/pid_auto_tune.h | 5 +- unit_tests/test_pid_auto.cpp | 29 ++++--- 3 files changed, 96 insertions(+), 26 deletions(-) diff --git a/firmware/controllers/math/pid_auto_tune.cpp b/firmware/controllers/math/pid_auto_tune.cpp index 491d74ae4e..f08987e481 100644 --- a/firmware/controllers/math/pid_auto_tune.cpp +++ b/firmware/controllers/math/pid_auto_tune.cpp @@ -46,7 +46,7 @@ PID_AutoTune::PID_AutoTune() { controlType = ZIEGLER_NICHOLS_PI; noiseBand = 0.5; - state = AUTOTUNER_OFF; + setState(AUTOTUNER_OFF); oStep = 10.0; 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) { // check ready for new input @@ -102,7 +116,7 @@ bool PID_AutoTune::Runtime(Logging *logging) if (state == AUTOTUNER_OFF) { // initialize working variables the first time around - peakType = NOT_A_PEAK; + setPeakType(NOT_A_PEAK); inputCount = 0; peakCount = 0; setpoint = input; @@ -122,11 +136,11 @@ bool PID_AutoTune::Runtime(Logging *logging) // move to new state if (controlType == AMIGOF_PI) { - state = STEADY_STATE_AT_BASELINE; + setState(STEADY_STATE_AT_BASELINE); } 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 if ((state == RELAY_STEP_UP) && (refVal > setpoint + workingNoiseBand)) { - state = RELAY_STEP_DOWN; + setState(RELAY_STEP_DOWN); justChanged = true; } else if ((state == RELAY_STEP_DOWN) && (refVal < setpoint - workingNoiseBand)) { - state = RELAY_STEP_UP; + setState(RELAY_STEP_UP); justChanged = true; } if (justChanged) @@ -183,6 +197,10 @@ bool PID_AutoTune::Runtime(Logging *logging) Serial.println(asymmetry); #endif +#if EFI_UNIT_TEST + printf("asymmetry=%f\r\n", asymmetry); +#endif /* EFI_UNIT_TEST */ + if (asymmetry > AUTOTUNE_STEP_ASYMMETRY_TOLERANCE) { // relay steps are asymmetric @@ -227,6 +245,10 @@ bool PID_AutoTune::Runtime(Logging *logging) Serial.println(relayBias); #endif +#if EFI_UNIT_TEST + printf("deltaRelayBias=%f relayBias=%f\r\n", deltaRelayBias, relayBias); +#endif /* EFI_UNIT_TEST */ + // reset relay step counter // to give the process value oscillation // time to settle with the new relay bias value @@ -296,6 +318,10 @@ bool PID_AutoTune::Runtime(Logging *logging) Serial.println(state); #endif +#if EFI_UNIT_TEST + printf("setpoint=%f refVal=%f\r\n", setpoint, refVal); +#endif /* EFI_UNIT_TEST */ + // store initial inputs // we don't want to trust the maxes or mins // until the input array is full @@ -354,6 +380,7 @@ bool PID_AutoTune::Runtime(Logging *logging) } avgInput /= (double)(inputCount + 1); + bool stable = (iMax - iMin) <= 2.0 * workingNoiseBand; #if defined (AUTOTUNE_DEBUG) Serial.print(F("iMax ")); Serial.println(iMax); @@ -362,9 +389,19 @@ bool PID_AutoTune::Runtime(Logging *logging) Serial.print(F("avgInput ")); Serial.println(avgInput); Serial.print(F("stable ")); - Serial.println((iMax - iMin) <= 2.0 * workingNoiseBand); + Serial.println(stable); #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 ((iMax - iMin) <= 2.0 * workingNoiseBand) { @@ -375,7 +412,7 @@ bool PID_AutoTune::Runtime(Logging *logging) if (state == STEADY_STATE_AT_BASELINE) { - state = STEADY_STATE_AFTER_STEP_UP; + setState(STEADY_STATE_AFTER_STEP_UP); lastPeaks[0] = avgInput; inputCount = 0; #if EFI_UNIT_TEST @@ -392,16 +429,20 @@ bool PID_AutoTune::Runtime(Logging *logging) Serial.println(K_process); #endif +#if EFI_UNIT_TEST + printf("K_process=%f\r\n", K_process); +#endif /* EFI_UNIT_TEST */ + // bad estimate of process gain if (K_process < 1e-10) // zero { - state = AUTOTUNER_OFF; + setState(AUTOTUNER_OFF); #if EFI_UNIT_TEST printf(":( 4\r\n"); #endif /* EFI_UNIT_TEST */ return false; } - state = RELAY_STEP_DOWN; + setState(RELAY_STEP_DOWN); #if defined (AUTOTUNE_RELAY_BIAS) sumInputSinceLastStep[0] = 0.0; @@ -431,7 +472,7 @@ bool PID_AutoTune::Runtime(Logging *logging) { justChanged = true; } - peakType = MAXIMUM; + setPeakType(MAXIMUM); } else if (isMin) { @@ -439,7 +480,7 @@ bool PID_AutoTune::Runtime(Logging *logging) { justChanged = true; } - peakType = MINIMUM; + setPeakType(MINIMUM); } // update peak times and values @@ -591,7 +632,7 @@ bool PID_AutoTune::Runtime(Logging *logging) // check convergence criterion for amplitude of induced oscillation 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) ) { - state = FAILED; + setState(FAILED); } 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; Td = 0.0; #if EFI_UNIT_TEST - printf("Happy end 1!\r\n"); + printf("Happy end AMIGOF_PI!\r\n"); #endif /* EFI_UNIT_TEST */ // converged return true; @@ -697,10 +738,25 @@ bool PID_AutoTune::Runtime(Logging *logging) Td = tuningRule[controlType].PI_controller() ? 0.0 : Pu / (double) tuningRule[controlType].divisor(TD_DIVISOR); #if EFI_UNIT_TEST - printf("Happy end 2!\r\n"); + printf("Happy end!\r\n"); #endif /* EFI_UNIT_TEST */ // converged return true; } +double PID_AutoTune::GetKp() +{ + return Kp; +} + +double PID_AutoTune::GetKi() +{ + return Kp / Ti; +} + +double PID_AutoTune::GetKd() +{ + return Kp * Td; +} + diff --git a/firmware/controllers/math/pid_auto_tune.h b/firmware/controllers/math/pid_auto_tune.h index 7a0c741b7e..953a46d4e7 100644 --- a/firmware/controllers/math/pid_auto_tune.h +++ b/firmware/controllers/math/pid_auto_tune.h @@ -160,6 +160,10 @@ public: double outputStart; unsigned long sampleTime; + byte nLookBack; + + void setState(AutoTunerState state); + void setPeakType(Peak peakType); private: @@ -171,7 +175,6 @@ private: bool running; // todo: remove this double noiseBand; - byte nLookBack; byte controlType; // * selects autotune algorithm enum AutoTunerState state; // * state of autotuner finite state machine diff --git a/unit_tests/test_pid_auto.cpp b/unit_tests/test_pid_auto.cpp index 13c25e4c1e..c24c889da9 100644 --- a/unit_tests/test_pid_auto.cpp +++ b/unit_tests/test_pid_auto.cpp @@ -20,12 +20,17 @@ Logging logging; static float zigZagOffset = 0; -static zigZagValue(int index) { - int i = index % 20; - if ( i <= 10) { - return i * 10 + zigZagOffset; +#define CYCLE 20 + +/** + * 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 { - return (20 - i) * 10 + zigZagOffset; + return (CYCLE - i) * (100.0 / 2 / CYCLE) + zigZagOffset; } } @@ -35,7 +40,10 @@ void testPidAutoZigZag() { mockTimeMs = 0; PID_AutoTune at; + at.SetLookbackSec(5); at.sampleTime = 0; // not used in math only used to filter values out + assertEqualsM("nLookBack", 20, at.nLookBack); + at.outputStart = 50; @@ -48,8 +56,9 @@ void testPidAutoZigZag() { // assertEqualsLM("min@1", 0, at.absMin); // assertEqualsLM("max@1", 10, at.absMax); assertEqualsM("peakCount", 0, at.peakCount); + int startMockMs = mockTimeMs; - for (; mockTimeMs <= 11; mockTimeMs++) { + for (; mockTimeMs <= 10 + startMockMs; mockTimeMs++) { at.input = zigZagValue(mockTimeMs); at.Runtime(&logging); @@ -68,14 +77,14 @@ void testPidAutoZigZag() { at.input = zigZagValue(mockTimeMs); at.Runtime(&logging); } - assertEqualsM("peakCount@41", 0, at.peakCount); + assertEqualsM("peakCount@41", 2, at.peakCount); // assertEqualsM("Pu@41", 1, cisnan(at.Pu)); for (; mockTimeMs <= 60; mockTimeMs++) { at.input = zigZagValue(mockTimeMs); at.Runtime(&logging); } - assertEqualsM("peakCount@60", 2, at.peakCount); + assertEqualsM("peakCount@60", 4, at.peakCount); //assertEqualsM("Pu@60", 0.02, at.Pu); // zigZagOffset = 10; @@ -84,7 +93,9 @@ void testPidAutoZigZag() { at.input = zigZagValue(mockTimeMs); 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 }