From 7a0d459641ffd4b38f83698ca9b696b99b37f9fa Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Fri, 8 Sep 2023 09:10:08 +0200
Subject: [PATCH] write controller connect and disconnect to log

---
 controller_teensy/src/main.cpp | 50 +++++++++++-----------------------
 1 file changed, 16 insertions(+), 34 deletions(-)

diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp
index acad2c3..11d889b 100644
--- a/controller_teensy/src/main.cpp
+++ b/controller_teensy/src/main.cpp
@@ -490,28 +490,24 @@ void readADC() {
 }
 
 void failChecks() {
-  /*
-  if ( loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT  ) { //controller disconnected
-    if (controllerFront_connected) { //just got disconnected
-      controllerFront_connected=false;
-      writeLogComment(Serial1,loopmillis, "Controller Front feedback timeout");
-      //Serial.println("Controller Front feedback timeout");
-    }
-  }else if(!controllerFront_connected && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before
-    controllerFront_connected=true;
-    writeLogComment(Serial1,loopmillis, "Controller Front connected");
+  static bool laststate_Front_getControllerConnected;
+  if ( !escFront.getControllerConnected() && laststate_Front_getControllerConnected) { //controller got disconnected and was connected before
+    laststate_Front_getControllerConnected=false;
+    writeLogComment(loopmillis, "Controller Front feedback timeout");  
+  }else if( escFront.getControllerConnected() && !laststate_Front_getControllerConnected) { //controller was disconnected and is now connected
+    laststate_Front_getControllerConnected=true;
+    writeLogComment(loopmillis, "Controller Front connected");
+  }
+
+  static bool laststate_Rear_getControllerConnected;
+  if ( !escRear.getControllerConnected() && laststate_Rear_getControllerConnected) { //controller got disconnected and was connected before
+    laststate_Rear_getControllerConnected=false;
+    writeLogComment(loopmillis, "Controller Rear feedback timeout");  
+  }else if( escRear.getControllerConnected() && !laststate_Rear_getControllerConnected) { //controller was disconnected and is now connected
+    laststate_Rear_getControllerConnected=true;
+    writeLogComment(loopmillis, "Controller Rear connected");
   }
 
-  if ( loopmillis > motorparamsRear.millis+FEEDBACKRECEIVETIMEOUT  ) { //controller disconnected
-    if (controllerRear_connected) { //just got disconnected
-      controllerRear_connected=false;
-      writeLogComment(Serial1,loopmillis, "Controller Rear feedback timeout");
-      //Serial.println("Controller Rear feedback timeout");
-    }
-  }else if(!controllerRear_connected  && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before
-    controllerRear_connected=true;
-    writeLogComment(Serial1,loopmillis, "Controller Rear connected");
-  }*/
 
   controllers_connected=escFront.getControllerConnected() & escRear.getControllerConnected();
 
@@ -787,20 +783,6 @@ void readButtons() {
     
   }
 
-  /* TODO: if works, remove this
-  if (button_start_state) { //pressed
-    if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long
-      if (throttle_pos<=0 && brake_pos<=0 && controllers_connected && !armed) {  //brake or thottle not pressed, controllers connected
-        armed=true; //arm if button pressed long enough
-        writeLogComment(loopmillis, "Armed by button");
-      }      
-    }else if (armed){ //not pressed long enough and is armed
-      armed=false; //disarm
-      writeLogComment(loopmillis, "Disarmed by button");
-    }
-  }
-  */
-
 }