From 7e0d3bfd19c3832739b9a66cd9292d69065a0116 Mon Sep 17 00:00:00 2001
From: "@CarlWachter" <carlwachter@web.de>
Date: Fri, 5 Jan 2024 15:28:02 +0100
Subject: [PATCH] removed tacos dependencies from rtos2-utils

---
 include/sta/tacos/can_bus.hpp |  8 ++++
 src/can_bus.cpp               | 90 ++++++++++++++++++++++++++++++++++-
 2 files changed, 97 insertions(+), 1 deletion(-)

diff --git a/include/sta/tacos/can_bus.hpp b/include/sta/tacos/can_bus.hpp
index f235311..aac82fd 100644
--- a/include/sta/tacos/can_bus.hpp
+++ b/include/sta/tacos/can_bus.hpp
@@ -8,6 +8,9 @@
 #include <sta/tacos/thread.hpp>
 #include <sta/rtos/queue.hpp>
 #include <sta/rtos/system/can_bus.hpp>
+#include <sta/debug/debug.hpp>
+
+#include <sta/tacos/statemachine.hpp>
 
 
 /**
@@ -115,6 +118,11 @@ namespace sta
 
         };
 
+
+	void handleSysMessage(const sta::CanRxHeader & header, const uint8_t * payload);
+
+	void handleDataMessage(const sta::IsotpMessage & msg);
+
     } /* namespace tacos */
 
 } /* namespace sta */
diff --git a/src/can_bus.cpp b/src/can_bus.cpp
index 3726a13..48ecfa2 100644
--- a/src/can_bus.cpp
+++ b/src/can_bus.cpp
@@ -6,6 +6,19 @@
 #include <sta/debug/debug.hpp>
 #include <sta/debug/assert.hpp>
 
+namespace sta
+{
+	namespace rtos
+	{
+
+		void initCanBus()
+		{
+			sta::tacos::CanBus::instance(getCanController())->start();
+		}
+
+	} // namespace rtos
+} // namespace sta
+
 namespace sta
 {
 	namespace tacos
@@ -15,7 +28,7 @@ namespace sta
 			  canBusController_(new STM32CanController(controller)),
 			  canBusSysQueue_(STA_RTOS_CAN_BUS_QUEUE_LENGTH),
 			  canBusDataQueue_(STA_RTOS_CAN_BUS_QUEUE_LENGTH),
-              canBus_{canBusController_, HAL_GetTick, dummy::handleSysMessage, dummy::handleDataMessage}
+              canBus_{canBusController_, HAL_GetTick, sta::tacos::handleSysMessage, sta::tacos::handleDataMessage}
 		{
 		}
 
@@ -118,4 +131,79 @@ namespace sta
     } /* namespace tacos */
 } /* namespace sta */
 
+namespace sta {
+	namespace tacos
+	{
+		void handleSysMessage(const sta::CanRxHeader & header, const uint8_t * payload)
+		{
+			// Write frame payload to DebugSerial
+			STA_DEBUG_PRINTLN("[event] RX sys frame");
+
+			debug::printFrameID(header.id);
+			debug::printPayloadHex(payload, header.payloadLength);
+
+			// 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)
+		{
+			STA_ASSERT(msg.buffer != nullptr);
+			STA_ASSERT(msg.size != 0);
+
+			STA_DEBUG_PRINTLN("[event] RX data message");
+
+			debug::printFrameID(msg.frameID);
+
+			// TODO Forward message to other threads
+
+	//		if (buffer[0] == DEMO_BMP_PACKET_ID)
+	//		{
+	//			BmpPacket packet;
+	//			if (unpack(buffer + 1, size - 1, &packet))
+	//			{
+	//				STA_DEBUG_PRINTLN();
+	//				STA_DEBUG_PRINTLN("# ############");
+	//				STA_DEBUG_PRINTLN("# ## BMP380 ##");
+	//				STA_DEBUG_PRINTLN("# ############");
+	//				STA_DEBUG_PRINTLN("#");
+	//
+	//				STA_DEBUG_PRINT("# temperature: ");
+	//				STA_DEBUG_PRINT(packet.temperature);
+	//				STA_DEBUG_PRINTLN(" *C");
+	//				STA_DEBUG_PRINT("# pressure: ");
+	//				STA_DEBUG_PRINT(packet.pressure);
+	//				STA_DEBUG_PRINTLN(" Pa");
+	//				STA_DEBUG_PRINT("# altitude: ");
+	//				STA_DEBUG_PRINT(packet.altitude);
+	//				STA_DEBUG_PRINTLN(" m");
+	//				STA_DEBUG_PRINTLN();
+	//			}
+	//			else
+	//			{
+	//				STA_DEBUG_PRINTLN("[error] BMP unpack failed");
+	//			}
+	//		}
+	//		else
+			{
+				STA_DEBUG_PRINT("ID: ");
+				STA_DEBUG_PRINTLN(msg.buffer[0], sta::IntegerBase::HEX);
+				STA_DEBUG_PRINT("size: ");
+				STA_DEBUG_PRINTLN(msg.size);
+			}
+		}
+	} // namespace tacos
+} // namespace sta
+
 #endif // STA_RTOS_CAN_BUS_ENABLE
\ No newline at end of file