board power checks unreliable and too complicated. backup commit

controller/controller.ino

@@ -76,6 +76,8 @@ long last_send = 0;
 
 boolean board1Enabled=false;
 boolean board2Enabled=false;
+#define EXPECT_FEEDBACK_ABOVE_SPEED 250 //for error checking
+#define SERIALACTIVECHECKTIME 3000 //time to say a board is online/active when last message received in the last x ms
 
 // Global variables for serial communication
 uint8_t idx1 = 0;                        // Index for new data pointer
@@ -84,6 +86,8 @@ byte *p1;                                // Pointer declaration for the new rece
 byte incomingByte1;
 byte incomingBytePrev1;
 long lastValidDataSerial1_time;
+long lastSendSerial1expectFeedback; //to check time difference between last >0 speed send and last received feedback
+long lastBoard1PowerChange;
 
 //Same for Serial2
 uint8_t idx2 = 0;                        // Index for new data pointer
@@ -92,7 +96,8 @@ byte *p2;                                // Pointer declaration for the new rece
 byte incomingByte2;
 byte incomingBytePrev2;
 long lastValidDataSerial2_time;
-
+long lastSendSerial2expectFeedback; //to check time difference between last >0 speed send and last received feedback
+long lastBoard2PowerChange;
 
 typedef struct{
    uint16_t start;
@@ -140,8 +145,8 @@ void setup()
 
   Serial.begin(115200); //Debug and Program. A9=TX1,  A10=RX1  (3v3 level)
 
-  Serial1.begin(SERIAL_CONTROL_BAUD); //control.  A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector
+  Serial1.begin(SERIAL_CONTROL_BAUD); //control.  A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector (Rear)
-  Serial2.begin(SERIAL_CONTROL_BAUD); //control.  B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector
+  Serial2.begin(SERIAL_CONTROL_BAUD); //control.  B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector (Front)
 
   // Pin Setup
   pinMode(PIN_STARTLED, OUTPUT);
@@ -203,6 +208,19 @@ void loop() {
       Serial.print("lastData1="); Serial.print(loopmillis-lastValidDataSerial1_time); Serial.print(", lastData2=");Serial.print(loopmillis-lastValidDataSerial2_time); Serial.print(", speedFL="); Serial.println(out_speedFL);
     }
   }
+
+  //for lastValidSerialData calculation
+  if (abs(out_speedRL)>EXPECT_FEEDBACK_ABOVE_SPEED || abs(out_speedRR)>EXPECT_FEEDBACK_ABOVE_SPEED) { //Rear is serial 1
+    lastSendSerial1expectFeedback=loopmillis;
+  }
+  if (abs(out_speedFL)>EXPECT_FEEDBACK_ABOVE_SPEED || abs(out_speedFR)>EXPECT_FEEDBACK_ABOVE_SPEED) { //Front is serial 2
+    lastSendSerial2expectFeedback=loopmillis;
+  }
+  
+
+  if (currentmode!=error) { //keep last errormessage
+    failChecks();
+  }
 }
 
 
@@ -346,31 +364,24 @@ void handleModeChange() {
               case 4: 
                 // ### Request On ###
                 if (requestmode==on) {//wait for both boards to send feedback
-                  if ( serial1Active() && serial2Active() ) { //got recent feedback from both boards
+                  state_modechange++;
-                    state_modechange++;
+                  board1Enabled=true; //assume board is online
-                    boardsPowered(); //check boards powered to set variable board1/2Enabled to true
+                  board2Enabled=true; //assume board is online
-                  }
+                  lastBoard1PowerChange=loopmillis;
-                  if (loopmillis - state_modechange_time > 5000) { //timeout
+                  lastBoard2PowerChange=loopmillis;
-                    currentmode=error; //error
+                  lastSendSerial1expectFeedback=loopmillis; //expect maybe feedback on power on
-                    requestmode=currentmode;
+                  lastSendSerial2expectFeedback=loopmillis;
-                    errormessage="No feedback from board(s) on startup";
+                  lastValidDataSerial1_time=loopmillis; //assume a feedback was received (boards do not send feedback on poweron)
-                    state_modechange=0;
+                  lastValidDataSerial2_time=loopmillis; //assume a feedback was received (boards do not send feedback on poweron)
-                    board1Enabled=false;
-                    board2Enabled=false;
-                  }
                 // ### Request Idle or Off (both power boards off) ###
-                }else if(requestmode==idle || requestmode==off) { //wait for no response
+                }else if(requestmode==idle || requestmode==off) {
-                  if ( !serial1Active() && !serial2Active() ) { //no new data since some time
+                  state_modechange++;
-                    state_modechange++;
+                  board1Enabled=false; //assume board is offline
-                    board1Enabled=false;
+                  board2Enabled=false; //assume board is offline
-                    board2Enabled=false;
+                  lastBoard1PowerChange=loopmillis;
-                  }
+                  lastBoard2PowerChange=loopmillis;
-                  if (loopmillis - state_modechange_time > 5000) { //timeout
+                  lastSendSerial1expectFeedback=loopmillis; //expect maybe feedback on power on
-                    currentmode=error; //error
+                  lastSendSerial2expectFeedback=loopmillis;
-                    requestmode=currentmode;
-                    errormessage="Boards did not turn off";
-                    state_modechange=0;
-                  }
                 }else{ //if changed off from error mode
                   state_modechange++;
                 }
@@ -399,11 +410,30 @@ void handleModeChange() {
   }
 }
 
