mirror of https://github.com/rusefi/rusefi.git
dead pid auto tune
This commit is contained in:
parent
45c4c9d6b3
commit
2c049d206a
|
@ -9,7 +9,6 @@
|
||||||
#if EFI_BOOST_CONTROL
|
#if EFI_BOOST_CONTROL
|
||||||
|
|
||||||
#include "boost_control.h"
|
#include "boost_control.h"
|
||||||
#include "pid_auto_tune.h"
|
|
||||||
#include "electronic_throttle.h"
|
#include "electronic_throttle.h"
|
||||||
|
|
||||||
#define NO_PIN_PERIOD 500
|
#define NO_PIN_PERIOD 500
|
||||||
|
|
|
@ -64,7 +64,6 @@
|
||||||
|
|
||||||
#include "dc_motor.h"
|
#include "dc_motor.h"
|
||||||
#include "dc_motors.h"
|
#include "dc_motors.h"
|
||||||
#include "pid_auto_tune.h"
|
|
||||||
#include "defaults.h"
|
#include "defaults.h"
|
||||||
|
|
||||||
#if defined(HAS_OS_ACCESS)
|
#if defined(HAS_OS_ACCESS)
|
||||||
|
|
|
@ -517,24 +517,6 @@ typedef enum __attribute__ ((__packed__)) {
|
||||||
|
|
||||||
} tChargeMode_e;
|
} 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__)) {
|
typedef enum __attribute__ ((__packed__)) {
|
||||||
INIT = 0,
|
INIT = 0,
|
||||||
TPS_THRESHOLD = 1,
|
TPS_THRESHOLD = 1,
|
||||||
|
|
|
@ -1,6 +1,5 @@
|
||||||
|
|
||||||
CONTROLLERS_MATH_SRC_CPP = $(PROJECT_DIR)/controllers/math/engine_math.cpp \
|
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/speed_density.cpp \
|
||||||
$(PROJECT_DIR)/controllers/math/closed_loop_fuel.cpp \
|
$(PROJECT_DIR)/controllers/math/closed_loop_fuel.cpp \
|
||||||
$(PROJECT_DIR)/controllers/math/closed_loop_fuel_cell.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_log_buffer.cpp \
|
||||||
tests/test_signal_executor.cpp \
|
tests/test_signal_executor.cpp \
|
||||||
tests/test_cpp_memory_layout.cpp \
|
tests/test_cpp_memory_layout.cpp \
|
||||||
tests/test_pid_auto.cpp \
|
|
||||||
tests/test_pid.cpp \
|
tests/test_pid.cpp \
|
||||||
tests/test_accel_enrichment.cpp \
|
tests/test_accel_enrichment.cpp \
|
||||||
tests/test_gpiochip.cpp \
|
tests/test_gpiochip.cpp \
|
||||||
|
|
Loading…
Reference in New Issue