mirror of https://github.com/FOME-Tech/fome-fw.git
120 lines
2.9 KiB
C++
120 lines
2.9 KiB
C++
/**
|
|
* @file pid.cpp
|
|
*
|
|
* https://en.wikipedia.org/wiki/Feedback
|
|
* http://en.wikipedia.org/wiki/PID_controller
|
|
*
|
|
* @date Sep 16, 2014
|
|
* @author Andrey Belomutskiy, (c) 2012-2020
|
|
*/
|
|
|
|
#include "pch.h"
|
|
|
|
#include "efi_pid.h"
|
|
#include "math.h"
|
|
|
|
Pid::Pid() : Pid(nullptr) { }
|
|
|
|
Pid::Pid(pid_s *parameters) {
|
|
initPidClass(parameters);
|
|
}
|
|
|
|
void Pid::initPidClass(pid_s *parameters) {
|
|
m_parameters = parameters;
|
|
resetCounter = 0;
|
|
|
|
reset();
|
|
}
|
|
|
|
bool Pid::isSame(const pid_s *parameters) const {
|
|
if (!m_parameters) {
|
|
// this 'null' could happen on first execution during initialization
|
|
return false;
|
|
}
|
|
|
|
efiAssert(ObdCode::OBD_PCM_Processor_Fault, parameters != NULL, "PID::isSame NULL", false);
|
|
return m_parameters->pFactor == parameters->pFactor
|
|
&& m_parameters->iFactor == parameters->iFactor
|
|
&& m_parameters->dFactor == parameters->dFactor
|
|
&& m_parameters->offset == parameters->offset;
|
|
}
|
|
|
|
float Pid::getUnclampedOutput(float target, float input, float dTime) {
|
|
float error = (target - input) * errorAmplificationCoef;
|
|
lastTarget = target;
|
|
lastInput = input;
|
|
|
|
float pTerm = m_parameters->pFactor * error;
|
|
updateITerm(m_parameters->iFactor * dTime * error);
|
|
dTerm = m_parameters->dFactor / dTime * (error - previousError);
|
|
|
|
previousError = error;
|
|
|
|
if (dTime <=0) {
|
|
warning(ObdCode::CUSTOM_PID_DTERM, "PID: unexpected dTime");
|
|
return pTerm + m_parameters->offset;
|
|
}
|
|
|
|
return pTerm + iTerm + dTerm + m_parameters->offset;
|
|
}
|
|
|
|
float Pid::getOutput(float target, float input, float dTime) {
|
|
float output = getUnclampedOutput(target, input, dTime);
|
|
|
|
if (output > m_parameters->maxValue) {
|
|
output = m_parameters->maxValue;
|
|
} else if (output < m_parameters->minValue) {
|
|
output = m_parameters->minValue;
|
|
}
|
|
|
|
lastOutput = output;
|
|
|
|
return output;
|
|
}
|
|
|
|
void Pid::reset() {
|
|
dTerm = iTerm = 0;
|
|
lastOutput = lastInput = lastTarget = previousError = 0;
|
|
errorAmplificationCoef = 1.0f;
|
|
resetCounter++;
|
|
}
|
|
|
|
float Pid::getIntegration() const {
|
|
return iTerm;
|
|
}
|
|
|
|
void Pid::setErrorAmplification(float coef) {
|
|
errorAmplificationCoef = coef;
|
|
}
|
|
|
|
#if EFI_TUNER_STUDIO
|
|
void Pid::postState(pid_status_s& pidStatus) const {
|
|
pidStatus.output = lastOutput;
|
|
pidStatus.error = previousError;
|
|
pidStatus.pTerm = m_parameters == nullptr ? 0 : m_parameters->pFactor * previousError;
|
|
pidStatus.iTerm = iTerm;
|
|
pidStatus.dTerm = dTerm;
|
|
}
|
|
#endif /* EFI_TUNER_STUDIO */
|
|
|
|
void Pid::updateITerm(float value) {
|
|
iTerm += value;
|
|
/**
|
|
* If we have exceeded the ability of the controlled device to hit target, the I factor will keep accumulating and approach infinity.
|
|
* Here we limit the I-term #353
|
|
*/
|
|
if (iTerm > m_parameters->maxValue * 100) {
|
|
iTerm = m_parameters->maxValue * 100;
|
|
}
|
|
if (iTerm > iTermMax) {
|
|
iTerm = iTermMax;
|
|
}
|
|
|
|
// this is kind of a hack. a proper fix would be having separate additional settings 'maxIValue' and 'minIValye'
|
|
if (iTerm < -m_parameters->maxValue * 100)
|
|
iTerm = -m_parameters->maxValue * 100;
|
|
if (iTerm < iTermMin) {
|
|
iTerm = iTermMin;
|
|
}
|
|
}
|