+
 boolean serial1Active() {
-  return loopmillis-lastValidDataSerial1_time < 2000;
+  if (loopmillis-lastBoard1PowerChange<=SERIALACTIVECHECKTIME) {
+    return board1Enabled; //unclear if mcu just started or board is just starting up. reply with exprected answer
+  }
+  if (lastValidDataSerial1_time==0 && lastSendSerial1expectFeedback==0) { //no valid data received and send ever
+    return false;
+  }
+  if (loopmillis-lastSendSerial1expectFeedback <= SERIALACTIVECHECKTIME){ //enough time passed since last exprected feedback
+    return board1Enabled; //unclear, return exprected value
+  }
+  return loopmillis-lastValidDataSerial1_time < SERIALACTIVECHECKTIME;
 }
 boolean serial2Active() {
-  return loopmillis-lastValidDataSerial2_time < 2000;
+  if (loopmillis-lastBoard2PowerChange<=SERIALACTIVECHECKTIME) {
+    return board2Enabled; //unclear if mcu just started or board is just starting up. reply with exprected answer
+  }
+  if (lastValidDataSerial2_time==0 && lastSendSerial2expectFeedback==0) { //no valid data received and send ever
+    return false;
+  }
+  if (loopmillis-lastSendSerial2expectFeedback <= SERIALACTIVECHECKTIME){ //enough time passed since last exprected feedback
+    return board2Enabled; //unclear, return exprected value
+  }
+  return loopmillis-lastValidDataSerial2_time < SERIALACTIVECHECKTIME;
 }
 
 void modeloops() {
@@ -459,16 +489,41 @@ void loop_off() {
 
 boolean boardsPowered()
 {
-  if (serial1Active()){
-    board1Enabled=true;
-  }
-  if (serial2Active()){
-    board2Enabled=true;
-  }
-  
   return (board1Enabled && board2Enabled); //true if both boards enabled
 }
 
+void failChecks() 
+{
+  if (serial1Active() && !board1Enabled){ //serial is active and board should be offline
+    errormessage="Board 1 is online but shouldnt";
+    requestmode=error;
+  }
+  if (serial2Active() && !board2Enabled){ //serial is active and board should be offline
+    errormessage="Board 2 is online but shouldnt";
+    requestmode=error;
+    Serial.print("loopmillis="); Serial.println(loopmillis);
+      Serial.print("lastBoard2PowerChange="); Serial.println(lastBoard2PowerChange);
+      Serial.print("lastSendSerial2expectFeedback="); Serial.println(lastSendSerial2expectFeedback);
+      Serial.print("lastValidDataSerial2_time="); Serial.println(lastValidDataSerial2_time);
+  }
+  if (out_speedRL != 0 || out_speedRR != 0) { //Rear is Serial1
+    if (!serial1Active() && board1Enabled){ //serial is not active and board should be online.  only check this if feedback can be exprected (out_speedFL/R > 0)
+      errormessage="Board 1 is offline but should be online";
+      requestmode=error;
+    }
+  }
+  if (out_speedFL != 0 || out_speedFR != 0) { //Front is Serial2
+    if (!serial2Active() && board2Enabled){ //serial is not active and board should be online.  only check this if feedback can be exprected (out_speedFL/R > 0)
+      errormessage="Board 2 is offline but should be online";
+      requestmode=error;
+      Serial.print("loopmillis="); Serial.println(loopmillis);
+      Serial.print("lastBoard2PowerChange="); Serial.println(lastBoard2PowerChange);
+      Serial.print("lastSendSerial2expectFeedback="); Serial.println(lastSendSerial2expectFeedback);
+      Serial.print("lastValidDataSerial2_time="); Serial.println(lastValidDataSerial2_time);
+    }
+  }
+  
+}
 
 String modeToString(uint8_t m) {
   if (m==idle) return "idle";