diff --git a/firmware/util/math/pid.cpp b/firmware/util/math/pid.cpp index 1d75460842..38b53ff0eb 100644 --- a/firmware/util/math/pid.cpp +++ b/firmware/util/math/pid.cpp @@ -58,7 +58,7 @@ float Pid::getUnclampedOutput(float target, float input, float dTime) { previousError = error; - return pTerm + iTerm + dTerm + parameters->offset; + return pTerm + iTerm + dTerm + getOffset(); } /** @@ -69,8 +69,8 @@ float Pid::getOutput(float target, float input, float dTime) { if (output > parameters->maxValue) { output = parameters->maxValue; - } else if (output < parameters->minValue) { - output = parameters->minValue; + } else if (output < getMinValue()) { + output = getMinValue(); } this->output = output; return output; @@ -114,6 +114,10 @@ float Pid::getOffset(void) const { return parameters->offset; } +float Pid::getMinValue(void) const { + return parameters->minValue; +} + void Pid::setErrorAmplification(float coef) { errorAmplificationCoef = coef; } @@ -152,7 +156,7 @@ void Pid::sleep() { void Pid::showPidStatus(Logging *logging, const char*msg) { scheduleMsg(logging, "%s settings: offset=%d P=%.5f I=%.5f D=%.5f period=%dms", msg, - parameters->offset, + getOffset(), parameters->pFactor, parameters->iFactor, parameters->dFactor, diff --git a/firmware/util/math/pid.h b/firmware/util/math/pid.h index 17c60f9ad2..6f3b9895c3 100644 --- a/firmware/util/math/pid.h +++ b/firmware/util/math/pid.h @@ -53,7 +53,8 @@ public: float getP(void) const; float getI(void) const; float getD(void) const; - float getOffset(void) const; + virtual float getOffset(void) const; + virtual float getMinValue(void) const; float getIntegration(void) const; float getPrevError(void) const; void setErrorAmplification(float coef);