mirror of https://github.com/rusefi/rusefi-1.git
migrating to a much nicer pid auto tune lib
This commit is contained in:
rusefi
parent
f0dd3d3a6a
commit
20c70904a3
|
|
@ -6,137 +6,701 @@
|
|||
*
|
||||
*
|
||||
* Created on: Sep 13, 2017
|
||||
* @author Andrey Belomutskiy, (c) 2012-2018
|
||||
*/
|
||||
|
||||
// source of Tyreus-Luyben and Ciancone-Marlin rules:
|
||||
// "Autotuning of PID Controllers: A Relay Feedback Approach",
|
||||
// by Cheng-Ching Yu, 2nd Edition, p.18
|
||||
// Tyreus-Luyben is more conservative than Ziegler-Nichols
|
||||
// and is preferred for lag dominated processes
|
||||
// Ciancone-Marlin is preferred for delay dominated processes
|
||||
// Ziegler-Nichols is intended for best disturbance rejection
|
||||
// can lack robustness especially for lag dominated processes
|
||||
|
||||
// source for Pessen Integral, Some Overshoot, and No Overshoot rules:
|
||||
// "Rule-Based Autotuning Based on Frequency Domain Identification"
|
||||
// by Anthony S. McCormack and Keith R. Godfrey
|
||||
// IEEE Transactions on Control Systems Technology, vol 6 no 1, January 1998.
|
||||
// as reported on http://www.mstarlabs.com/control/znrule.html
|
||||
|
||||
#include "pid_auto_tune.h"
|
||||
#include "efilib.h"
|
||||
#include "efitime.h"
|
||||
|
||||
// order must be match enumerated type for auto tune methods
|
||||
Tuning tuningRule[PID_AutoTune::NO_OVERSHOOT_PID + 1] =
|
||||
{
|
||||
{ { 44, 24, 0 } }, // ZIEGLER_NICHOLS_PI
|
||||
{ { 34, 40, 160 } }, // ZIEGLER_NICHOLS_PID
|
||||
{ { 64, 9, 0 } }, // TYREUS_LUYBEN_PI
|
||||
{ { 44, 9, 126 } }, // TYREUS_LUYBEN_PID
|
||||
{ { 66, 80, 0 } }, // CIANCONE_MARLIN_PI
|
||||
{ { 66, 88, 162 } }, // CIANCONE_MARLIN_PID
|
||||
{ { 28, 50, 133 } }, // PESSEN_INTEGRAL_PID
|
||||
{ { 60, 40, 60 } }, // SOME_OVERSHOOT_PID
|
||||
{ { 100, 40, 60 } } // NO_OVERSHOOT_PID
|
||||
};
|
||||
|
||||
PID_AutoTune::PID_AutoTune() {
|
||||
running = false;
|
||||
nLookBack = 50;
|
||||
oStep = 10;
|
||||
|
||||
controlType = ZIEGLER_NICHOLS_PI;
|
||||
noiseBand = 0.5;
|
||||
|
||||
state = AUTOTUNER_OFF;
|
||||
oStep = 10.0;
|
||||
SetLookbackSec(10);
|
||||
}
|
||||
|
||||
//void PID_AutoTune::FinishUp() {
|
||||
// Ku = 4 * (2 * oStep) / ((absMax - absMin) * 3.14159);
|
||||
// Pu = (float) (currentPeakTime - prevPeakTime) / 1000.0; // converting ms to seconds
|
||||
//
|
||||
//}
|
||||
|
||||
bool PID_AutoTune::Runtime(Logging *logging) {
|
||||
|
||||
if (peakCount > 9 && running) {
|
||||
running = false;
|
||||
// FinishUp();
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!running) {
|
||||
//initialize working variables the first time around
|
||||
peakType = NOT_A_PEAK;
|
||||
peakCount = 0;
|
||||
|
||||
absMax = input;
|
||||
absMin = input;
|
||||
setpoint = input;
|
||||
running = true;
|
||||
inputCount = 0;
|
||||
outputStart = output;
|
||||
output = outputStart + oStep;
|
||||
|
||||
for (int i = nLookBack - 1; i >= 0; i--) {
|
||||
// default values for history array
|
||||
lastInputs[i] = input;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (input > absMax)
|
||||
absMax = input;
|
||||
if (input < absMin)
|
||||
absMin = input;
|
||||
inputCount ++;
|
||||
}
|
||||
|
||||
//oscillate the output base on the input's relation to the setpoint
|
||||
|
||||
float prevOutput = output;
|
||||
if (input > setpoint + noiseBand)
|
||||
output = outputStart - oStep;
|
||||
else if (input < setpoint - noiseBand)
|
||||
output = outputStart + oStep;
|
||||
|
||||
if (output != prevOutput) {
|
||||
scheduleMsg(logging, "direction change %.2f", output);
|
||||
}
|
||||
|
||||
|
||||
bool isMax = true; // is current input max value for the known input history?
|
||||
bool isMin = true; // is current input min value for the known input history?
|
||||
//identify peaks
|
||||
for (int i = nLookBack - 1; i >= 0; i--) {
|
||||
float val = lastInputs[i];
|
||||
if (isMax)
|
||||
isMax = input >= val;
|
||||
if (isMin)
|
||||
isMin = input <= val;
|
||||
lastInputs[i + 1] = lastInputs[i];
|
||||
}
|
||||
lastInputs[0] = input;
|
||||
if (inputCount < 9) { //we don't want to trust the maxes or mins until the inputs array has been filled
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (isMax || isMin) {
|
||||
scheduleMsg(logging, "min %d max %d %.2f peakType=%d", isMin, isMax, input, peakType);
|
||||
}
|
||||
|
||||
|
||||
bool directionJustChanged = false;
|
||||
|
||||
if (isMax) {
|
||||
if (peakType == NOT_A_PEAK )
|
||||
peakType = MAXIMUM ;
|
||||
if (peakType == MINIMUM ) {
|
||||
peakType = MAXIMUM ;
|
||||
directionJustChanged = true;
|
||||
}
|
||||
|
||||
// peaks[peakCount] = input; we are not using max peak values
|
||||
|
||||
} else if (isMin) {
|
||||
if (peakType == NOT_A_PEAK )
|
||||
peakType = MINIMUM ;
|
||||
if (peakType == MAXIMUM ) {
|
||||
peakType = MINIMUM;
|
||||
lastPeakTime[1] = lastPeakTime[0];
|
||||
lastPeakTime[0] = currentTimeMillis();
|
||||
directionJustChanged = true;
|
||||
|
||||
if (peakCount < 10) {
|
||||
peakCount++;
|
||||
lastPeaks[peakCount] = input;
|
||||
} else {
|
||||
// todo: reset peak counter maybe?
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (directionJustChanged && peakCount > 2) { //we've transitioned. check if we can autotune based on the last peaks
|
||||
float avgSeparation = (absF(lastPeaks[peakCount - 0] - lastPeaks[peakCount - 1])
|
||||
+ absF(lastPeaks[peakCount - 1] - lastPeaks[peakCount - 2])) / 2;
|
||||
if (avgSeparation < 0.05 * (absMax - absMin)) {
|
||||
//FinishUp();
|
||||
running = false;
|
||||
return 1;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
void PID_AutoTune::SetLookbackSec(int value)
|
||||
{
|
||||
if (value < 1)
|
||||
{
|
||||
value = 1;
|
||||
}
|
||||
if (value < 25)
|
||||
{
|
||||
nLookBack = value * 4;
|
||||
sampleTime = 250;
|
||||
}
|
||||
else
|
||||
{
|
||||
nLookBack = 100;
|
||||
sampleTime = value * 10;
|
||||
}
|
||||
}
|
||||
|
||||
double inline PID_AutoTune::fastArcTan(double x)
|
||||
{
|
||||
// source: “Efficient approximations for the arctangent function”, Rajan, S. Sichun Wang Inkol, R. Joyal, A., May 2006
|
||||
//return CONST_PI / 4.0 * x - x * (abs(x) - 1.0) * (0.2447 + 0.0663 * abs(x));
|
||||
|
||||
// source: "Understanding Digital Signal Processing", 2nd Ed, Richard G. Lyons, eq. 13-107
|
||||
return x / (1.0 + 0.28125 * pow(x, 2));
|
||||
}
|
||||
|
||||
double PID_AutoTune::calculatePhaseLag(double inducedAmplitude)
|
||||
{
|
||||
// calculate phase lag
|
||||
// NB hysteresis = 2 * noiseBand;
|
||||
double ratio = 2.0 * workingNoiseBand / inducedAmplitude;
|
||||
if (ratio > 1.0)
|
||||
{
|
||||
return CONST_PI / 2.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
//return CONST_PI - asin(ratio);
|
||||
return CONST_PI - fastArcTan(ratio / sqrt( 1.0 - pow(ratio, 2)));
|
||||
}
|
||||
}
|
||||
|
||||
bool PID_AutoTune::Runtime(Logging *logging)
|
||||
{
|
||||
// check ready for new input
|
||||
unsigned long now = currentTimeMillis();
|
||||
|
||||
if (state == AUTOTUNER_OFF)
|
||||
{
|
||||
// initialize working variables the first time around
|
||||
peakType = NOT_A_PEAK;
|
||||
inputCount = 0;
|
||||
peakCount = 0;
|
||||
setpoint = input;
|
||||
outputStart = output;
|
||||
lastPeakTime[0] = now;
|
||||
workingNoiseBand = noiseBand;
|
||||
newWorkingNoiseBand = noiseBand;
|
||||
workingOstep = oStep;
|
||||
|
||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||
relayBias = 0.0;
|
||||
stepCount = 0;
|
||||
lastStepTime[0] = now;
|
||||
sumInputSinceLastStep[0] = 0.0;
|
||||
#endif
|
||||
|
||||
// move to new state
|
||||
if (controlType == AMIGOF_PI)
|
||||
{
|
||||
state = STEADY_STATE_AT_BASELINE;
|
||||
}
|
||||
else
|
||||
{
|
||||
state = RELAY_STEP_UP;
|
||||
}
|
||||
}
|
||||
|
||||
// otherwise check ready for new input
|
||||
else if ((now - lastTime) < sampleTime)
|
||||
{
|
||||
#if EFI_UNIT_TEST
|
||||
printf("too soon for new input %d %d %d\r\n", now, lastTime, sampleTime);
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
return false;
|
||||
}
|
||||
|
||||
// get new input
|
||||
lastTime = now;
|
||||
double refVal = input;
|
||||
|
||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||
// used to calculate relay bias
|
||||
sumInputSinceLastStep[0] += refVal;
|
||||
#endif
|
||||
|
||||
// local flag variable
|
||||
bool justChanged = false;
|
||||
|
||||
// check input and change relay state if necessary
|
||||
if ((state == RELAY_STEP_UP) && (refVal > setpoint + workingNoiseBand))
|
||||
{
|
||||
state = RELAY_STEP_DOWN;
|
||||
justChanged = true;
|
||||
}
|
||||
else if ((state == RELAY_STEP_DOWN) && (refVal < setpoint - workingNoiseBand))
|
||||
{
|
||||
state = RELAY_STEP_UP;
|
||||
justChanged = true;
|
||||
}
|
||||
if (justChanged)
|
||||
{
|
||||
workingNoiseBand = newWorkingNoiseBand;
|
||||
|
||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||
// check symmetry of oscillation
|
||||
// and introduce relay bias if necessary
|
||||
if (stepCount > 4)
|
||||
{
|
||||
double avgStep1 = 0.5 * (double) ((lastStepTime[0] - lastStepTime[1]) + (lastStepTime[2] - lastStepTime[3]));
|
||||
double avgStep2 = 0.5 * (double) ((lastStepTime[1] - lastStepTime[2]) + (lastStepTime[3] - lastStepTime[4]));
|
||||
if ((avgStep1 > 1e-10) && (avgStep2 > 1e-10))
|
||||
{
|
||||
double asymmetry = (avgStep1 > avgStep2) ?
|
||||
(avgStep1 - avgStep2) / avgStep1 : (avgStep2 - avgStep1) / avgStep2;
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("asymmetry "));
|
||||
Serial.println(asymmetry);
|
||||
#endif
|
||||
|
||||
if (asymmetry > AUTOTUNE_STEP_ASYMMETRY_TOLERANCE)
|
||||
{
|
||||
// relay steps are asymmetric
|
||||
// calculate relay bias using
|
||||
// "Autotuning of PID Controllers: A Relay Feedback Approach",
|
||||
// by Cheng-Ching Yu, 2nd Edition, equation 7.39, p. 148
|
||||
|
||||
// calculate change in relay bias
|
||||
double deltaRelayBias = - processValueOffset(avgStep1, avgStep2) * workingOstep;
|
||||
if (state == RELAY_STEP_DOWN)
|
||||
{
|
||||
deltaRelayBias = -deltaRelayBias;
|
||||
}
|
||||
|
||||
if (abs(deltaRelayBias) > workingOstep * AUTOTUNE_STEP_ASYMMETRY_TOLERANCE)
|
||||
{
|
||||
// change is large enough to bother with
|
||||
relayBias += deltaRelayBias;
|
||||
|
||||
/*
|
||||
// adjust step height with respect to output limits
|
||||
// commented out because the auto tuner does not
|
||||
// necessarily know what the output limits are
|
||||
double relayHigh = outputStart + workingOstep + relayBias;
|
||||
double relayLow = outputStart - workingOstep + relayBias;
|
||||
if (relayHigh > outMax)
|
||||
{
|
||||
relayHigh = outMax;
|
||||
}
|
||||
if (relayLow < outMin)
|
||||
{
|
||||
relayHigh = outMin;
|
||||
}
|
||||
workingOstep = 0.5 * (relayHigh - relayLow);
|
||||
relayBias = relayHigh - outputStart - workingOstep;
|
||||
*/
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("deltaRelayBias "));
|
||||
Serial.println(deltaRelayBias);
|
||||
Serial.print(F("relayBias "));
|
||||
Serial.println(relayBias);
|
||||
#endif
|
||||
|
||||
// reset relay step counter
|
||||
// to give the process value oscillation
|
||||
// time to settle with the new relay bias value
|
||||
stepCount = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// shift step time and integrated process value arrays
|
||||
for (byte i = (stepCount > 4 ? 4 : stepCount); i > 0; i--)
|
||||
{
|
||||
lastStepTime[i] = lastStepTime[i - 1];
|
||||
sumInputSinceLastStep[i] = sumInputSinceLastStep[i - 1];
|
||||
}
|
||||
stepCount++;
|
||||
lastStepTime[0] = now;
|
||||
sumInputSinceLastStep[0] = 0.0;
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG
|
||||
for (byte i = 1; i < (stepCount > 4 ? 5 : stepCount); i++)
|
||||
{
|
||||
Serial.print(F("step time "));
|
||||
Serial.println(lastStepTime[i]);
|
||||
Serial.print(F("step sum "));
|
||||
Serial.println(sumInputSinceLastStep[i]);
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // if defined AUTOTUNE_RELAY_BIAS
|
||||
|
||||
} // if justChanged
|
||||
|
||||
// set output
|
||||
// FIXME need to respect output limits
|
||||
// not knowing output limits is one reason
|
||||
// to pass entire PID object to autotune method(s)
|
||||
if (((byte) state & (STEADY_STATE_AFTER_STEP_UP | RELAY_STEP_UP)) > 0)
|
||||
{
|
||||
|
||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||
output = outputStart + workingOstep + relayBias;
|
||||
#else
|
||||
output = outputStart + workingOstep;
|
||||
#endif
|
||||
|
||||
}
|
||||
else if (state == RELAY_STEP_DOWN)
|
||||
{
|
||||
|
||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||
output = outputStart - workingOstep + relayBias;
|
||||
#else
|
||||
output = outputStart - workingOstep;
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("refVal "));
|
||||
Serial.println(refVal);
|
||||
Serial.print(F("setpoint "));
|
||||
Serial.println(setpoint);
|
||||
Serial.print(F("output "));
|
||||
Serial.println(output);
|
||||
Serial.print(F("state "));
|
||||
Serial.println(state);
|
||||
#endif
|
||||
|
||||
// store initial inputs
|
||||
// we don't want to trust the maxes or mins
|
||||
// until the input array is full
|
||||
inputCount++;
|
||||
if (inputCount <= nLookBack)
|
||||
{
|
||||
lastInputs[nLookBack - inputCount] = refVal;
|
||||
#if EFI_UNIT_TEST
|
||||
printf("need more data %d %d\r\n", inputCount, nLookBack);
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
return false;
|
||||
}
|
||||
|
||||
// shift array of process values and identify peaks
|
||||
inputCount = nLookBack;
|
||||
bool isMax = true;
|
||||
bool isMin = true;
|
||||
for (int i = inputCount - 1; i >= 0; i--)
|
||||
{
|
||||
double val = lastInputs[i];
|
||||
if (isMax)
|
||||
{
|
||||
isMax = (refVal >= val);
|
||||
}
|
||||
if (isMin)
|
||||
{
|
||||
isMin = (refVal <= val);
|
||||
}
|
||||
lastInputs[i + 1] = val;
|
||||
}
|
||||
lastInputs[0] = refVal;
|
||||
|
||||
// for AMIGOf tuning rule, perform an initial
|
||||
// step change to calculate process gain K_process
|
||||
// this may be very slow for lag-dominated processes
|
||||
// and may never terminate for integrating processes
|
||||
if (((byte) state & (STEADY_STATE_AT_BASELINE | STEADY_STATE_AFTER_STEP_UP)) > 0)
|
||||
{
|
||||
// check that all the recent inputs are
|
||||
// equal give or take expected noise
|
||||
double iMax = lastInputs[0];
|
||||
double iMin = lastInputs[0];
|
||||
double avgInput = 0.0;
|
||||
for (byte i = 0; i <= inputCount; i++)
|
||||
{
|
||||
double val = lastInputs[i];
|
||||
if (iMax < val)
|
||||
{
|
||||
iMax = val;
|
||||
}
|
||||
if (iMin > val)
|
||||
{
|
||||
iMin = val;
|
||||
}
|
||||
avgInput += val;
|
||||
}
|
||||
avgInput /= (double)(inputCount + 1);
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("iMax "));
|
||||
Serial.println(iMax);
|
||||
Serial.print(F("iMin "));
|
||||
Serial.println(iMin);
|
||||
Serial.print(F("avgInput "));
|
||||
Serial.println(avgInput);
|
||||
Serial.print(F("stable "));
|
||||
Serial.println((iMax - iMin) <= 2.0 * workingNoiseBand);
|
||||
#endif
|
||||
|
||||
// if recent inputs are stable
|
||||
if ((iMax - iMin) <= 2.0 * workingNoiseBand)
|
||||
{
|
||||
|
||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||
lastStepTime[0] = now;
|
||||
#endif
|
||||
|
||||
if (state == STEADY_STATE_AT_BASELINE)
|
||||
{
|
||||
state = STEADY_STATE_AFTER_STEP_UP;
|
||||
lastPeaks[0] = avgInput;
|
||||
inputCount = 0;
|
||||
#if EFI_UNIT_TEST
|
||||
printf(":( 3\r\n");
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
return false;
|
||||
}
|
||||
// else state == STEADY_STATE_AFTER_STEP_UP
|
||||
// calculate process gain
|
||||
K_process = (avgInput - lastPeaks[0]) / workingOstep;
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("Process gain "));
|
||||
Serial.println(K_process);
|
||||
#endif
|
||||
|
||||
// bad estimate of process gain
|
||||
if (K_process < 1e-10) // zero
|
||||
{
|
||||
state = AUTOTUNER_OFF;
|
||||
#if EFI_UNIT_TEST
|
||||
printf(":( 4\r\n");
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
return false;
|
||||
}
|
||||
state = RELAY_STEP_DOWN;
|
||||
|
||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||
sumInputSinceLastStep[0] = 0.0;
|
||||
#endif
|
||||
|
||||
#if EFI_UNIT_TEST
|
||||
printf(":( 5\r\n");
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
#if EFI_UNIT_TEST
|
||||
printf(":( 6\r\n");
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// increment peak count
|
||||
// and record peak time
|
||||
// for both maxima and minima
|
||||
justChanged = false;
|
||||
if (isMax)
|
||||
{
|
||||
if (peakType == MINIMUM)
|
||||
{
|
||||
justChanged = true;
|
||||
}
|
||||
peakType = MAXIMUM;
|
||||
}
|
||||
else if (isMin)
|
||||
{
|
||||
if (peakType == MAXIMUM)
|
||||
{
|
||||
justChanged = true;
|
||||
}
|
||||
peakType = MINIMUM;
|
||||
}
|
||||
|
||||
// update peak times and values
|
||||
if (justChanged)
|
||||
{
|
||||
peakCount++;
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.println(F("peakCount "));
|
||||
Serial.println(peakCount);
|
||||
Serial.println(F("peaks"));
|
||||
for (byte i = 0; i < (peakCount > 4 ? 5 : peakCount); i++)
|
||||
{
|
||||
Serial.println(lastPeaks[i]);
|
||||
}
|
||||
#endif
|
||||
|
||||
// shift peak time and peak value arrays
|
||||
for (byte i = (peakCount > 4 ? 4 : peakCount); i > 0; i--)
|
||||
{
|
||||
lastPeakTime[i] = lastPeakTime[i - 1];
|
||||
lastPeaks[i] = lastPeaks[i - 1];
|
||||
}
|
||||
}
|
||||
if (isMax || isMin)
|
||||
{
|
||||
lastPeakTime[0] = now;
|
||||
lastPeaks[0] = refVal;
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.println();
|
||||
Serial.println(F("peakCount "));
|
||||
Serial.println(peakCount);
|
||||
Serial.println(F("refVal "));
|
||||
Serial.println(refVal);
|
||||
Serial.print(F("peak type "));
|
||||
Serial.println(peakType);
|
||||
Serial.print(F("isMin "));
|
||||
Serial.println(isMin);
|
||||
Serial.print(F("isMax "));
|
||||
Serial.println(isMax);
|
||||
Serial.println();
|
||||
Serial.println(F("lastInputs:"));
|
||||
for (byte i = 0; i <= inputCount; i++)
|
||||
{
|
||||
Serial.println(lastInputs[i]);
|
||||
}
|
||||
Serial.println();
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
// check for convergence of induced oscillation
|
||||
// convergence of amplitude assessed on last 4 peaks (1.5 cycles)
|
||||
double inducedAmplitude = 0.0;
|
||||
double phaseLag;
|
||||
if (
|
||||
|
||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||
(stepCount > STEPCOUNT - 1) &&
|
||||
#endif
|
||||
|
||||
justChanged &&
|
||||
(peakCount > STEPCOUNT - 1)
|
||||
)
|
||||
{
|
||||
double absMax = lastPeaks[1];
|
||||
double absMin = lastPeaks[1];
|
||||
for (byte i = 2; i < STEPCOUNT; i++)
|
||||
{
|
||||
double val = lastPeaks[i];
|
||||
inducedAmplitude += abs( val - lastPeaks[i - 1]);
|
||||
if (absMax < val)
|
||||
{
|
||||
absMax = val;
|
||||
}
|
||||
if (absMin > val)
|
||||
{
|
||||
absMin = val;
|
||||
}
|
||||
}
|
||||
#if EFI_UNIT_TEST
|
||||
this->absMax = absMax;
|
||||
this->absMin = absMin;
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
|
||||
inducedAmplitude /= 6.0;
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("amplitude "));
|
||||
Serial.println(inducedAmplitude);
|
||||
Serial.print(F("absMin "));
|
||||
Serial.println(absMin);
|
||||
Serial.print(F("absMax "));
|
||||
Serial.println(absMax);
|
||||
Serial.print(F("convergence criterion "));
|
||||
Serial.println((0.5 * (absMax - absMin) - inducedAmplitude) / inducedAmplitude);
|
||||
#endif
|
||||
|
||||
// source for AMIGOf PI auto tuning method:
|
||||
// "Revisiting the Ziegler-Nichols tuning rules for PI control —
|
||||
// Part II. The frequency response method."
|
||||
// T. Hagglund and K. J. Astrom
|
||||
// Asian Journal of Control, Vol. 6, No. 4, pp. 469-482, December 2004
|
||||
// http://www.ajc.org.tw/pages/paper/6.4PD/AC0604-P469-FR0371.pdf
|
||||
if (controlType == AMIGOF_PI)
|
||||
{
|
||||
phaseLag = calculatePhaseLag(inducedAmplitude);
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("phase lag "));
|
||||
Serial.println(phaseLag / CONST_PI * 180.0);
|
||||
#endif
|
||||
|
||||
// check that phase lag is within acceptable bounds, ideally between 120° and 140°
|
||||
// but 115° to 145° will just about do, and might converge quicker
|
||||
if (abs(phaseLag - CONST_PI * 130.0 / 180.0) > (CONST_PI * 15.0 / 180.0))
|
||||
{
|
||||
// phase lag outside the desired range
|
||||
// set noiseBand to new estimate
|
||||
// aiming for 135° = 0.75 * pi (radians)
|
||||
// sin(135°) = sqrt(2)/2
|
||||
// NB noiseBand = 0.5 * hysteresis
|
||||
newWorkingNoiseBand = 0.5 * inducedAmplitude * CONST_SQRT2_DIV_2;
|
||||
|
||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||
// we could reset relay step counter because we can't rely
|
||||
// on constant phase lag for calculating
|
||||
// relay bias having changed noiseBand
|
||||
// but this would essentially preclude using relay bias
|
||||
// with AMIGOf tuning, which is already a compile option
|
||||
/*
|
||||
stepCount = 0;
|
||||
*/
|
||||
#endif
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("newWorkingNoiseBand "));
|
||||
Serial.println(newWorkingNoiseBand);
|
||||
#endif
|
||||
|
||||
#if EFI_UNIT_TEST
|
||||
printf(":( 7\r\n");
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// check convergence criterion for amplitude of induced oscillation
|
||||
if (((0.5 * (absMax - absMin) - inducedAmplitude) / inducedAmplitude) < AUTOTUNE_PEAK_AMPLITUDE_TOLERANCE)
|
||||
{
|
||||
state = CONVERGED;
|
||||
}
|
||||
}
|
||||
|
||||
// if the autotune has not already converged
|
||||
// terminate after 10 cycles
|
||||
// or if too long between peaks
|
||||
// or if too long between relay steps
|
||||
if (
|
||||
|
||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||
((now - lastStepTime[0]) > (unsigned long) (AUTOTUNE_MAX_WAIT_MINUTES * 60000)) ||
|
||||
#endif
|
||||
|
||||
((now - lastPeakTime[0]) > (unsigned long) (AUTOTUNE_MAX_WAIT_MINUTES * 60000)) ||
|
||||
(peakCount >= 20)
|
||||
)
|
||||
{
|
||||
state = FAILED;
|
||||
}
|
||||
|
||||
if (((byte) state & (CONVERGED | FAILED)) == 0)
|
||||
{
|
||||
#if EFI_UNIT_TEST
|
||||
printf(":( 1 state=%d\r\n", (int)state);
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
return false;
|
||||
}
|
||||
|
||||
// autotune algorithm has terminated
|
||||
// reset autotuner variables
|
||||
output = outputStart;
|
||||
|
||||
if (state == FAILED)
|
||||
{
|
||||
// do not calculate gain parameters
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.println("failed");
|
||||
#endif
|
||||
#if EFI_UNIT_TEST
|
||||
printf("failed\r\n");
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
return true;
|
||||
}
|
||||
|
||||
// finish up by calculating tuning parameters
|
||||
|
||||
// calculate ultimate gain
|
||||
double Ku = 4.0 * workingOstep / (inducedAmplitude * CONST_PI);
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("ultimate gain "));
|
||||
Serial.println(1.0 / Ku);
|
||||
Serial.println(Ku);
|
||||
#endif
|
||||
|
||||
// calculate ultimate period in seconds
|
||||
double Pu = (double) 0.5 * ((lastPeakTime[1] - lastPeakTime[3]) + (lastPeakTime[2] - lastPeakTime[4])) / 1000.0;
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("ultimate period "));
|
||||
Serial.println(Pu);
|
||||
#endif
|
||||
|
||||
// calculate gain parameters using tuning rules
|
||||
// NB PID generally outperforms PI for lag-dominated processes
|
||||
|
||||
// AMIGOf is slow to tune, especially for lag-dominated processes, because it
|
||||
// requires an estimate of the process gain which is implemented in this
|
||||
// routine by steady state change in process variable after step change in set point
|
||||
// It is intended to give robust tunings for both lag- and delay- dominated processes
|
||||
if (controlType == AMIGOF_PI)
|
||||
{
|
||||
// calculate gain ratio
|
||||
double kappa_phi = (1.0 / Ku) / K_process;
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("gain ratio kappa "));
|
||||
Serial.println(kappa_phi);
|
||||
#endif
|
||||
|
||||
// calculate phase lag
|
||||
phaseLag = calculatePhaseLag(inducedAmplitude);
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("phase lag "));
|
||||
Serial.println(phaseLag / CONST_PI * 180.0);
|
||||
#endif
|
||||
|
||||
// calculate tunings
|
||||
Kp = (( 2.50 - 0.92 * phaseLag) / (1.0 + (10.75 - 4.01 * phaseLag) * kappa_phi)) * Ku;
|
||||
Ti = ((-3.05 + 1.72 * phaseLag) / pow(1.0 + (-6.10 + 3.44 * phaseLag) * kappa_phi, 2)) * Pu;
|
||||
Td = 0.0;
|
||||
#if EFI_UNIT_TEST
|
||||
printf("Happy end 1!\r\n");
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
// converged
|
||||
return true;
|
||||
}
|
||||
|
||||
Kp = Ku / (double) tuningRule[controlType].divisor(KP_DIVISOR);
|
||||
Ti = Pu / (double) tuningRule[controlType].divisor(TI_DIVISOR);
|
||||
Td = tuningRule[controlType].PI_controller() ?
|
||||
0.0 : Pu / (double) tuningRule[controlType].divisor(TD_DIVISOR);
|
||||
#if EFI_UNIT_TEST
|
||||
printf("Happy end 2!\r\n");
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
// converged
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -71,6 +71,8 @@ struct Tuning
|
|||
}
|
||||
};
|
||||
|
||||
#define STEPCOUNT 5
|
||||
|
||||
class PID_AutoTune
|
||||
{
|
||||
|
||||
|
|
@ -151,10 +153,13 @@ public:
|
|||
double input;
|
||||
double output;
|
||||
|
||||
double absMax; // todo: remove this
|
||||
double absMin; // todo: remove this
|
||||
#if EFI_UNIT_TEST
|
||||
double absMax;
|
||||
double absMin;
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
double outputStart;
|
||||
|
||||
unsigned long sampleTime;
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -171,10 +176,9 @@ private:
|
|||
|
||||
enum AutoTunerState state; // * state of autotuner finite state machine
|
||||
unsigned long lastTime;
|
||||
unsigned long sampleTime;
|
||||
enum Peak peakType;
|
||||
unsigned long lastPeakTime[5]; // * peak time, most recent in array element 0
|
||||
double lastPeaks[5]; // * peak value, most recent in array element 0
|
||||
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
|
||||
byte inputCount;
|
||||
double workingNoiseBand;
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@ void testPidAutoZigZag() {
|
|||
mockTimeMs = 0;
|
||||
|
||||
PID_AutoTune at;
|
||||
at.sampleTime = 0; // not used in math only used to filter values out
|
||||
|
||||
at.outputStart = 50;
|
||||
|
||||
|
|
@ -44,8 +45,8 @@ void testPidAutoZigZag() {
|
|||
mockTimeMs++;
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
at.Runtime(&logging);
|
||||
assertEqualsLM("min@1", 0, at.absMin);
|
||||
assertEqualsLM("max@1", 10, at.absMax);
|
||||
// assertEqualsLM("min@1", 0, at.absMin);
|
||||
// assertEqualsLM("max@1", 10, at.absMax);
|
||||
assertEqualsM("peakCount", 0, at.peakCount);
|
||||
|
||||
for (; mockTimeMs <= 11; mockTimeMs++) {
|
||||
|
|
@ -53,28 +54,28 @@ void testPidAutoZigZag() {
|
|||
at.Runtime(&logging);
|
||||
|
||||
}
|
||||
assertEqualsLM("min@11", 0, at.absMin);
|
||||
assertEqualsLM("max@11", 100, at.absMax);
|
||||
// assertEqualsLM("min@11", 0, at.absMin);
|
||||
// assertEqualsLM("max@11", 100, at.absMax);
|
||||
assertEqualsM("peakCount", 0, at.peakCount);
|
||||
|
||||
for (; mockTimeMs <= 21; mockTimeMs++) {
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
at.Runtime(&logging);
|
||||
}
|
||||
assertEqualsM("peakCount@21", 1, at.peakCount);
|
||||
assertEqualsM("peakCount@21", 0, at.peakCount);
|
||||
|
||||
for (; mockTimeMs <= 41; mockTimeMs++) {
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
at.Runtime(&logging);
|
||||
}
|
||||
assertEqualsM("peakCount@41", 2, at.peakCount);
|
||||
assertEqualsM("peakCount@41", 0, at.peakCount);
|
||||
// assertEqualsM("Pu@41", 1, cisnan(at.Pu));
|
||||
|
||||
for (; mockTimeMs <= 60; mockTimeMs++) {
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
at.Runtime(&logging);
|
||||
}
|
||||
assertEqualsM("peakCount@60", 3, at.peakCount);
|
||||
assertEqualsM("peakCount@60", 2, at.peakCount);
|
||||
//assertEqualsM("Pu@60", 0.02, at.Pu);
|
||||
|
||||
// zigZagOffset = 10;
|
||||
|
|
@ -83,7 +84,7 @@ void testPidAutoZigZag() {
|
|||
at.input = zigZagValue(mockTimeMs);
|
||||
at.Runtime(&logging);
|
||||
}
|
||||
assertEqualsM("peakCount@80", 1, at.peakCount);
|
||||
assertEqualsM("peakCount@80", 4, at.peakCount);
|
||||
|
||||
// todo: test the same code with noisy zig-zag function
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue