From 4316e604daa61471a9fb36d087481817a153b770 Mon Sep 17 00:00:00 2001
From: jazzpi <jasper@mezzo.de>
Date: Sat, 30 Jul 2022 22:22:05 +0200
Subject: [PATCH] Check individual heartbeat frames for timeouts

---
 Core/Inc/AMS_Errorcodes.h    |  1 +
 Core/Inc/Slave_Monitoring.h  |  2 ++
 Core/Src/CAN_Communication.c |  1 +
 Core/Src/Slave_Monitoring.c  | 29 +++++++++++++++++++++++++++--
 4 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/Core/Inc/AMS_Errorcodes.h b/Core/Inc/AMS_Errorcodes.h
index 9caf9f8..c96572f 100644
--- a/Core/Inc/AMS_Errorcodes.h
+++ b/Core/Inc/AMS_Errorcodes.h
@@ -12,5 +12,6 @@
 
 #define SlavesTimeoutError 1
 #define SlavesErrorFrameError 2
+#define SLAVES_FRAME_TIMEOUT_ERROR 3
 
 #endif /* INC_AMS_ERRORCODES_H_ */
diff --git a/Core/Inc/Slave_Monitoring.h b/Core/Inc/Slave_Monitoring.h
index e2899a4..a64437f 100644
--- a/Core/Inc/Slave_Monitoring.h
+++ b/Core/Inc/Slave_Monitoring.h
@@ -19,6 +19,7 @@
 #define NUMBEROFTEMPS 32
 
 #define SLAVETIMEOUT 500
+#define SLAVE_HEARTBEAT_FRAMES 11
 
 typedef struct {
 
@@ -28,6 +29,7 @@ typedef struct {
   uint32_t timestamp;
   uint8_t error;
   uint8_t timeout;
+  uint32_t frame_timestamps[SLAVE_HEARTBEAT_FRAMES];
 
 } SlaveHandler;
 
diff --git a/Core/Src/CAN_Communication.c b/Core/Src/CAN_Communication.c
index 748e229..66060d2 100644
--- a/Core/Src/CAN_Communication.c
+++ b/Core/Src/CAN_Communication.c
@@ -210,5 +210,6 @@ void updateSlaveInfo(uint8_t slaveID, uint8_t MessageID, canFrame rxFrame) {
       break;
     }
     slaves[slaveID].timestamp = rxFrame.timestamp;
+    slaves[slaveID].frame_timestamps[MessageID] = rxFrame.timestamp;
   }
 }
diff --git a/Core/Src/Slave_Monitoring.c b/Core/Src/Slave_Monitoring.c
index 166b048..6b8b7be 100644
--- a/Core/Src/Slave_Monitoring.c
+++ b/Core/Src/Slave_Monitoring.c
@@ -7,6 +7,11 @@
 
 #include "Slave_Monitoring.h"
 
+#include "AMS_Errorcodes.h"
+#include "main.h"
+
+#include "stm32g4xx_hal.h"
+
 SlaveHandler slaves[NUMBEROFSLAVES];
 
 void initSlaves() {
@@ -25,15 +30,18 @@ void initSlaves() {
 
   for (int n = 0; n < NUMBEROFSLAVES; n++) {
     for (int i = 0; i < NUMBEROFTEMPS; i++)
-      slaves[n].cellTemps[i] = 30000;
+      slaves[n].cellTemps[i] = 0;
 
     for (int j = 0; j < NUMBEROFCELLS; j++)
-      slaves[n].cellVoltages[j] = 25000;
+      slaves[n].cellVoltages[j] = 32768;
 
     slaves[n].error = 0;
     slaves[n].timeout = 0;
     slaves[n].timestamp = HAL_GetTick();
     slaves[n].slaveID = n;
+    for (int i = 0; i < SLAVE_HEARTBEAT_FRAMES; i++) {
+      slaves[n].frame_timestamps[i] = HAL_GetTick();
+    }
   }
 }
 
@@ -53,6 +61,23 @@ uint8_t checkSlaveTimeout() {
       AMS_Error_Handler(&timeouterror);
       return 1;
     }
+
+    if (HAL_GetTick() < 20000) {
+      continue;
+    }
+
+    for (int i = 0; i < SLAVE_HEARTBEAT_FRAMES; i++) {
+      if ((int)(HAL_GetTick() - slaves[n].frame_timestamps[i]) >
+          SLAVETIMEOUT * 2) {
+        slaves[n].timeout = 1;
+        AMSErrorHandle timeouterror;
+        timeouterror.errorcode = SLAVES_FRAME_TIMEOUT_ERROR;
+        timeouterror.errorarg[0] = n;
+        timeouterror.errorarg[1] = i;
+        AMS_Error_Handler(&timeouterror);
+        return 1;
+      }
+    }
   }
   return 0;
 }