diff --git a/firmware/controllers/math/pid_auto_tune.cpp b/firmware/controllers/math/pid_auto_tune.cpp
index 06459876ef..e1d2024eef 100644
--- a/firmware/controllers/math/pid_auto_tune.cpp
+++ b/firmware/controllers/math/pid_auto_tune.cpp
@@ -13,6 +13,8 @@ PID_AutoTune::PID_AutoTune() {
 	nLookBack = 50;
 	oStep = 10;
 	noiseBand = 0.5;
+	Ku = NAN;
+	Pu = NAN;
 }
 
 void PID_AutoTune::FinishUp() {
@@ -123,8 +125,8 @@ 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 - 1] - peaks[peakCount - 2])
-				+ absF(peaks[peakCount - 2] - peaks[peakCount - 3])) / 2;
+		float avgSeparation = (absF(peaks[peakCount - 0] - peaks[peakCount - 1])
+				+ absF(peaks[peakCount - 1] - peaks[peakCount - 2])) / 2;
 		if (avgSeparation < 0.05 * (absMax - absMin)) {
 			FinishUp();
 			running = false;
diff --git a/unit_tests/test_pid_auto.cpp b/unit_tests/test_pid_auto.cpp
index 0a7c194128..44c014ea8e 100644
--- a/unit_tests/test_pid_auto.cpp
+++ b/unit_tests/test_pid_auto.cpp
@@ -56,13 +56,20 @@ void testPidAutoZigZag() {
 		at.input = zigZagValue(mockTimeMs);
 		at.Runtime(&logging);
 	}
-	assertEqualsM("peakCount", 1, at.peakCount);
+	assertEqualsM("peakCount@21", 1, at.peakCount);
 
 	for (;mockTimeMs<=41;mockTimeMs++) {
 		at.input = zigZagValue(mockTimeMs);
 		at.Runtime(&logging);
 	}
-	assertEqualsM("peakCount", 2, at.peakCount);
+	assertEqualsM("peakCount@41", 2, at.peakCount);
+	assertEqualsM("Pu@41", 1, cisnan(at.Pu));
 
+	for (;mockTimeMs<=60;mockTimeMs++) {
+		at.input = zigZagValue(mockTimeMs);
+		at.Runtime(&logging);
+	}
+	assertEqualsM("peakCount@60", 3, at.peakCount);
+	assertEqualsM("Pu@60", 0.02, at.Pu);
 
 }