From 243d52f71eb60ba057222ba77e3c3d914be84da7 Mon Sep 17 00:00:00 2001
From: "@CarlWachter" <carlwachter@web.de>
Date: Thu, 4 Jan 2024 16:55:21 +0100
Subject: [PATCH] State transition can msg handling

---
 src/system/can_bus.cpp | 17 ++++++++++++++++-
 1 file changed, 16 insertions(+), 1 deletion(-)

diff --git a/src/system/can_bus.cpp b/src/system/can_bus.cpp
index 3e6a8b0..9fc36be 100644
--- a/src/system/can_bus.cpp
+++ b/src/system/can_bus.cpp
@@ -20,6 +20,9 @@
 #include <sta/rtos/system/can_bus.hpp>
 #include <sta/tacos/can_bus.hpp>
 
+#include <sta/config.hpp>
+#include <sta/tacos/statemachine.hpp>
+
 #include <cmsis_os2.h>
 #include <FreeRTOS.h>
 
@@ -87,7 +90,19 @@ namespace dummy
 		debug::printFrameID(header.id);
 		debug::printPayloadHex(payload, header.payloadLength);
 
-		// TODO Forward message to other threads
+		// Sysmessage is mainly only state change from GRSM
+		// TODO add other cases
+
+		// 0 is from state, 1 is to state, 2 is lockout, 3 is failsafe (-1 if inactive)
+		if(payload[1] > 0 && payload[1] < STA_TACOS_NUM_STATES){
+
+			if(payload[3] == -1){
+				sta::tacos::Statemachine::instance()->requestStateTransition(payload[0], payload[1], payload[2]);
+			}
+			else{
+				sta::tacos::Statemachine::instance()->requestTimedStateTransition(payload[0], payload[1], payload[2], payload[3]);
+			}
+		}
 	}
 
 	void handleDataMessage(const sta::IsotpMessage & msg)