From f8e37c9c92bd3ede4d4eb4f314a64919a54638a2 Mon Sep 17 00:00:00 2001
From: rusefi <arro239+rusefi@gmail.com>
Date: Wed, 13 Sep 2017 22:46:55 -0400
Subject: [PATCH] PID auto-tune

---
 firmware/controllers/math/math.mk           |  1 +
 firmware/controllers/math/pid_auto_tune.cpp | 83 ++++++++++++++++++++-
 firmware/controllers/math/pid_auto_tune.h   | 13 +++-
 3 files changed, 91 insertions(+), 6 deletions(-)

diff --git a/firmware/controllers/math/math.mk b/firmware/controllers/math/math.mk
index fe41b81d02..7a7f6a9919 100644
--- a/firmware/controllers/math/math.mk
+++ b/firmware/controllers/math/math.mk
@@ -4,4 +4,5 @@ CONTROLLERS_MATH_SRC =
 CONTROLLERS_MATH_SRC_CPP = $(PROJECT_DIR)/controllers/math/engine_math.cpp \
 	$(PROJECT_DIR)/controllers/math/pid.cpp \
 	$(PROJECT_DIR)/controllers/math/biquad.cpp \
+	$(PROJECT_DIR)/controllers/math/pid_auto_tune.cpp \
 	$(PROJECT_DIR)/controllers/math/speed_density.cpp
diff --git a/firmware/controllers/math/pid_auto_tune.cpp b/firmware/controllers/math/pid_auto_tune.cpp
index d4c16135a2..9ab39de050 100644
--- a/firmware/controllers/math/pid_auto_tune.cpp
+++ b/firmware/controllers/math/pid_auto_tune.cpp
@@ -6,6 +6,7 @@
  */
 
 #include "pid_auto_tune.h"
+#include "efilib.h"
 
 PID_AutoTune::PID_AutoTune() {
 	running = false;
@@ -14,7 +15,13 @@ PID_AutoTune::PID_AutoTune() {
 	noiseBand = 0.5;
 }
 
-int PID_AutoTune::Runtime() {
+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;
 	if (peakCount > 9 && running) {
 		running = false;
@@ -27,11 +34,12 @@ int PID_AutoTune::Runtime() {
 		//initialize working variables the first time around
 		peakType = 0;
 		peakCount = 0;
-		justchanged = false;
+
 		absMax = input;
 		absMin = input;
 		setpoint = input;
 		running = true;
+		dataPointsCount = 0;
 		outputStart = output;
 		output = outputStart + oStep;
 
@@ -45,15 +53,86 @@ int PID_AutoTune::Runtime() {
 			absMax = input;
 		if (input < absMin)
 			absMin = input;
+		dataPointsCount++;
 	}
 
 	//oscillate the output base on the input's relation to the setpoint
 
+	float prevOutput = output;
 	if (input > setpoint + noiseBand)
 		output = outputStart - oStep;
 	else if (input < setpoint - noiseBand)
 		output = outputStart + oStep;
 
+	if (output != prevOutput) {
+		scheduleMsg(logging, "direction change");
+	}
+
+
+	bool isMax = true; // is current input max value for the known input history?
+	bool isMin = true; // is current input min value for the known input history?
+	//identify peaks
+	for (int i = nLookBack - 1; i >= 0; i--) {
+		float val = lastInputs[i];
+		if (isMax)
+			isMax = input > val;
+		if (isMin)
+			isMin = input < val;
+		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
+		return 0;
+	}
+
+	if (isMax || isMin) {
+		scheduleMsg(logging, "min %d max %d %f peakType=%d", isMin, isMax, input, peakType);
+	}
+
+
+	bool directionJustChanged = false;
+
+	if (isMax) {
+		if (peakType == 0)
+			peakType = 1;
+		if (peakType == -1) {
+			peakType = 1;
+			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();
+			directionJustChanged = true;
+
+			if (peakCount < 10) {
+				peakCount++;
+				peaks[peakCount] = input;
+			} else {
+				// todo: reset peak counter maybe?
+			}
+
+		}
+
+	}
+
+	if (directionJustChanged && peakCount > 2) { //we've transitioned.  check if we can autotune based on the last peaks
+		float avgSeparation = (absF(peaks[peakCount - 1] - peaks[peakCount - 2])
+				+ absF(peaks[peakCount - 2] - peaks[peakCount - 3])) / 2;
+		if (avgSeparation < 0.05 * (absMax - absMin)) {
+			FinishUp();
+			running = false;
+			return 1;
+
+		}
+	}
+
 	return 0;
 }
 
diff --git a/firmware/controllers/math/pid_auto_tune.h b/firmware/controllers/math/pid_auto_tune.h
index 4dd4a0b544..535dd14011 100644
--- a/firmware/controllers/math/pid_auto_tune.h
+++ b/firmware/controllers/math/pid_auto_tune.h
@@ -11,13 +11,16 @@
 #ifndef CONTROLLERS_MATH_PID_AUTO_TUNE_H_
 #define CONTROLLERS_MATH_PID_AUTO_TUNE_H_
 
+#include "main.h"
+#include "rusefi_types.h"
+
 class PID_AutoTune {
 public:
 	PID_AutoTune();
 	void reset();
 	void FinishUp();
-	int Runtime();
-	bool isMax, isMin;
+	int Runtime(Logging *logging);
+//	bool isMax, isMin;
 	/**
 	 * sensor position
 	 */
@@ -33,14 +36,16 @@ public:
 	float noiseBand;
 	//int controlType = 1;
 	bool running;
-	unsigned int peak1, peak2, lastTime;
+	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;
-	bool justchanged;
+	int dataPointsCount;
+	//bool justchanged; //
 	bool justevaled;
 	float absMax, absMin;
 	float oStep;