diff --git a/firmware/controllers/electronic_throttle.cpp b/firmware/controllers/electronic_throttle.cpp index 90a3ac461e..6a61d3b760 100644 --- a/firmware/controllers/electronic_throttle.cpp +++ b/firmware/controllers/electronic_throttle.cpp @@ -94,6 +94,9 @@ static percent_t currentEtbDuty; static bool wasEtbBraking = false; +// todo: need to fix PWM so that it supports zero duty cycle +#define PERCENT_TO_DUTY(X) (maxF((minI(X, 99.9)) / 100.0, 0.1)) + static msg_t etbThread(void *arg) { UNUSED(arg); while (true) { @@ -127,11 +130,14 @@ static msg_t etbThread(void *arg) { tuneWorkingPid.updateFactors(autoTune.output, 0, 0); float value = tuneWorkingPid.getValue(50, actualThrottlePosition); - scheduleMsg(&logger, "output %f value=%f", autoTune.output, value); - etbPwmUp.setSimplePwmDutyCycle(value); + scheduleMsg(&logger, "AT input=%f output=%f PID=%f", autoTune.input, + autoTune.output, + value); + scheduleMsg(&logger, "AT PID=%f", value); + etbPwmUp.setSimplePwmDutyCycle(PERCENT_TO_DUTY(value)); if (result) { - scheduleMsg(&logger, "GREAT NEWS!"); + scheduleMsg(&logger, "GREAT NEWS! %f/%f/%f", autoTune.GetKp(), autoTune.GetKi(), autoTune.GetKd()); } tuneWorkingPid.sleep(); @@ -144,7 +150,7 @@ static msg_t etbThread(void *arg) { currentEtbDuty = pid.getValue(targetPosition, actualThrottlePosition); - etbPwmUp.setSimplePwmDutyCycle(currentEtbDuty / 100); + etbPwmUp.setSimplePwmDutyCycle(PERCENT_TO_DUTY(currentEtbDuty)); if (boardConfiguration->etbDirectionPin2 != GPIO_UNASSIGNED) { bool needEtbBraking = absF(targetPosition - actualThrottlePosition) < 3; @@ -169,13 +175,13 @@ static msg_t etbThread(void *arg) { } /** + * set_etb X * manual duty cycle control without PID. Percent value from 0 to 100 */ static void setThrottleDutyCycle(float level) { scheduleMsg(&logger, "setting ETB duty=%f", level); - float dc = (minI(level, 98)) / 100.0; - dc = maxF(dc, 0.00001); // todo: need to fix PWM so that it supports zero duty cycle + float dc = PERCENT_TO_DUTY(level); valueOverride = dc; etbPwmUp.setSimplePwmDutyCycle(dc); print("etb duty = %.2f\r\n", dc); @@ -285,15 +291,25 @@ static void setTempOutput(float value) { autoTune.output = value; } -static void setTempStep(float value) { +/** + * set_etbat_step X + */ +static void setAutoStep(float value) { + autoTune.reset(); autoTune.SetOutputStep(value); } +static void setAutoPeriod(int period) { + tuneWorkingPidSettings.period = period; + autoTune.reset(); +} + void initElectronicThrottle(void) { addConsoleAction("ethinfo", showEthInfo); if (!hasPedalPositionSensor()) { return; } + autoTune.SetOutputStep(0.1); startETBPins(); @@ -304,14 +320,16 @@ void initElectronicThrottle(void) { tuneWorkingPidSettings.pFactor = 1; tuneWorkingPidSettings.iFactor = 0; tuneWorkingPidSettings.dFactor = 0; - tuneWorkingPidSettings.offset = 10; // todo: not hard-coded value +// tuneWorkingPidSettings.offset = 10; // todo: not hard-coded value //todo tuneWorkingPidSettings.period = 10; tuneWorkingPidSettings.minValue = 0; tuneWorkingPidSettings.maxValue = 100; + tuneWorkingPidSettings.period = 100; // this is useful one you do "enable etb_auto" - addConsoleActionF("set_etb_output", setTempOutput); - addConsoleActionF("set_etb_step", setTempStep); + addConsoleActionF("set_etbat_output", setTempOutput); + addConsoleActionF("set_etbat_step", setAutoStep); + addConsoleActionI("set_etbat_period", setAutoPeriod); applyPidSettings(); diff --git a/firmware/controllers/math/pid_auto_tune.cpp b/firmware/controllers/math/pid_auto_tune.cpp index c87a0f265c..9e5134cdad 100644 --- a/firmware/controllers/math/pid_auto_tune.cpp +++ b/firmware/controllers/math/pid_auto_tune.cpp @@ -43,14 +43,28 @@ Tuning tuningRule[PID_AutoTune::NO_OVERSHOOT_PID + 1] = }; PID_AutoTune::PID_AutoTune() { + reset(); +} + +void PID_AutoTune::Cancel() +{ + state = AUTOTUNER_OFF; +} + +void PID_AutoTune::reset() { running = false; controlType = ZIEGLER_NICHOLS_PID; noiseBand = 0.5; setState(AUTOTUNER_OFF); oStep = 10.0; + memset(lastPeaks, 0, sizeof(lastPeaks)); + memset(lastInputs, 0, sizeof(lastInputs)); + + logger = NULL; input = output = 0; SetLookbackSec(10); + } void PID_AutoTune::SetLookbackSec(int value) @@ -115,6 +129,8 @@ void PID_AutoTune::setPeakType(PidAutoTune_Peak peakType) { */ bool PID_AutoTune::Runtime(Logging *logger) { + + this->logger = logger; // a bit lazy but good enough // check ready for new input unsigned long now = currentTimeMillis(); @@ -772,6 +788,11 @@ float PID_AutoTune::GetKd() void PID_AutoTune::setOutput(float output) { this->output = output; + + if (logger != NULL) { + scheduleMsg(logger, "setOutput %f %s", output, getPidAutoTune_AutoTunerState(state)); + } + #if EFI_UNIT_TEST printf("output=%f\r\n", output); #endif /* EFI_UNIT_TEST */ diff --git a/firmware/controllers/math/pid_auto_tune.h b/firmware/controllers/math/pid_auto_tune.h index a55b5a1f48..3cb633ff5a 100644 --- a/firmware/controllers/math/pid_auto_tune.h +++ b/firmware/controllers/math/pid_auto_tune.h @@ -110,6 +110,8 @@ public: // returns true when done, otherwise returns false void Cancel(); // * Stops the AutoTune + void reset(); + void SetOutputStep(double); // * how far above and below the starting value will // the output step? double GetOutputStep(); // @@ -128,6 +130,7 @@ public: float GetKi(); // computed tuning parameters. float GetKd(); // + Logging *logger; byte peakCount; float input; // suggested P coefficient while auto-tuning