diff --git a/firmware/controllers/electronic_throttle.cpp b/firmware/controllers/electronic_throttle.cpp index 7731d14f36..990ec72a1e 100644 --- a/firmware/controllers/electronic_throttle.cpp +++ b/firmware/controllers/electronic_throttle.cpp @@ -116,7 +116,10 @@ static msg_t etbThread(void *arg) { continue; } + percent_t actualThrottlePosition = getTPS(); + if (engine->etbAutoTune) { + autoTune.input = actualThrottlePosition; autoTune.Runtime(&logger); etbPwmUp.setSimplePwmDutyCycle(autoTune.output); @@ -128,7 +131,6 @@ static msg_t etbThread(void *arg) { percent_t targetPosition = getPedalPosition(PASS_ENGINE_PARAMETER_SIGNATURE); - percent_t actualThrottlePosition = getTPS(); currentEtbDuty = pid.getValue(targetPosition, actualThrottlePosition); @@ -274,7 +276,7 @@ static void setTempOutput(float value) { } static void setTempStep(float value) { - autoTune.oStep = value; + autoTune.SetOutputStep(value); } void initElectronicThrottle(void) { @@ -288,6 +290,7 @@ void initElectronicThrottle(void) { // addConsoleActionF("set_etb", setThrottleDutyCycle); + // this is useful one you do "enable etb_auto" addConsoleActionF("set_etb_output", setTempOutput); addConsoleActionF("set_etb_step", setTempStep); diff --git a/firmware/controllers/math/pid_auto_tune.cpp b/firmware/controllers/math/pid_auto_tune.cpp index ffa5cd4b0d..745fa73fbe 100644 --- a/firmware/controllers/math/pid_auto_tune.cpp +++ b/firmware/controllers/math/pid_auto_tune.cpp @@ -44,7 +44,7 @@ Tuning tuningRule[PID_AutoTune::NO_OVERSHOOT_PID + 1] = PID_AutoTune::PID_AutoTune() { running = false; - controlType = ZIEGLER_NICHOLS_PI; + controlType = ZIEGLER_NICHOLS_PID; noiseBand = 0.5; setState(AUTOTUNER_OFF); oStep = 10.0; @@ -127,7 +127,7 @@ bool PID_AutoTune::Runtime(Logging *logging) lastPeakTime[0] = now; workingNoiseBand = noiseBand; newWorkingNoiseBand = noiseBand; - workingOstep = oStep; + workingOutputstep = oStep; #if defined (AUTOTUNE_RELAY_BIAS) relayBias = 0.0; @@ -293,9 +293,9 @@ bool PID_AutoTune::Runtime(Logging *logging) { #if defined (AUTOTUNE_RELAY_BIAS) - output = outputStart + workingOstep + relayBias; + setOutput(outputStart + workingOstep + relayBias); #else - output = outputStart + workingOstep; + setOutput(outputStart + workingOutputstep); #endif } @@ -303,9 +303,9 @@ bool PID_AutoTune::Runtime(Logging *logging) { #if defined (AUTOTUNE_RELAY_BIAS) - output = outputStart - workingOstep + relayBias; + setOutput(outputStart - workingOstep + relayBias); #else - output = outputStart - workingOstep; + setOutput(outputStart - workingOutputstep); #endif } @@ -425,7 +425,7 @@ bool PID_AutoTune::Runtime(Logging *logging) } // else state == STEADY_STATE_AFTER_STEP_UP // calculate process gain - K_process = (avgInput - lastPeaks[0]) / workingOstep; + K_process = (avgInput - lastPeaks[0]) / workingOutputstep; #if defined (AUTOTUNE_DEBUG) Serial.print(F("Process gain ")); @@ -639,6 +639,8 @@ bool PID_AutoTune::Runtime(Logging *logging) } } + bool tooManyCycles = peakCount >= 20; + bool tooLongBetween = ((now - lastPeakTime[0]) > (unsigned long) (AUTOTUNE_MAX_WAIT_MINUTES * 60000)); // if the autotune has not already converged // terminate after 10 cycles // or if too long between peaks @@ -649,10 +651,13 @@ bool PID_AutoTune::Runtime(Logging *logging) ((now - lastStepTime[0]) > (unsigned long) (AUTOTUNE_MAX_WAIT_MINUTES * 60000)) || #endif - ((now - lastPeakTime[0]) > (unsigned long) (AUTOTUNE_MAX_WAIT_MINUTES * 60000)) || - (peakCount >= 20) + tooLongBetween || + tooManyCycles ) { +#if EFI_UNIT_TEST + printf("tooManyCycles=%d tooLongBetween=%d\r\n", tooManyCycles, tooLongBetween); +#endif /* EFI_UNIT_TEST */ setState(FAILED); } @@ -666,7 +671,7 @@ bool PID_AutoTune::Runtime(Logging *logging) // autotune algorithm has terminated // reset autotuner variables - output = outputStart; + setOutput( outputStart); if (state == FAILED) { @@ -684,7 +689,7 @@ bool PID_AutoTune::Runtime(Logging *logging) // finish up by calculating tuning parameters // calculate ultimate gain - double Ku = 4.0 * workingOstep / (inducedAmplitude * CONST_PI); + double Ku = 4.0 * workingOutputstep / (inducedAmplitude * CONST_PI); #if defined (AUTOTUNE_DEBUG) Serial.print(F("ultimate gain ")); @@ -762,4 +767,30 @@ float PID_AutoTune::GetKd() return Kp * Td; } +void PID_AutoTune::setOutput(float output) { + this->output = output; +#if EFI_UNIT_TEST + printf("output=%f\r\n", output); +#endif /* EFI_UNIT_TEST */ +} + +void PID_AutoTune::SetOutputStep(double Step) +{ + oStep = Step; +} + +double PID_AutoTune::GetOutputStep() +{ + return oStep; +} + +void PID_AutoTune::SetControlType(byte type) +{ + controlType = type; +} + +byte PID_AutoTune::GetControlType() +{ + return controlType; +} diff --git a/firmware/controllers/math/pid_auto_tune.h b/firmware/controllers/math/pid_auto_tune.h index cbd775c704..093906cf49 100644 --- a/firmware/controllers/math/pid_auto_tune.h +++ b/firmware/controllers/math/pid_auto_tune.h @@ -128,11 +128,12 @@ public: float GetKi(); // computed tuning parameters. float GetKd(); // - double oStep; byte peakCount; float input; float output; + void setOutput(float output); + #if EFI_UNIT_TEST double absMax; double absMin; @@ -147,6 +148,7 @@ public: PidAutoTune_AutoTunerState state; // * state of autotuner finite state machine private: + double oStep; double processValueOffset(double, // * returns an estimate of the process value offset double); // as a proportion of the amplitude @@ -165,7 +167,7 @@ private: float lastInputs[101]; // * process values, most recent in array element 0 byte inputCount; float workingNoiseBand; - float workingOstep; + float workingOutputstep; float inducedAmplitude; float Kp, Ti, Td; diff --git a/unit_tests/test_pid_auto.cpp b/unit_tests/test_pid_auto.cpp index 46e503eed0..c98a2c8fae 100644 --- a/unit_tests/test_pid_auto.cpp +++ b/unit_tests/test_pid_auto.cpp @@ -45,6 +45,7 @@ static void testPidAutoZigZagStable() { PID_AutoTune at; at.SetLookbackSec(5); + at.SetControlType(PID_AutoTune::ZIEGLER_NICHOLS_PI); at.sampleTime = 0; // not used in math only used to filter values out assertEqualsM("nLookBack", 20, at.nLookBack); @@ -108,7 +109,7 @@ static void testPidAutoZigZagStable() { bool result = at.Runtime(&logging); assertEqualsM("should be true", 1, result); - assertEqualsM("output", 0.0, at.output); + assertEqualsM("testPidAutoZigZagStable::output", 0.0, at.output); assertEqualsM("peakCount@80", 5, at.peakCount); assertEqualsM("ki", 27.7798, at.GetKi()); @@ -157,9 +158,35 @@ static void testPidAutoZigZagGrowingOsc() { } +static void testPidAutoZigZagZero() { + printf("*************************************************** testPidAutoZigZagGrowingOsc\r\n"); + + oscRange = 0; + 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<110;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; + } +} + void testPidAutoZigZag() { printf("*************************************************** testPidAutoZigZag\r\n"); + + testPidAutoZigZagZero(); testPidAutoZigZagStable(); testPidAutoZigZagGrowingOsc();