From 7e986470a45f673bf3e6da96afb9d7d10e04fbaf Mon Sep 17 00:00:00 2001
From: Matthew Kennedy <matthewkennedy@outlook.com>
Date: Wed, 10 Aug 2022 21:39:21 -0700
Subject: [PATCH] LOL I overrode chThdSleepMilliseconds

---
 pt2001/src/pt2001.cpp | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/pt2001/src/pt2001.cpp b/pt2001/src/pt2001.cpp
index 71f63b8..00d86ad 100644
--- a/pt2001/src/pt2001.cpp
+++ b/pt2001/src/pt2001.cpp
@@ -19,7 +19,7 @@
 
 #include <PT2001_LoadData.h>
 
-void chThdSleepMilliseconds(int) { }
+#include <ch.h>
 
 const int MC_CK = 6; // PLL x24 / CLK_DIV 4 = 6Mhz
 
@@ -111,6 +111,8 @@ static uint16_t dacEquation(float current) {
 }
 
 void Pt2001Base::setTimings() {
+	setBoostVoltage(getBoostVoltage());
+
 	// Convert mA to DAC values
 	writeDram(MC33816Mem::Iboost, dacEquation(getBoostCurrent()));
 	writeDram(MC33816Mem::Ipeak, dacEquation(getPeakCurrent()));
@@ -416,15 +418,14 @@ bool Pt2001Base::restart() {
 
 	// Start with everything off
 	shutdown();
+	deselect();
+	chThdSleepMilliseconds(10);
 
 	if (getVbatt() < 8) {
 		// efiPrintf("GDI not Restarting until we see VBatt");
 		return false;
 	}
 
-	// Does starting turn this high to begin with??
-	deselect();
-
 	//delay/wait? .. possibly only 30us for each needed, per datasheet
 	setResetB(false);
 	chThdSleepMilliseconds(10);
@@ -474,6 +475,10 @@ bool Pt2001Base::restart() {
 
 	// Finished downloading, let's run the code
 	enableFlash();
+
+	// give it a moment to take effect
+	chThdSleepMilliseconds(10);
+
 	if (!checkFlash()) {
 		onError("MC33 no flash");
 		shutdown();
@@ -487,8 +492,7 @@ bool Pt2001Base::restart() {
 		return false;
 	}
 
-	// Drive High Voltage if possible
-	setBoostVoltage(getBoostVoltage());
+	// Drive High Voltage
 	setDriveEN(true); // driven = HV
 	chThdSleepMilliseconds(10); // Give it a moment
 	status = readDriverStatus();