From e0a5c3dc1286f373451579c8bf934e2b32088124 Mon Sep 17 00:00:00 2001
From: rusefi <rusefillc@gmail.com>
Date: Mon, 19 Oct 2020 22:04:06 -0400
Subject: [PATCH] GDI Epic #1448

---
 firmware/controllers/algo/engine.cpp |  7 +++++
 firmware/hw_layer/mc33816.cpp        | 47 +++++++++++++++++++++-------
 firmware/hw_layer/mc33816.h          |  2 ++
 3 files changed, 44 insertions(+), 12 deletions(-)

diff --git a/firmware/controllers/algo/engine.cpp b/firmware/controllers/algo/engine.cpp
index a9df75bbc1..0b7b3bf326 100644
--- a/firmware/controllers/algo/engine.cpp
+++ b/firmware/controllers/algo/engine.cpp
@@ -29,6 +29,9 @@
 #include "sensor.h"
 #include "gppwm.h"
 #include "tachometer.h"
+#if EFI_MC33816
+ #include "mc33816.h"
+#endif // EFI_MC33816
 
 #if EFI_TUNER_STUDIO
 #include "tunerstudio_outputs.h"
@@ -257,6 +260,10 @@ void Engine::updateSlowSensors(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	vBattForTle8888 = sensors.vBatt;
 #endif /* BOARD_TLE8888_COUNT */
 
+#if EFI_MC33816
+	initMc33816IfNeeded();
+#endif // EFI_MC33816
+
 	engineState.running.injectorLag = getInjectorLag(sensors.vBatt PASS_ENGINE_PARAMETER_SUFFIX);
 #endif
 }
diff --git a/firmware/hw_layer/mc33816.cpp b/firmware/hw_layer/mc33816.cpp
index a5c5ac8a16..9d7d40f921 100644
--- a/firmware/hw_layer/mc33816.cpp
+++ b/firmware/hw_layer/mc33816.cpp
@@ -20,13 +20,14 @@
 
 #include "mc33816.h"
 #include "mc33816_memory_map.h"
-#include "engine_configuration.h"
+#include "engine.h"
 #include "efi_gpio.h"
 #include "hardware.h"
 #include "mc33816_data.h"
 #include "mpu_util.h"
+#include "voltage.h"
 
-EXTERN_CONFIG;
+EXTERN_ENGINE;
 
 
 static OutputPin chipSelect;
@@ -427,18 +428,20 @@ static void download_register(int r_target) {
 	   spiUnselect(driver);
 }
 
+static bool haveMc33816 = false;
+
 void initMc33816(Logging *sharedLogger) {
 	logger = sharedLogger;
 
 	//
-	// see setTest33816EngineConfiguration  for default configuration
+	// see setTest33816EngineConfiguration for default configuration
 	// Pins
-	if (CONFIG(mc33816_cs) == GPIO_UNASSIGNED)
-		return;
-	if (CONFIG(mc33816_rstb) == GPIO_UNASSIGNED)
-		return;
-	if (CONFIG(mc33816_driven) == GPIO_UNASSIGNED)
+	if (CONFIG(mc33816_cs) == GPIO_UNASSIGNED ||
+			CONFIG(mc33816_rstb) == GPIO_UNASSIGNED ||
+			CONFIG(mc33816_driven) == GPIO_UNASSIGNED
+			) {
 		return;
+	}
 	if (CONFIG(mc33816_flag0) != GPIO_UNASSIGNED) {
 		efiSetPadMode("mc33816 flag0", CONFIG(mc33816_flag0), getInputMode(PI_DEFAULT));
 	}
@@ -469,6 +472,7 @@ void initMc33816(Logging *sharedLogger) {
 	addConsoleAction("mc33_restart", mcRestart);
 	//addConsoleActionI("mc33_send", sendWord);
 
+	haveMc33816 = true;
 	mcRestart();
 }
 
@@ -486,6 +490,11 @@ static void mcRestart() {
 
 	driven.setValue(0); // ensure driven is off
 
+	if (engine->sensors.vBatt < LOW_VBATT) {
+		scheduleMsg(logger, "GDI not Restarting until we see VBatt");
+		return;
+	}
+
 	// Does starting turn this high to begin with??
 	spiUnselect(driver);
 
@@ -503,7 +512,7 @@ static void mcRestart() {
 
 	mcClearDriverStatus(); // Initial clear necessary
     mcDriverStatus = readDriverStatus();
-    if(checkUndervoltV5(mcDriverStatus)){
+    if (checkUndervoltV5(mcDriverStatus)) {
     	firmwareError(OBD_PCM_Processor_Fault, "MC33 5V Under-Voltage!");
     	mcShutdown();
     	return;
@@ -541,15 +550,14 @@ static void mcRestart() {
 
     // Finished downloading, let's run the code
     enable_flash();
-    if(!check_flash())
-    {
+    if (!check_flash()) {
     	firmwareError(OBD_PCM_Processor_Fault, "MC33 no flash");
     	mcShutdown();
     	return;
     }
 
     mcDriverStatus = readDriverStatus();
-    if(checkUndervoltVccP(mcDriverStatus)){
+    if (checkUndervoltVccP(mcDriverStatus)) {
     	firmwareError(OBD_PCM_Processor_Fault, "MC33 VccP (7V) Under-Voltage!");
     	mcShutdown();
     	return;
@@ -574,4 +582,19 @@ static void mcRestart() {
     }
 }
 
+void initMc33816IfNeeded() {
+	if (!haveMc33816) {
+		return;
+	}
+	static bool isInitializaed = false;
+	if (engine->sensors.vBatt < LOW_VBATT) {
+		isInitializaed = false;
+	} else {
+		if (!isInitializaed) {
+			mcRestart();
+			isInitializaed = true;
+		}
+	}
+}
+
 #endif /* EFI_MC33816 */
diff --git a/firmware/hw_layer/mc33816.h b/firmware/hw_layer/mc33816.h
index f1352ffd8f..8ebf17af53 100644
--- a/firmware/hw_layer/mc33816.h
+++ b/firmware/hw_layer/mc33816.h
@@ -27,3 +27,5 @@ enum {
 };
 
 void initMc33816(Logging *logger);
+void initMc33816IfNeeded();
+