PID auto tune unit test

firmware/controllers/math/pid_auto_tune.cpp

@@ -108,6 +108,9 @@ void PID_AutoTune::setPeakType(PidAutoTune_Peak peakType) {
 #endif /* EFI_UNIT_TEST */
 }
 
+/**
+ * returns true when done, otherwise returns false
+ */
 bool PID_AutoTune::Runtime(Logging *logging)
 {
   // check ready for new input
@@ -656,7 +659,7 @@ bool PID_AutoTune::Runtime(Logging *logging)
   if (((byte) state & (CONVERGED | FAILED)) == 0)
   {
 #if EFI_UNIT_TEST
-		printf(":( 1 state=%d\r\n", (int)state);
+		printf(":( 1 state=%s\r\n", getPidAutoTune_AutoTunerState(state));
 #endif /* EFI_UNIT_TEST */
 	return false;
   }
@@ -744,17 +747,17 @@ bool PID_AutoTune::Runtime(Logging *logging)
   return true;
 }
 
-double PID_AutoTune::GetKp()
+float PID_AutoTune::GetKp()
 {
   return Kp;
 }
 
-double PID_AutoTune::GetKi()
+float PID_AutoTune::GetKi()
 {
   return Kp / Ti;
 }
 
-double PID_AutoTune::GetKd()
+float PID_AutoTune::GetKd()
 {
   return Kp * Td;
 }

firmware/controllers/math/pid_auto_tune.h

@@ -124,14 +124,14 @@ public:
                                         //   than this value
   double GetNoiseBand();                //   this should be accurately set
 
-  double GetKp();                       // * once autotune is complete, these functions contain the
+  float GetKp();                       // * once autotune is complete, these functions contain the
-  double GetKi();                       //   computed tuning parameters.
+  float GetKi();                       //   computed tuning parameters.
-  double GetKd();                       //
+  float GetKd();                       //
 
   double oStep;
   byte peakCount;
-  double input;
+  float input;
-  double output;
+  float output;
 
 #if EFI_UNIT_TEST
   double absMax;
@@ -144,6 +144,7 @@ public:
 
   void setState(PidAutoTune_AutoTunerState state);
   void setPeakType(PidAutoTune_Peak peakType);
+  PidAutoTune_AutoTunerState state;            // * state of autotuner finite state machine
 
 private:
 
@@ -157,17 +158,16 @@ private:
   double noiseBand;
   byte controlType;                     // * selects autotune algorithm
 
-  PidAutoTune_AutoTunerState state;            // * state of autotuner finite state machine
   unsigned long lastTime;
   PidAutoTune_Peak peakType;
   unsigned long lastPeakTime[STEPCOUNT];        // * peak time, most recent in array element 0
-  double lastPeaks[STEPCOUNT];                  // * peak value, most recent in array element 0
+  float lastPeaks[STEPCOUNT];                  // * peak value, most recent in array element 0
-  double lastInputs[101];               // * process values, most recent in array element 0
+  float lastInputs[101];               // * process values, most recent in array element 0
   byte inputCount;
-  double workingNoiseBand;
+  float workingNoiseBand;
-  double workingOstep;
+  float workingOstep;
-  double inducedAmplitude;
+  float inducedAmplitude;
-  double Kp, Ti, Td;
+  float Kp, Ti, Td;
 
   // used by AMIGOf tuning rule
   double calculatePhaseLag(double);     // * calculate phase lag from noiseBand and inducedAmplitude

unit_tests/test_pid_auto.cpp

@@ -22,21 +22,25 @@ 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 * (100.0 / 2 / CYCLE) + zigZagOffset;
+		return i * (oscRange / 2 / CYCLE) + zigZagOffset;
 	} else {
-		return (CYCLE - i) * (100.0 / 2 / CYCLE) + zigZagOffset;
+		return (CYCLE - i) * (oscRange / 2 / CYCLE) + zigZagOffset;
 	}
 }
 
-void testPidAutoZigZag() {
+static void testPidAutoZigZagStable() {
-	printf("*************************************************** testPidAutoZigZag\r\n");
+	printf("*************************************************** testPidAutoZigZagStable\r\n");
 
+	oscRange = 100;
 	mockTimeMs = 0;
 
 	PID_AutoTune at;
@@ -60,7 +64,8 @@ void testPidAutoZigZag() {
 
 	for (; mockTimeMs <= 10 + startMockMs; mockTimeMs++) {
 		at.input = zigZagValue(mockTimeMs);
-		at.Runtime(&logging);
+		bool result = at.Runtime(&logging);
+		assertFalseM("should be false#1", result);
 
 	}
 //	assertEqualsLM("min@11", 0, at.absMin);
@@ -69,33 +74,93 @@ void testPidAutoZigZag() {
 
 	for (; mockTimeMs <= 21; mockTimeMs++) {
 		at.input = zigZagValue(mockTimeMs);
-		at.Runtime(&logging);
+		bool result = at.Runtime(&logging);
+		assertFalseM("should be false#2", result);
 	}
 	assertEqualsM("peakCount@21", 0, at.peakCount);
 
 	for (; mockTimeMs <= 41; mockTimeMs++) {
 		at.input = zigZagValue(mockTimeMs);
-		at.Runtime(&logging);
+		bool result = at.Runtime(&logging);
+		assertFalseM("should be false#2_2", result);
 	}
 	assertEqualsM("peakCount@41", 2, at.peakCount);
 //	assertEqualsM("Pu@41", 1, cisnan(at.Pu));
 
 	for (; mockTimeMs <= 60; mockTimeMs++) {
 		at.input = zigZagValue(mockTimeMs);
-		at.Runtime(&logging);
+		bool result = at.Runtime(&logging);
+		assertFalseM("should be false#4", result);
 	}
 	assertEqualsM("peakCount@60", 4, at.peakCount);
 	//assertEqualsM("Pu@60", 0.02, at.Pu);
 
 //	zigZagOffset = 10;
 
-	for (; mockTimeMs <= 80; mockTimeMs++) {
+	for (; mockTimeMs <= 69; mockTimeMs++) {
+
 		at.input = zigZagValue(mockTimeMs);
-		at.Runtime(&logging);
+		bool result = at.Runtime(&logging);
+		assertFalseM("should be false#4", result);
 	}
-	assertEqualsM("peakCount@80", 6, at.peakCount);
+
+	at.input = zigZagValue(mockTimeMs);
+	bool result = at.Runtime(&logging);
+	assertEqualsM("should be true", 1, result);
+
+	assertEqualsM("output", 0.0, at.output);
+
+	assertEqualsM("peakCount@80", 5, at.peakCount);
 	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);
+			assertFalseM("should be false#4", result);
+		}
+		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);
+//		assertFalseM("should be false#5", result);
+//	}
+
+	at.input = zigZagValue(mockTimeMs);
+	bool result = at.Runtime(&logging);
+	assertTrueM("should be true#2", result);
+	assertEqualsM("FAiled", FAILED, at.state);
+
+	assertEqualsM("output Growing", 0.0, at.output);
+
+}
+
+void testPidAutoZigZag() {
+	printf("*************************************************** testPidAutoZigZag\r\n");
+
+	testPidAutoZigZagStable();
+
+	testPidAutoZigZagGrowingOsc();
+}