mirror of https://github.com/rusefi/rusefi.git
dead pid auto tune
This commit is contained in:
Matthew Kennedy
rusefillc
parent
45c4c9d6b3
commit
2c049d206a
|
|
@ -9,7 +9,6 @@
|
|||
#if EFI_BOOST_CONTROL
|
||||
|
||||
#include "boost_control.h"
|
||||
#include "pid_auto_tune.h"
|
||||
#include "electronic_throttle.h"
|
||||
|
||||
#define NO_PIN_PERIOD 500
|
||||
|
|
|
|||
|
|
@ -64,7 +64,6 @@
|
|||
|
||||
#include "dc_motor.h"
|
||||
#include "dc_motors.h"
|
||||
#include "pid_auto_tune.h"
|
||||
#include "defaults.h"
|
||||
|
||||
#if defined(HAS_OS_ACCESS)
|
||||
|
|
|
|||
|
|
@ -517,24 +517,6 @@ typedef enum __attribute__ ((__packed__)) {
|
|||
|
||||
} tChargeMode_e;
|
||||
|
||||
// peak type
|
||||
typedef enum {
|
||||
MINIMUM = -1,
|
||||
NOT_A_PEAK = 0,
|
||||
MAXIMUM = 1
|
||||
} PidAutoTune_Peak;
|
||||
|
||||
// auto tuner state
|
||||
typedef enum {
|
||||
AUTOTUNER_OFF = 0,
|
||||
STEADY_STATE_AT_BASELINE = 1,
|
||||
STEADY_STATE_AFTER_STEP_UP = 2,
|
||||
RELAY_STEP_UP = 4,
|
||||
RELAY_STEP_DOWN = 8,
|
||||
CONVERGED = 16,
|
||||
FAILED = 128
|
||||
} PidAutoTune_AutoTunerState;
|
||||
|
||||
typedef enum __attribute__ ((__packed__)) {
|
||||
INIT = 0,
|
||||
TPS_THRESHOLD = 1,
|
||||
|
|
|
|||
|
|
@ -1,6 +1,5 @@
|
|||
|
||||
CONTROLLERS_MATH_SRC_CPP = $(PROJECT_DIR)/controllers/math/engine_math.cpp \
|
||||
$(PROJECT_DIR)/controllers/math/pid_auto_tune.cpp \
|
||||
$(PROJECT_DIR)/controllers/math/speed_density.cpp \
|
||||
$(PROJECT_DIR)/controllers/math/closed_loop_fuel.cpp \
|
||||
$(PROJECT_DIR)/controllers/math/closed_loop_fuel_cell.cpp \
|
||||
|
|
|
|||
|
|
@ -1,855 +0,0 @@
|
|||
/*
|
||||
* pid_auto_tune.cpp
|
||||
*
|
||||
* See https://github.com/br3ttb/Arduino-PID-AutoTune-Library/blob/master/PID_AutoTune_v0/PID_AutoTune_v0.cpp
|
||||
* See https://github.com/t0mpr1c3/Arduino-PID-AutoTune-Library/blob/master/PID_AutoTune_v0/PID_AutoTune_v0.cpp
|
||||
*
|
||||
*
|
||||
* Created on: Sep 13, 2017
|
||||
*/
|
||||
|
||||
// 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 "pch.h"
|
||||
|
||||
#include "pid_auto_tune.h"
|
||||
|
||||
#if EFI_UNIT_TEST
|
||||
extern bool verboseMode;
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
|
||||
// see https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method
|
||||
// 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() {
|
||||
reset();
|
||||
}
|
||||
|
||||
void PID_AutoTune::Cancel()
|
||||
{
|
||||
state = AUTOTUNER_OFF;
|
||||
}
|
||||
|
||||
void PID_AutoTune::reset() {
|
||||
|
||||
controlType = ZIEGLER_NICHOLS_PID;
|
||||
noiseBand = 0.5;
|
||||
state = AUTOTUNER_OFF; // cannot invoke setter here since logger is not initialized yet
|
||||
oStep = 10.0;
|
||||
memset(lastPeaks, 0, sizeof(lastPeaks));
|
||||
memset(lastInputs, 0, sizeof(lastInputs));
|
||||
|
||||
logger = nullptr;
|
||||
input = output = 0;
|
||||
SetLookbackSec(10);
|
||||
|
||||
}
|
||||
|
||||
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: <20>Efficient approximations for the arctangent function<6F>, 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 p_inducedAmplitude)
|
||||
{
|
||||
// calculate phase lag
|
||||
// NB hysteresis = 2 * noiseBand;
|
||||
double ratio = 2.0 * workingNoiseBand / p_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)));
|
||||
}
|
||||
}
|
||||
|
||||
void PID_AutoTune::setState(PidAutoTune_AutoTunerState p_state) {
|
||||
this->state = p_state;
|
||||
efiPrintf("setState %s", getPidAutoTune_AutoTunerState(state));
|
||||
#if EFI_UNIT_TEST
|
||||
if (verboseMode)
|
||||
printf("setState %s\r\n", getPidAutoTune_AutoTunerState(state));
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
}
|
||||
|
||||
void PID_AutoTune::setPeakType(PidAutoTune_Peak p_peakType) {
|
||||
this->peakType = p_peakType;
|
||||
efiPrintf("setPeakType %s", getPidAutoTune_Peak(peakType));
|
||||
#if EFI_UNIT_TEST
|
||||
if (verboseMode)
|
||||
printf("peakType %s\r\n", getPidAutoTune_Peak(peakType));
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
}
|
||||
|
||||
/**
|
||||
* returns true when done, otherwise returns false
|
||||
*/
|
||||
bool PID_AutoTune::Runtime(Logging *p_logger)
|
||||
{
|
||||
|
||||
this->logger = p_logger; // a bit lazy but good enough
|
||||
// check ready for new input
|
||||
unsigned long now = getTimeNowMs();
|
||||
|
||||
if (state == AUTOTUNER_OFF)
|
||||
{
|
||||
// initialize working variables the first time around
|
||||
setPeakType(NOT_A_PEAK);
|
||||
inputCount = 0;
|
||||
peakCount = 0;
|
||||
setpoint = input;
|
||||
outputStart = output;
|
||||
lastPeakTime[0] = now;
|
||||
workingNoiseBand = noiseBand;
|
||||
newWorkingNoiseBand = noiseBand;
|
||||
workingOutputstep = 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)
|
||||
{
|
||||
setState(STEADY_STATE_AT_BASELINE);
|
||||
}
|
||||
else
|
||||
{
|
||||
efiPrintf("starting...");
|
||||
setState(RELAY_STEP_UP);
|
||||
}
|
||||
}
|
||||
|
||||
// otherwise check ready for new input
|
||||
else if ((now - lastTime) < sampleTime)
|
||||
{
|
||||
#if EFI_UNIT_TEST
|
||||
if (verboseMode)
|
||||
printf("too soon for new input %d %d %d\r\n", now, lastTime, sampleTime);
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
efiPrintf("AT skipping now=%d %d %d", now, lastTime, sampleTime);
|
||||
|
||||
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))
|
||||
{
|
||||
efiPrintf("noise crossed up %f s=%f n=%f", refVal, setpoint, workingNoiseBand);
|
||||
setState(RELAY_STEP_DOWN);
|
||||
justChanged = true;
|
||||
}
|
||||
else if ((state == RELAY_STEP_DOWN) && (refVal < setpoint - workingNoiseBand))
|
||||
{
|
||||
efiPrintf("noise crossed down %f s=%f n=%f", refVal, setpoint, workingNoiseBand);
|
||||
setState(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 /* AUTOTUNE_DEBUG */
|
||||
|
||||
#if EFI_UNIT_TEST
|
||||
if (verboseMode) {
|
||||
printf("asymmetry=%f\r\n", asymmetry);
|
||||
}
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
|
||||
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 /* AUTOTUNE_DEBUG */
|
||||
|
||||
#if EFI_UNIT_TEST
|
||||
if (verboseMode) {
|
||||
printf("deltaRelayBias=%f relayBias=%f\r\n", deltaRelayBias, relayBias);
|
||||
}
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
|
||||
// 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 /* AUTOTUNE_DEBUG */
|
||||
|
||||
#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)
|
||||
setOutput(outputStart + workingOstep + relayBias);
|
||||
#else
|
||||
efiPrintf("AT adding %f", workingOutputstep);
|
||||
setOutput(outputStart + workingOutputstep);
|
||||
#endif
|
||||
|
||||
}
|
||||
else if (state == RELAY_STEP_DOWN)
|
||||
{
|
||||
|
||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||
setOutput(outputStart - workingOstep + relayBias);
|
||||
#else
|
||||
efiPrintf("AT subtracting %f", workingOutputstep);
|
||||
setOutput(outputStart - workingOutputstep);
|
||||
#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
|
||||
|
||||
#if EFI_UNIT_TEST
|
||||
if (verboseMode) {
|
||||
printf("setpoint=%f refVal=%f\r\n", setpoint, refVal);
|
||||
}
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
|
||||
// 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;
|
||||
efiPrintf("AT need more data %d %d", inputCount, nLookBack);
|
||||
#if EFI_UNIT_TEST
|
||||
if (verboseMode) {
|
||||
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;
|
||||
efiPrintf("isMin=%d isMax=%d", isMin, isMax);
|
||||
|
||||
// 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) || EFI_UNIT_TEST
|
||||
bool stable = (iMax - iMin) <= 2.0 * workingNoiseBand;
|
||||
#endif
|
||||
#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(stable);
|
||||
#endif
|
||||
|
||||
|
||||
#if EFI_UNIT_TEST
|
||||
if (verboseMode) {
|
||||
printf("iMax=%f iMin=%f\r\n", iMax, iMin);
|
||||
printf("avgInput=%f stable=%d\r\n", avgInput, stable);
|
||||
}
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
|
||||
|
||||
// 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)
|
||||
{
|
||||
setState(STEADY_STATE_AFTER_STEP_UP);
|
||||
lastPeaks[0] = avgInput;
|
||||
inputCount = 0;
|
||||
#if EFI_UNIT_TEST
|
||||
if (verboseMode) {
|
||||
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]) / workingOutputstep;
|
||||
|
||||
#if defined (AUTOTUNE_DEBUG)
|
||||
Serial.print(F("Process gain "));
|
||||
Serial.println(K_process);
|
||||
#endif
|
||||
|
||||
#if EFI_UNIT_TEST
|
||||
if (verboseMode) {
|
||||
printf("K_process=%f\r\n", K_process);
|
||||
}
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
|
||||
// bad estimate of process gain
|
||||
if (K_process < 1e-10) // zero
|
||||
{
|
||||
setState(AUTOTUNER_OFF);
|
||||
#if EFI_UNIT_TEST
|
||||
printf(":( 4\r\n");
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
return false;
|
||||
}
|
||||
setState(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;
|
||||
}
|
||||
setPeakType(MAXIMUM);
|
||||
}
|
||||
else if (isMin)
|
||||
{
|
||||
if (peakType == MAXIMUM)
|
||||
{
|
||||
justChanged = true;
|
||||
}
|
||||
setPeakType(MINIMUM);
|
||||
}
|
||||
|
||||
// update peak times and values
|
||||
if (justChanged)
|
||||
{
|
||||
peakCount++;
|
||||
efiPrintf("peakCount=%d", 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 <20>
|
||||
// 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<32> and 140<34>
|
||||
// but 115<31> to 145<34> 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<33> = 0.75 * pi (radians)
|
||||
// sin(135<33>) = 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)
|
||||
{
|
||||
setState(CONVERGED);
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
// or if too long between relay steps
|
||||
if (
|
||||
|
||||
#if defined (AUTOTUNE_RELAY_BIAS)
|
||||
((now - lastStepTime[0]) > (unsigned long) (AUTOTUNE_MAX_WAIT_MINUTES * 60000)) ||
|
||||
#endif
|
||||
|
||||
tooLongBetween ||
|
||||
tooManyCycles
|
||||
)
|
||||
{
|
||||
#if EFI_UNIT_TEST
|
||||
printf("tooManyCycles=%d tooLongBetween=%d\r\n", tooManyCycles, tooLongBetween);
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
setState(FAILED);
|
||||
}
|
||||
|
||||
if (((byte) state & (CONVERGED | FAILED)) == 0)
|
||||
{
|
||||
#if EFI_UNIT_TEST
|
||||
if (verboseMode) {
|
||||
printf(":( 1 state=%s\r\n", getPidAutoTune_AutoTunerState(state));
|
||||
}
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
return false;
|
||||
}
|
||||
|
||||
// autotune algorithm has terminated
|
||||
// reset autotuner variables
|
||||
efiPrintf("AT done");
|
||||
setOutput( 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 * workingOutputstep / (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 AMIGOF_PI!\r\n");
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
efiPrintf("output %f", output);
|
||||
// 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!\r\n");
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
// converged
|
||||
return true;
|
||||
}
|
||||
|
||||
float PID_AutoTune::GetKp() const
|
||||
{
|
||||
return Kp;
|
||||
}
|
||||
|
||||
float PID_AutoTune::GetKi() const
|
||||
{
|
||||
return Kp / Ti;
|
||||
}
|
||||
|
||||
float PID_AutoTune::GetKd() const
|
||||
{
|
||||
return Kp * Td;
|
||||
}
|
||||
|
||||
void PID_AutoTune::setOutput(float p_output) {
|
||||
this->output = p_output;
|
||||
|
||||
efiPrintf("setOutput %f %s", output, getPidAutoTune_AutoTunerState(state));
|
||||
|
||||
#if EFI_UNIT_TEST
|
||||
if (verboseMode) {
|
||||
printf("output=%f\r\n", output);
|
||||
}
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
}
|
||||
|
||||
void PID_AutoTune::SetOutputStep(double Step)
|
||||
{
|
||||
oStep = Step;
|
||||
}
|
||||
|
||||
double PID_AutoTune::GetOutputStep() const
|
||||
{
|
||||
return oStep;
|
||||
}
|
||||
|
||||
void PID_AutoTune::SetControlType(byte type)
|
||||
{
|
||||
controlType = type;
|
||||
}
|
||||
|
||||
byte PID_AutoTune::GetControlType() const
|
||||
{
|
||||
return controlType;
|
||||
}
|
||||
|
||||
|
|
@ -1,190 +0,0 @@
|
|||
/*
|
||||
* pid_auto_tune.h
|
||||
*
|
||||
* http://brettbeauregard.com/blog/2012/01/arduino-pid-autotune-library/
|
||||
* https://www.ripublication.com/ijeer17/ijeerv9n6_02.pdf
|
||||
*
|
||||
*
|
||||
* Created on: Sep 13, 2017
|
||||
* @author Andrey Belomutskiy, (c) 2012-2020
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include "global.h"
|
||||
|
||||
// that's a weird piece of code for sure
|
||||
#ifndef byte
|
||||
typedef unsigned char byte;
|
||||
#endif
|
||||
|
||||
// irrational constants
|
||||
#define CONST_SQRT2_DIV_2 0.70710678118654752440
|
||||
|
||||
|
||||
|
||||
// verbose debug option
|
||||
#undef AUTOTUNE_DEBUG
|
||||
|
||||
|
||||
// defining this option implements relay bias
|
||||
// this is useful to adjust the relay output values
|
||||
// during the auto tuning to recover symmetric
|
||||
// oscillations
|
||||
// this can compensate for load disturbance
|
||||
// and equivalent signals arising from nonlinear
|
||||
// or non-stationary processes
|
||||
// any improvement in the tunings seems quite modest
|
||||
// but sometimes unbalanced oscillations can be
|
||||
// persuaded to converge where they might not
|
||||
// otherwise have done so
|
||||
#undef AUTOTUNE_RELAY_BIAS
|
||||
|
||||
// average amplitude of successive peaks must differ by no more than this proportion
|
||||
// relative to half the difference between maximum and minimum of last 2 cycles
|
||||
#define AUTOTUNE_PEAK_AMPLITUDE_TOLERANCE 0.05
|
||||
|
||||
// ratio of up/down relay step duration should differ by no more than this tolerance
|
||||
// biasing the relay con give more accurate estimates of the tuning parameters but
|
||||
// setting the tolerance too low will prolong the autotune procedure unnecessarily
|
||||
// this parameter also sets the minimum bias in the relay as a proportion of its amplitude
|
||||
#define AUTOTUNE_STEP_ASYMMETRY_TOLERANCE 0.20
|
||||
|
||||
// auto tune terminates if waiting too long between peaks or relay steps
|
||||
// set larger value for processes with long delays or time constants
|
||||
#define AUTOTUNE_MAX_WAIT_MINUTES 5
|
||||
|
||||
// Ziegler-Nichols type auto tune rules
|
||||
// in tabular form
|
||||
struct Tuning
|
||||
{
|
||||
byte _divisor[3];
|
||||
|
||||
bool PI_controller() const
|
||||
{
|
||||
return _divisor[2] == 0;
|
||||
}
|
||||
|
||||
double divisor(byte index) const
|
||||
{
|
||||
return (double)(_divisor[index] * 0.05);
|
||||
}
|
||||
};
|
||||
|
||||
#define STEPCOUNT 5
|
||||
|
||||
class PID_AutoTune
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
// constants ***********************************************************************************
|
||||
|
||||
// auto tune method
|
||||
enum
|
||||
{
|
||||
ZIEGLER_NICHOLS_PI = 0,
|
||||
ZIEGLER_NICHOLS_PID = 1,
|
||||
TYREUS_LUYBEN_PI,
|
||||
TYREUS_LUYBEN_PID,
|
||||
CIANCONE_MARLIN_PI,
|
||||
CIANCONE_MARLIN_PID,
|
||||
AMIGOF_PI,
|
||||
PESSEN_INTEGRAL_PID,
|
||||
SOME_OVERSHOOT_PID,
|
||||
NO_OVERSHOOT_PID
|
||||
};
|
||||
|
||||
// tuning rule divisor
|
||||
enum
|
||||
{
|
||||
KP_DIVISOR = 0,
|
||||
TI_DIVISOR = 1,
|
||||
TD_DIVISOR = 2
|
||||
};
|
||||
|
||||
|
||||
// commonly used methods ***********************************************************************
|
||||
PID_AutoTune(); // * Constructor. links the Autotune to a given PID
|
||||
bool Runtime(Logging *logging); // * Similar to the PID Compute function,
|
||||
// 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() const; //
|
||||
|
||||
void SetControlType(byte); // * Determines tuning algorithm
|
||||
byte GetControlType() const; // * Returns tuning algorithm
|
||||
|
||||
void SetLookbackSec(int); // * how far back are we looking to identify peaks
|
||||
int GetLookbackSec() const; //
|
||||
|
||||
void SetNoiseBand(double); // * the autotune will ignore signal chatter smaller
|
||||
// than this value
|
||||
double GetNoiseBand(); // this should be accurately set
|
||||
|
||||
float GetKp() const; // * once autotune is complete, these functions contain the
|
||||
float GetKi() const; // computed tuning parameters.
|
||||
float GetKd() const; //
|
||||
|
||||
Logging *logger;
|
||||
byte peakCount;
|
||||
float input;
|
||||
// suggested P coefficient while auto-tuning
|
||||
float output;
|
||||
|
||||
void setOutput(float output);
|
||||
|
||||
#if EFI_UNIT_TEST
|
||||
double absMax;
|
||||
double absMin;
|
||||
#endif /* EFI_UNIT_TEST */
|
||||
double outputStart;
|
||||
|
||||
unsigned long sampleTime;
|
||||
byte nLookBack;
|
||||
|
||||
void setState(PidAutoTune_AutoTunerState state);
|
||||
void setPeakType(PidAutoTune_Peak peakType);
|
||||
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
|
||||
|
||||
double setpoint;
|
||||
|
||||
double noiseBand;
|
||||
byte controlType; // * selects autotune algorithm
|
||||
|
||||
unsigned long lastTime;
|
||||
PidAutoTune_Peak peakType;
|
||||
unsigned long lastPeakTime[STEPCOUNT]; // * peak time, most recent in array element 0
|
||||
float lastPeaks[STEPCOUNT]; // * peak value, most recent in array element 0
|
||||
float lastInputs[101]; // * process values, most recent in array element 0
|
||||
byte inputCount;
|
||||
float workingNoiseBand;
|
||||
float workingOutputstep;
|
||||
// float inducedAmplitude;
|
||||
float Kp, Ti, Td;
|
||||
|
||||
// used by AMIGOf tuning rule
|
||||
double calculatePhaseLag(double); // * calculate phase lag from noiseBand and inducedAmplitude
|
||||
double fastArcTan(double);
|
||||
double newWorkingNoiseBand;
|
||||
double K_process;
|
||||
|
||||
#if defined AUTOTUNE_RELAY_BIAS
|
||||
double relayBias;
|
||||
unsigned long lastStepTime[5]; // * step time, most recent in array element 0
|
||||
double sumInputSinceLastStep[5]; // * integrated process values, most recent in array element 0
|
||||
byte stepCount;
|
||||
#endif
|
||||
|
||||
};
|
||||
|
|
@ -1,220 +0,0 @@
|
|||
/*
|
||||
* @file test_pid_auto.cpp
|
||||
*
|
||||
* @date Sep 14, 2017
|
||||
* @author Andrey Belomutskiy, (c) 2012-2020
|
||||
*/
|
||||
|
||||
#include "pch.h"
|
||||
#include "pid_auto_tune.h"
|
||||
|
||||
efitimems_t mockTimeMs = 0;
|
||||
|
||||
efitimems_t getTimeNowMs(void) {
|
||||
return mockTimeMs;
|
||||
}
|
||||
|
||||
LoggingWithStorage logging("test");
|
||||
|
||||
static float zigZagOffset = 0;
|
||||
|
||||
#define CYCLE 20
|
||||
|
||||
// range of oscillation
|
||||
static float oscRange;
|
||||
|
||||
/**
|
||||
* output linearly goes from 0 to 100 and back within each 'CYCLE' steps
|
||||
*/
|
||||
static float zigZagValue(int index) {
|
||||
int i = index % CYCLE;
|
||||
if ( i <= CYCLE / 2) {
|
||||
return i * (oscRange / 2 / CYCLE) + zigZagOffset;
|
||||
} else {
|
||||
return (CYCLE - i) * (oscRange / 2 / CYCLE) + zigZagOffset;
|
||||
}
|
||||
}
|
||||
|
||||
static void testPidAutoZigZagStable() {
|
||||
printf("*************************************************** testPidAutoZigZagStable\r\n");
|
||||
|
||||
oscRange = 100;
|
||||
mockTimeMs = 0;
|
||||
|
||||
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
|
||||
ASSERT_EQ( 20, at.nLookBack) << "nLookBack";
|
||||
|
||||
|
||||
at.outputStart = 50;
|
||||
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
at.Runtime(&logging);
|
||||
|
||||
mockTimeMs++;
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
at.Runtime(&logging);
|
||||
// assertEqualsLM("min@1", 0, at.absMin);
|
||||
// assertEqualsLM("max@1", 10, at.absMax);
|
||||
ASSERT_EQ( 0, at.peakCount) << "peakCount";
|
||||
int startMockMs = mockTimeMs;
|
||||
|
||||
for (; mockTimeMs <= 10 + startMockMs; mockTimeMs++) {
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
bool result = at.Runtime(&logging);
|
||||
ASSERT_FALSE(result) << "should be false#1";
|
||||
|
||||
}
|
||||
// assertEqualsLM("min@11", 0, at.absMin);
|
||||
// assertEqualsLM("max@11", 100, at.absMax);
|
||||
ASSERT_EQ( 0, at.peakCount) << "peakCount";
|
||||
|
||||
for (; mockTimeMs <= 21; mockTimeMs++) {
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
bool result = at.Runtime(&logging);
|
||||
ASSERT_FALSE(result) << "should be false#2";
|
||||
}
|
||||
ASSERT_EQ( 0, at.peakCount) << "peakCount@21";
|
||||
|
||||
for (; mockTimeMs <= 41; mockTimeMs++) {
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
bool result = at.Runtime(&logging);
|
||||
ASSERT_FALSE(result) << "should be false#2_2";
|
||||
}
|
||||
ASSERT_EQ( 2, at.peakCount) << "peakCount@41";
|
||||
// ASSERT_EQ( 1, cisnan(at.Pu)) << "Pu@41";
|
||||
|
||||
for (; mockTimeMs <= 60; mockTimeMs++) {
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
bool result = at.Runtime(&logging);
|
||||
ASSERT_FALSE(result) << "should be false#4";
|
||||
}
|
||||
ASSERT_EQ( 4, at.peakCount) << "peakCount@60";
|
||||
//assertEqualsM("Pu@60", 0.02, at.Pu);
|
||||
|
||||
// zigZagOffset = 10;
|
||||
|
||||
for (; mockTimeMs <= 69; mockTimeMs++) {
|
||||
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
bool result = at.Runtime(&logging);
|
||||
ASSERT_FALSE(result) << "should be false#4";
|
||||
}
|
||||
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
bool result = at.Runtime(&logging);
|
||||
ASSERT_EQ( 1, result) << "should be true";
|
||||
|
||||
assertEqualsM("testPidAutoZigZagStable::output", 0.0, at.output);
|
||||
|
||||
ASSERT_EQ( 5, at.peakCount) << "peakCount@80";
|
||||
assertEqualsM("ki", 27.7798, at.GetKi());
|
||||
assertEqualsM("kd", 0.0, at.GetKd());
|
||||
|
||||
// todo: test the same code with noisy zig-zag function
|
||||
}
|
||||
|
||||
static void testPidAutoZigZagGrowingOsc() {
|
||||
printf("*************************************************** testPidAutoZigZagGrowingOsc\r\n");
|
||||
|
||||
oscRange = 100;
|
||||
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<11;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);
|
||||
ASSERT_FALSE(result) << "should be false#4";
|
||||
}
|
||||
oscRange *= 1.5;
|
||||
}
|
||||
|
||||
startMockMs = mockTimeMs;
|
||||
// for (; mockTimeMs < CYCLE + startMockMs; mockTimeMs++) {
|
||||
// printf("loop2=%d\r\n", mockTimeMs);
|
||||
// at.input = zigZagValue(mockTimeMs);
|
||||
// bool result = at.Runtime(&logging);
|
||||
// ASSERT_FALSE(result) << "should be false#5";
|
||||
// }
|
||||
|
||||
at.input = zigZagValue(mockTimeMs);
|
||||
bool result = at.Runtime(&logging);
|
||||
ASSERT_TRUE(result) << "should be true#2";
|
||||
assertEqualsM("FAiled", FAILED, at.state);
|
||||
|
||||
assertEqualsM("output Growing", 0.0, at.output);
|
||||
|
||||
}
|
||||
|
||||
TEST(pidAutoTune, zeroLine) {
|
||||
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 = 0;
|
||||
bool result = at.Runtime(&logging);
|
||||
ASSERT_FALSE(result) << "should be false#4";
|
||||
}
|
||||
}
|
||||
// nothing happens in this test since we do not allow time play a role
|
||||
}
|
||||
|
||||
TEST(pidAutoTune, delayLine) {
|
||||
|
||||
static const int delayBufSize = 8;
|
||||
|
||||
// we use a small FIFO buf to imitate some "response delay" of our virtual PID-controlled "device"
|
||||
cyclic_buffer<float, delayBufSize> delayBuf;
|
||||
delayBuf.clear();
|
||||
|
||||
mockTimeMs = 0;
|
||||
|
||||
PID_AutoTune at;
|
||||
at.SetLookbackSec(5);
|
||||
at.sampleTime = 0; // not used in math only used to filter values out
|
||||
|
||||
int startMockMs;
|
||||
bool result = false;
|
||||
for (int i = 0; i < 110 && !result; i++) {
|
||||
startMockMs = mockTimeMs;
|
||||
//at.input = delayBuf.get(delayBuf.currentIndex - 1);
|
||||
int numElems = minI(delayBuf.getSize(), delayBuf.getCount());
|
||||
// our "device" is an averaging delay line
|
||||
at.input = (numElems == 0) ? 0 : (delayBuf.sum(numElems) / delayBuf.getSize());
|
||||
result = at.Runtime(&logging);
|
||||
// this is how our "device" is controlled by auto-tuner
|
||||
delayBuf.add(at.output);
|
||||
printf("[%d] %d in=%f out=%f\r\n", i, startMockMs, at.input, at.output);
|
||||
mockTimeMs++;
|
||||
}
|
||||
|
||||
if (result)
|
||||
printf("*** Converged! Got result: P=%f I=%f D=%f\r\n", at.GetKp(), at.GetKi(), at.GetKd());
|
||||
ASSERT_TRUE(result) << "should be true#5";
|
||||
}
|
||||
|
||||
TEST(pidAutoTune, testPidAuto) {
|
||||
printf("*************************************************** testPidAuto\r\n");
|
||||
|
||||
testPidAutoZigZagStable();
|
||||
|
||||
testPidAutoZigZagGrowingOsc();
|
||||
}
|
||||
|
|
@ -79,7 +79,6 @@ TESTS_SRC_CPP = \
|
|||
tests/test_log_buffer.cpp \
|
||||
tests/test_signal_executor.cpp \
|
||||
tests/test_cpp_memory_layout.cpp \
|
||||
tests/test_pid_auto.cpp \
|
||||
tests/test_pid.cpp \
|
||||
tests/test_accel_enrichment.cpp \
|
||||
tests/test_gpiochip.cpp \
|
||||
|
|
|
|||
Loading…
Reference in New Issue