From 77f600ddb3b1ac9fa4fc9cc0c0d9c25d9ea36e53 Mon Sep 17 00:00:00 2001
From: jazzpi <jasper@mezzo.de>
Date: Thu, 4 Aug 2022 16:20:18 +0200
Subject: [PATCH] Prevent stack overflow on slave panic

---
 Core/Inc/Slave_Monitoring.h  |  1 +
 Core/Src/CAN_Communication.c |  8 +++-----
 Core/Src/Slave_Monitoring.c  | 10 ++++++++--
 3 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/Core/Inc/Slave_Monitoring.h b/Core/Inc/Slave_Monitoring.h
index bc0524c..1305f21 100644
--- a/Core/Inc/Slave_Monitoring.h
+++ b/Core/Inc/Slave_Monitoring.h
@@ -32,6 +32,7 @@ typedef struct {
   uint8_t error;
   uint8_t timeout;
   uint32_t frame_timestamps[SLAVE_HEARTBEAT_FRAMES];
+  uint8_t error_frame[8];
 
 } SlaveHandler;
 
diff --git a/Core/Src/CAN_Communication.c b/Core/Src/CAN_Communication.c
index 66060d2..baf1b23 100644
--- a/Core/Src/CAN_Communication.c
+++ b/Core/Src/CAN_Communication.c
@@ -72,11 +72,9 @@ uint8_t CAN_Receive(FDCAN_HandleTypeDef* hcan) {
       uint8_t messageID = msg & 0x00F;
       updateSlaveInfo(slaveID, messageID, rxFrame);
     } else if (rxFrame.FrameID == SLAVE_EMERGENCY_ADDRESS) {
-      AMSErrorHandle errorframe = {0};
-      errorframe.errorcode = SlavesErrorFrameError;
-      memcpy(errorframe.errorarg, rxFrame.data, 7);
-
-      AMS_Error_Handler(&errorframe);
+      uint8_t slave_id = rxFrame.data[0];
+      slaves[slave_id].error = 1;
+      memcpy(slaves[slave_id].error_frame, rxFrame.data, 8);
     }
   }
 
diff --git a/Core/Src/Slave_Monitoring.c b/Core/Src/Slave_Monitoring.c
index 1508eff..57857c4 100644
--- a/Core/Src/Slave_Monitoring.c
+++ b/Core/Src/Slave_Monitoring.c
@@ -13,6 +13,8 @@
 #include "stm32g4xx_hal.h"
 #include "stm32g4xx_hal_gpio.h"
 
+#include <string.h>
+
 SlaveHandler slaves[NUMBEROFSLAVES];
 
 void initSlaves() {
@@ -63,8 +65,12 @@ uint8_t checkSlaveTimeout() {
       return 1;
     }
 
-    if (HAL_GetTick() < 20000) {
-      continue;
+    if (slaves[n].error) {
+      AMSErrorHandle errorframe;
+      errorframe.errorcode = SlavesErrorFrameError;
+      memcpy(errorframe.errorarg, slaves[n].error_frame, 7);
+      AMS_Error_Handler(&errorframe);
+      return 1;
     }
 
     for (int i = 0; i < SLAVE_HEARTBEAT_FRAMES; i++) {