From a417be192fff6736aadba0260e3c6152963a4e8c Mon Sep 17 00:00:00 2001 From: rusefi Date: Thu, 22 Nov 2018 22:14:32 -0500 Subject: [PATCH] PID auto tune unit test --- firmware/controllers/math/pid_auto_tune.cpp | 11 ++- firmware/controllers/math/pid_auto_tune.h | 24 +++--- unit_tests/test_pid_auto.cpp | 87 ++++++++++++++++++--- 3 files changed, 95 insertions(+), 27 deletions(-) diff --git a/firmware/controllers/math/pid_auto_tune.cpp b/firmware/controllers/math/pid_auto_tune.cpp index b206a25285..ffa5cd4b0d 100644 --- a/firmware/controllers/math/pid_auto_tune.cpp +++ b/firmware/controllers/math/pid_auto_tune.cpp @@ -108,6 +108,9 @@ void PID_AutoTune::setPeakType(PidAutoTune_Peak peakType) { #endif /* EFI_UNIT_TEST */ } +/** + * returns true when done, otherwise returns false + */ bool PID_AutoTune::Runtime(Logging *logging) { // check ready for new input @@ -656,7 +659,7 @@ bool PID_AutoTune::Runtime(Logging *logging) if (((byte) state & (CONVERGED | FAILED)) == 0) { #if EFI_UNIT_TEST - printf(":( 1 state=%d\r\n", (int)state); + printf(":( 1 state=%s\r\n", getPidAutoTune_AutoTunerState(state)); #endif /* EFI_UNIT_TEST */ return false; } @@ -744,17 +747,17 @@ bool PID_AutoTune::Runtime(Logging *logging) return true; } -double PID_AutoTune::GetKp() +float PID_AutoTune::GetKp() { return Kp; } -double PID_AutoTune::GetKi() +float PID_AutoTune::GetKi() { return Kp / Ti; } -double PID_AutoTune::GetKd() +float 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 8a015dbc97..cbd775c704 100644 --- a/firmware/controllers/math/pid_auto_tune.h +++ b/firmware/controllers/math/pid_auto_tune.h @@ -124,14 +124,14 @@ public: // than this value double GetNoiseBand(); // this should be accurately set - double GetKp(); // * once autotune is complete, these functions contain the - double GetKi(); // computed tuning parameters. - double GetKd(); // + float GetKp(); // * once autotune is complete, these functions contain the + float GetKi(); // computed tuning parameters. + float GetKd(); // double oStep; byte peakCount; - double input; - double output; + float input; + float output; #if EFI_UNIT_TEST double absMax; @@ -144,6 +144,7 @@ public: void setState(PidAutoTune_AutoTunerState state); void setPeakType(PidAutoTune_Peak peakType); + PidAutoTune_AutoTunerState state; // * state of autotuner finite state machine private: @@ -157,17 +158,16 @@ private: double noiseBand; byte controlType; // * selects autotune algorithm - PidAutoTune_AutoTunerState state; // * state of autotuner finite state machine unsigned long lastTime; PidAutoTune_Peak peakType; unsigned long lastPeakTime[STEPCOUNT]; // * peak time, most recent in array element 0 - double lastPeaks[STEPCOUNT]; // * peak value, most recent in array element 0 - double lastInputs[101]; // * process values, most recent in array element 0 + float lastPeaks[STEPCOUNT]; // * peak value, most recent in array element 0 + float lastInputs[101]; // * process values, most recent in array element 0 byte inputCount; - double workingNoiseBand; - double workingOstep; - double inducedAmplitude; - double Kp, Ti, Td; + float workingNoiseBand; + float workingOstep; + float inducedAmplitude; + float Kp, Ti, Td; // used by AMIGOf tuning rule double calculatePhaseLag(double); // * calculate phase lag from noiseBand and inducedAmplitude diff --git a/unit_tests/test_pid_auto.cpp b/unit_tests/test_pid_auto.cpp index c24c889da9..46e503eed0 100644 --- a/unit_tests/test_pid_auto.cpp +++ b/unit_tests/test_pid_auto.cpp @@ -22,21 +22,25 @@ static float zigZagOffset = 0; #define CYCLE 20 +// range of oscillation +static float oscRange; + /** * 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; + return i * (oscRange / 2 / CYCLE) + zigZagOffset; } else { - return (CYCLE - i) * (100.0 / 2 / CYCLE) + zigZagOffset; + return (CYCLE - i) * (oscRange / 2 / CYCLE) + zigZagOffset; } } -void testPidAutoZigZag() { - printf("*************************************************** testPidAutoZigZag\r\n"); +static void testPidAutoZigZagStable() { + printf("*************************************************** testPidAutoZigZagStable\r\n"); + oscRange = 100; mockTimeMs = 0; PID_AutoTune at; @@ -60,7 +64,8 @@ void testPidAutoZigZag() { for (; mockTimeMs <= 10 + startMockMs; mockTimeMs++) { at.input = zigZagValue(mockTimeMs); - at.Runtime(&logging); + bool result = at.Runtime(&logging); + assertFalseM("should be false#1", result); } // assertEqualsLM("min@11", 0, at.absMin); @@ -69,33 +74,93 @@ void testPidAutoZigZag() { for (; mockTimeMs <= 21; mockTimeMs++) { at.input = zigZagValue(mockTimeMs); - at.Runtime(&logging); + bool result = at.Runtime(&logging); + assertFalseM("should be false#2", result); } assertEqualsM("peakCount@21", 0, at.peakCount); for (; mockTimeMs <= 41; mockTimeMs++) { at.input = zigZagValue(mockTimeMs); - at.Runtime(&logging); + bool result = at.Runtime(&logging); + assertFalseM("should be false#2_2", result); } assertEqualsM("peakCount@41", 2, at.peakCount); // assertEqualsM("Pu@41", 1, cisnan(at.Pu)); for (; mockTimeMs <= 60; mockTimeMs++) { at.input = zigZagValue(mockTimeMs); - at.Runtime(&logging); + bool result = at.Runtime(&logging); + assertFalseM("should be false#4", result); } assertEqualsM("peakCount@60", 4, at.peakCount); //assertEqualsM("Pu@60", 0.02, at.Pu); // zigZagOffset = 10; - for (; mockTimeMs <= 80; mockTimeMs++) { + for (; mockTimeMs <= 69; mockTimeMs++) { + at.input = zigZagValue(mockTimeMs); - at.Runtime(&logging); + bool result = at.Runtime(&logging); + assertFalseM("should be false#4", result); } - assertEqualsM("peakCount@80", 6, at.peakCount); + + at.input = zigZagValue(mockTimeMs); + bool result = at.Runtime(&logging); + assertEqualsM("should be true", 1, result); + + assertEqualsM("output", 0.0, at.output); + + assertEqualsM("peakCount@80", 5, at.peakCount); assertEqualsM("ki", 27.7798, at.GetKi()); assertEqualsM("kd", 0.0, at.GetKd()); // todo: test the same code with noisy zig-zag function } + +static void testPidAutoZigZagGrowingOsc() { + printf("*************************************************** testPidAutoZigZagGrowingOsc\r\n"); + + oscRange = 100; + mockTimeMs = 0; + + PID_AutoTune at; + at.SetLookbackSec(5); + at.sampleTime = 0; // not used in math only used to filter values out + + int startMockMs; + + for (int i =0;i<11;i++) { + startMockMs = mockTimeMs; + printf("loop=%d %d\r\n", i, startMockMs); + for (; mockTimeMs < CYCLE + startMockMs; mockTimeMs++) { + at.input = zigZagValue(mockTimeMs); + bool result = at.Runtime(&logging); + assertFalseM("should be false#4", result); + } + oscRange *= 1.5; + } + + startMockMs = mockTimeMs; +// for (; mockTimeMs < CYCLE + startMockMs; mockTimeMs++) { +// printf("loop2=%d\r\n", mockTimeMs); +// at.input = zigZagValue(mockTimeMs); +// bool result = at.Runtime(&logging); +// assertFalseM("should be false#5", result); +// } + + at.input = zigZagValue(mockTimeMs); + bool result = at.Runtime(&logging); + assertTrueM("should be true#2", result); + assertEqualsM("FAiled", FAILED, at.state); + + assertEqualsM("output Growing", 0.0, at.output); + +} + +void testPidAutoZigZag() { + printf("*************************************************** testPidAutoZigZag\r\n"); + + testPidAutoZigZagStable(); + + testPidAutoZigZagGrowingOsc(); +}