mirror of https://github.com/rusefi/rusefi-1.git
moving to nicer implementation
This commit is contained in:
rusefi
parent
f4e0f375fb
commit
02c0508b20
|
|
@ -1,6 +1,10 @@
|
|||
/*
|
||||
* 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
|
||||
* @author Andrey Belomutskiy, (c) 2012-2018
|
||||
*/
|
||||
|
|
@ -13,35 +17,33 @@ PID_AutoTune::PID_AutoTune() {
|
|||
nLookBack = 50;
|
||||
oStep = 10;
|
||||
noiseBand = 0.5;
|
||||
Ku = NAN;
|
||||
Pu = NAN;
|
||||
}
|
||||
|
||||
void PID_AutoTune::FinishUp() {
|
||||
Ku = 4 * (2 * oStep) / ((absMax - absMin) * 3.14159);
|
||||
Pu = (float) (currentPeakTime - prevPeakTime) / 1000.0; // converting ms to seconds
|
||||
|
||||
}
|
||||
|
||||
int PID_AutoTune::Runtime(Logging *logging) {
|
||||
justevaled = false;
|
||||
//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();
|
||||
// FinishUp();
|
||||
return 1;
|
||||
}
|
||||
|
||||
justevaled = true;
|
||||
if (!running) {
|
||||
//initialize working variables the first time around
|
||||
peakType = 0;
|
||||
peakType = NOT_A_PEAK;
|
||||
peakCount = 0;
|
||||
|
||||
absMax = input;
|
||||
absMin = input;
|
||||
setpoint = input;
|
||||
running = true;
|
||||
dataPointsCount = 0;
|
||||
inputCount = 0;
|
||||
outputStart = output;
|
||||
output = outputStart + oStep;
|
||||
|
||||
|
|
@ -55,7 +57,7 @@ int PID_AutoTune::Runtime(Logging *logging) {
|
|||
absMax = input;
|
||||
if (input < absMin)
|
||||
absMin = input;
|
||||
dataPointsCount++;
|
||||
inputCount ++;
|
||||
}
|
||||
|
||||
//oscillate the output base on the input's relation to the setpoint
|
||||
|
|
@ -83,7 +85,7 @@ int PID_AutoTune::Runtime(Logging *logging) {
|
|||
lastInputs[i + 1] = lastInputs[i];
|
||||
}
|
||||
lastInputs[0] = input;
|
||||
if (dataPointsCount < 9) { //we don't want to trust the maxes or mins until the inputs array has been filled
|
||||
if (inputCount < 9) { //we don't want to trust the maxes or mins until the inputs array has been filled
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -95,27 +97,27 @@ int PID_AutoTune::Runtime(Logging *logging) {
|
|||
bool directionJustChanged = false;
|
||||
|
||||
if (isMax) {
|
||||
if (peakType == 0)
|
||||
peakType = 1;
|
||||
if (peakType == -1) {
|
||||
peakType = 1;
|
||||
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 == 0)
|
||||
peakType = -1;
|
||||
if (peakType == 1) {
|
||||
peakType = -1;
|
||||
prevPeakTime = currentPeakTime;
|
||||
currentPeakTime = currentTimeMillis();
|
||||
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++;
|
||||
peaks[peakCount] = input;
|
||||
lastPeaks[peakCount] = input;
|
||||
} else {
|
||||
// todo: reset peak counter maybe?
|
||||
}
|
||||
|
|
@ -125,10 +127,10 @@ int PID_AutoTune::Runtime(Logging *logging) {
|
|||
}
|
||||
|
||||
if (directionJustChanged && peakCount > 2) { //we've transitioned. check if we can autotune based on the last peaks
|
||||
float avgSeparation = (absF(peaks[peakCount - 0] - peaks[peakCount - 1])
|
||||
+ absF(peaks[peakCount - 1] - peaks[peakCount - 2])) / 2;
|
||||
float avgSeparation = (absF(lastPeaks[peakCount - 0] - lastPeaks[peakCount - 1])
|
||||
+ absF(lastPeaks[peakCount - 1] - lastPeaks[peakCount - 2])) / 2;
|
||||
if (avgSeparation < 0.05 * (absMax - absMin)) {
|
||||
FinishUp();
|
||||
//FinishUp();
|
||||
running = false;
|
||||
return 1;
|
||||
|
||||
|
|
|
|||
|
|
@ -15,43 +15,190 @@
|
|||
#include "global.h"
|
||||
#include "rusefi_types.h"
|
||||
|
||||
class PID_AutoTune {
|
||||
public:
|
||||
PID_AutoTune();
|
||||
void reset();
|
||||
void FinishUp();
|
||||
int Runtime(Logging *logging);
|
||||
// bool isMax, isMin;
|
||||
/**
|
||||
* sensor position
|
||||
*/
|
||||
float input;
|
||||
/**
|
||||
* actuator duty cycle
|
||||
*/
|
||||
float output;
|
||||
/**
|
||||
* trigger line
|
||||
*/
|
||||
float setpoint;
|
||||
float noiseBand;
|
||||
//int controlType = 1;
|
||||
bool running;
|
||||
efitimems_t currentPeakTime, prevPeakTime;
|
||||
// unsigned int peak1, peak2, lastTime;
|
||||
//int sampleTime;
|
||||
int nLookBack;
|
||||
int peakType; // todo: convert to enum
|
||||
float lastInputs[101];
|
||||
float peaks[10];
|
||||
int peakCount;
|
||||
int dataPointsCount;
|
||||
//bool justchanged; //
|
||||
bool justevaled;
|
||||
float absMax, absMin;
|
||||
float oStep;
|
||||
float outputStart;
|
||||
float Ku, Pu;
|
||||
#define byte uint8_t
|
||||
|
||||
// irrational constants
|
||||
#define CONST_PI 3.14159265358979323846
|
||||
#define CONST_SQRT2_DIV_2 0.70710678118654752440
|
||||
|
||||
|
||||
|
||||
// verbose debug option
|
||||
// requires open Serial port
|
||||
#undef AUTOTUNE_DEBUG
|
||||
|
||||
#define USE_SIMULATION
|
||||
|
||||
// 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()
|
||||
{
|
||||
return _divisor[2] == 0;
|
||||
}
|
||||
|
||||
double divisor(byte index)
|
||||
{
|
||||
return (double)(_divisor[index] * 0.05);
|
||||
}
|
||||
};
|
||||
|
||||
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
|
||||
};
|
||||
|
||||
// peak type
|
||||
enum Peak
|
||||
{
|
||||
MINIMUM = -1,
|
||||
NOT_A_PEAK = 0,
|
||||
MAXIMUM = 1
|
||||
};
|
||||
|
||||
// auto tuner state
|
||||
enum AutoTunerState
|
||||
{
|
||||
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
|
||||
};
|
||||
|
||||
// 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 SetOutputStep(double); // * how far above and below the starting value will
|
||||
// the output step?
|
||||
double GetOutputStep(); //
|
||||
|
||||
void SetControlType(byte); // * Determines tuning algorithm
|
||||
byte GetControlType(); // * Returns tuning algorithm
|
||||
|
||||
void SetLookbackSec(int); // * how far back are we looking to identify peaks
|
||||
int GetLookbackSec(); //
|
||||
|
||||
void SetNoiseBand(double); // * the autotune will ignore signal chatter smaller
|
||||
// than this value
|
||||
double GetNoiseBand(); // this should be accurately set
|
||||
|
||||
double GetKp(); // * once autotune is complete, these functions contain the
|
||||
double GetKi(); // computed tuning parameters.
|
||||
double GetKd(); //
|
||||
|
||||
double oStep;
|
||||
byte peakCount;
|
||||
double input;
|
||||
double output;
|
||||
|
||||
double absMax; // todo: remove this
|
||||
double absMin; // todo: remove this
|
||||
double outputStart;
|
||||
|
||||
|
||||
private:
|
||||
|
||||
double processValueOffset(double, // * returns an estimate of the process value offset
|
||||
double); // as a proportion of the amplitude
|
||||
|
||||
double setpoint;
|
||||
|
||||
bool running; // todo: remove this
|
||||
|
||||
double noiseBand;
|
||||
byte nLookBack;
|
||||
byte controlType; // * selects autotune algorithm
|
||||
|
||||
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
|
||||
double lastInputs[101]; // * process values, most recent in array element 0
|
||||
byte inputCount;
|
||||
double workingNoiseBand;
|
||||
double workingOstep;
|
||||
double inducedAmplitude;
|
||||
double 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
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif /* CONTROLLERS_MATH_PID_AUTO_TUNE_H_ */
|
||||
|
|
|
|||
Loading…
Reference in New Issue