From 39024e4a8c53ed19f9313a1d52adff763d06ba75 Mon Sep 17 00:00:00 2001
From: "@CarlWachter" <carlwachter@web.de>
Date: Fri, 8 Mar 2024 19:03:50 +0100
Subject: [PATCH] Basic working can implementation

---
 src/can_bus.cpp | 48 +++++++++++++++++++++++++++++++++---------------
 1 file changed, 33 insertions(+), 15 deletions(-)

diff --git a/src/can_bus.cpp b/src/can_bus.cpp
index 4ac2256..e6213e3 100644
--- a/src/can_bus.cpp
+++ b/src/can_bus.cpp
@@ -28,7 +28,7 @@ namespace sta
         void CanBus::func()
         {
         	messageEvent.clear(STA_RTOS_CAN_ANY);
-            uint32_t flags = messageEvent.wait(STA_RTOS_CAN_ANY, 800);
+            uint32_t flags = messageEvent.wait(STA_RTOS_CAN_ANY, 500);
 
             if (flags != static_cast<uint32_t>(osErrorTimeout))
             {
@@ -40,7 +40,6 @@ namespace sta
                     CanSysMsg msg;
                     while (CanBus::_instance->getCanBusMsg(&msg, 0))
                     {
-                    	STA_DEBUG_PRINTLN("CanBus about to send");
                         canBus_.send(msg);
                     }
                 }
@@ -49,7 +48,7 @@ namespace sta
                 {
                     STA_DEBUG_PRINTLN("[event] CAN INT");
 
-                    canBus_.processRx();
+                    //canBus_.processRx();
                 }
 
                 if (flags & STA_RTOS_CAN_FLAG_SHOW_STATS)
@@ -61,19 +60,37 @@ namespace sta
 			CanRxHeader rxHeader;					//CAN Bus Receive Header
 			uint8_t canRX[8] = {0,0,0,0,0,0,0,0};  	//CAN Bus Receive Buffer
 
-			canBusController_->receiveFrame(CAN_RX_FIFO0, &rxHeader, canRX);
+			bool received_ = canBusController_->receiveFrame(CAN_RX_FIFO0, &rxHeader, canRX);
+			if (received_){
+				// Create a CANSysMsg from the received data
+				CanSysMsg sysMsg;
+				sysMsg.header.sid = rxHeader.id.sid;
+				sysMsg.header.payloadLength = rxHeader.payloadLength;
+				std::memcpy(sysMsg.payload, canRX, rxHeader.payloadLength);
 
-			// Create a CANSysMsg from the received data
-			CanSysMsg sysMsg;
-			sysMsg.header.sid = rxHeader.id.sid;
-			sysMsg.header.payloadLength = rxHeader.payloadLength;
-			std::memcpy(sysMsg.payload, canRX, rxHeader.payloadLength);
+				// Append to the correct thread's queue
+				for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads()){
+					if (thread->getCanID() == rxHeader.id.sid){
+						thread->CAN_queue_.put(sysMsg);
+						break;
+					}
+				}
+			}
+			received_ = canBusController_->receiveFrame(CAN_RX_FIFO1, &rxHeader, canRX);
+			if (received_){
+				// Create a CANSysMsg from the received data
+				CanSysMsg sysMsg;
+				sysMsg.header.sid = rxHeader.id.sid;
+				sysMsg.header.payloadLength = rxHeader.payloadLength;
+				sysMsg.header.format = 0;
+				std::memcpy(sysMsg.payload, canRX, rxHeader.payloadLength);
 
-			// Append to the correct thread's queue
-			for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads()){
-				if (thread->getCanID() == rxHeader.id.sid){
-					thread->CAN_queue_.put(sysMsg);
-					break;
+				// Append to the correct thread's queue
+				for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads()){
+					if (thread->getCanID() == rxHeader.id.sid){
+						thread->CAN_queue_.put(sysMsg);
+						break;
+					}
 				}
 			}
 
@@ -99,7 +116,8 @@ namespace sta
 
 		bool CanBus::queueCanBusMsg(const CanSysMsg& msg, uint32_t timeout)
 		{
-			STA_ASSERT((msg.header.sid & ~STA_CAN_SID_SYS_BITS) == 0);
+			// This technicall should check if we are using a system message, but we just pretending that everything is one of those rn
+			//STA_ASSERT((msg.header.sid & ~STA_CAN_SID_SYS_BITS) == 0);
 
 			if (canBusSysQueue_.put(msg, timeout))
 			{