moving to nicer implementation

firmware/controllers/math/pid_auto_tune.cpp

@@ -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) {
+//void PID_AutoTune::FinishUp() {
-	justevaled = false;
+//	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)
+		if (peakType == NOT_A_PEAK )
-			peakType = 1;
+			peakType = MAXIMUM ;
-		if (peakType == -1) {
+		if (peakType == MINIMUM ) {
-			peakType = 1;
+			peakType = MAXIMUM ;
 			directionJustChanged = true;
 		}
 
 //		peaks[peakCount] = input; we are not using max peak values
 
 	} else if (isMin) {
-		if (peakType == 0)
+		if (peakType == NOT_A_PEAK )
-			peakType = -1;
+			peakType = MINIMUM ;
-		if (peakType == 1) {
+		if (peakType == MAXIMUM ) {
-			peakType = -1;
+			peakType = MINIMUM;
-			prevPeakTime = currentPeakTime;
+			lastPeakTime[1] = lastPeakTime[0];
-			currentPeakTime = currentTimeMillis();
+			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])
+		float avgSeparation = (absF(lastPeaks[peakCount - 0] - lastPeaks[peakCount - 1])
-				+ absF(peaks[peakCount - 1] - peaks[peakCount - 2])) / 2;
+				+ absF(lastPeaks[peakCount - 1] - lastPeaks[peakCount - 2])) / 2;
 		if (avgSeparation < 0.05 * (absMax - absMin)) {
-			FinishUp();
+			//FinishUp();
 			running = false;
 			return 1;
 

firmware/controllers/math/pid_auto_tune.h

@@ -15,43 +15,190 @@
 #include "global.h"
 #include "rusefi_types.h"
 
-class PID_AutoTune {
+#define byte uint8_t
-public:
+
-	PID_AutoTune();
+  // irrational constants
-	void reset();
+#define CONST_PI           3.14159265358979323846
-	void FinishUp();
+#define CONST_SQRT2_DIV_2  0.70710678118654752440
-	int Runtime(Logging *logging);
+
-//	bool isMax, isMin;
+
-	/**
+
-	 * sensor position
+// verbose debug option
-	 */
+// requires open Serial port
-	float input;
+#undef AUTOTUNE_DEBUG
-	/**
+
-	 * actuator duty cycle
+#define USE_SIMULATION
-	 */
+
-	float output;
+// defining this option implements relay bias
-	/**
+// this is useful to adjust the relay output values
-	 * trigger line
+// during the auto tuning to recover symmetric
-	 */
+// oscillations
-	float setpoint;
+// this can compensate for load disturbance
-	float noiseBand;
+// and equivalent signals arising from nonlinear
-	//int controlType = 1;
+// or non-stationary processes
-	bool running;
+// any improvement in the tunings seems quite modest
-	efitimems_t currentPeakTime, prevPeakTime;
+// but sometimes unbalanced oscillations can be
-//	unsigned int peak1, peak2, lastTime;
+// persuaded to converge where they might not
-	//int sampleTime;
+// otherwise have done so
-	int nLookBack;
+#undef AUTOTUNE_RELAY_BIAS
-	int peakType; // todo: convert to enum
+
-	float lastInputs[101];
+// average amplitude of successive peaks must differ by no more than this proportion
-	float peaks[10];
+// relative to half the difference between maximum and minimum of last 2 cycles
-	int peakCount;
+#define AUTOTUNE_PEAK_AMPLITUDE_TOLERANCE 0.05
-	int dataPointsCount;
+
-	//bool justchanged; //
+// ratio of up/down relay step duration should differ by no more than this tolerance
-	bool justevaled;
+// biasing the relay con give more accurate estimates of the tuning parameters but
-	float absMax, absMin;
+// setting the tolerance too low will prolong the autotune procedure unnecessarily
-	float oStep;
+// this parameter also sets the minimum bias in the relay as a proportion of its amplitude
-	float outputStart;
+#define AUTOTUNE_STEP_ASYMMETRY_TOLERANCE 0.20
-	float Ku, Pu;
+
+// 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_ */