From 7147d6de8308110eace178b11ab88548ca29177a Mon Sep 17 00:00:00 2001
From: "@CarlWachter" <carlwachter@web.de>
Date: Fri, 8 Mar 2024 13:55:01 +0100
Subject: [PATCH] CAN implementation for standard CAN

---
 include/sta/tacos.hpp        |  4 +---
 include/sta/tacos/thread.hpp | 23 +++++++++++++++++++++++
 src/can_bus.cpp              | 36 ++++++++++++++++++++----------------
 src/tacos.cpp                |  6 +-----
 src/thread.cpp               | 14 ++++++++++++++
 5 files changed, 59 insertions(+), 24 deletions(-)

diff --git a/include/sta/tacos.hpp b/include/sta/tacos.hpp
index f969315..55f30f8 100644
--- a/include/sta/tacos.hpp
+++ b/include/sta/tacos.hpp
@@ -78,9 +78,7 @@ namespace sta
             return thread_ptr;
         }
 
-		bool queueCanBusMsg(const CanDataMsg & msg, uint32_t timeout);
-
-		bool queueCanBusMsg(const CanSysMsg & msg, uint32_t timeout);
+		bool queueCanBusMsg(CanSysMsg & msg, uint32_t timeout);
 	}  // namespace tacos
 }
 
diff --git a/include/sta/tacos/thread.hpp b/include/sta/tacos/thread.hpp
index 82a07dd..6344d55 100644
--- a/include/sta/tacos/thread.hpp
+++ b/include/sta/tacos/thread.hpp
@@ -12,6 +12,8 @@
 
 #include <sta/config.hpp>
 #include <sta/rtos/thread.hpp>
+#include <sta/rtos/queue.hpp>
+#include <sta/rtos/system/can_bus.hpp>
 
 /**
  * @defgroup tacos_thread TACOS Thread
@@ -126,6 +128,24 @@ namespace sta
 			 */
 			void sleep(uint32_t ticks);
 
+#ifdef STA_CAN_BUS_ENABLE
+			/**
+			 * @brief Set the ID of the CAN bus this thread is associated with.
+			 * 
+			 * @param canID 
+			 */
+			void setCanID(uint32_t canID);
+
+			/**
+			 * @brief Get the ID of the CAN bus this thread is associated with.
+			 * 
+			 * @return can ID 
+			 */
+			uint32_t getCanID();
+
+			sta::RtosQueue<CanSysMsg> CAN_queue_;
+#endif // STA_CAN_BUS_ENABLE
+
 #ifdef STA_TACOS_WATCHDOG_ENABLED
 			/**
 			 * @brief This macro wraps a given statement into waiting() and heartbeat() to make the code more readable.
@@ -191,6 +211,9 @@ namespace sta
 			bool running_;
 #ifdef STA_TACOS_WATCHDOG_ENABLED
 			ThreadStatus status_;
+#endif // STA_TACOS_WATCHDOG_ENABLED
+#ifdef STA_CAN_BUS_ENABLE
+			uint32_t canID_;
 #endif // STA_TACOS_WATCHDOG_ENABLED
 			bool terminate_;
 		};
diff --git a/src/can_bus.cpp b/src/can_bus.cpp
index a1aaf96..4ac2256 100644
--- a/src/can_bus.cpp
+++ b/src/can_bus.cpp
@@ -3,6 +3,8 @@
 
 #include <sta/tacos/can_bus.hpp>
 #include <sta/debug/assert.hpp>
+#include <sta/tacos/manager.hpp>
+
 // TODO fix this shit
 extern CAN_HandleTypeDef hcan1;
 namespace sta
@@ -43,18 +45,6 @@ namespace sta
                     }
                 }
 
-                if (flags & STA_RTOS_CAN_FLAG_DATA_QUEUED)
-                {
-                    // Take messages from queue until empty
-                    CanDataMsg msg;
-                    while (CanBus::_instance->getCanBusMsg(&msg, 0))
-                    {
-                        canBus_.send(msg);
-                    }
-
-					//canBus_.tx_.processFrame();
-                }
-
                 if (flags & STA_RTOS_CAN_FLAG_MSG_AVAIL)
                 {
                     STA_DEBUG_PRINTLN("[event] CAN INT");
@@ -68,11 +58,25 @@ namespace sta
                 }
             }
 
-			// Synchronus message check
-			canBus_.processRx();
+			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);
+
+			// 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;
+				}
+			}
 
-            // Process ISOTP transmissions
-            canBus_.processTx();
         }
 
         bool CanBus::queueCanBusMsg(const CanDataMsg& msg, uint32_t timeout)
diff --git a/src/tacos.cpp b/src/tacos.cpp
index 5c95424..6860a51 100644
--- a/src/tacos.cpp
+++ b/src/tacos.cpp
@@ -25,12 +25,8 @@ namespace sta
         {
             Statemachine::instance()->requestTimedStateTransition(from, to, millis, lockout);
         }
-		
-        bool queueCanBusMsg(const CanDataMsg & msg, uint32_t timeout){
-			return CanBus::instance()->queueCanBusMsg(msg, timeout);
-		}
 
-        bool queueCanBusMsg(const CanSysMsg & msg, uint32_t timeout){
+        bool queueCanBusMsg(CanSysMsg & msg, uint32_t timeout){
 			return CanBus::instance()->queueCanBusMsg(msg, timeout);
 		}
     } // namespace tacos
diff --git a/src/thread.cpp b/src/thread.cpp
index 41e2b16..a536b67 100644
--- a/src/thread.cpp
+++ b/src/thread.cpp
@@ -27,6 +27,10 @@ namespace sta
 #ifdef STA_TACOS_WATCHDOG_ENABLED
 				  status_{ThreadStatus::STOPPED},
 #endif // STA_TACOS_WATCHDOG_ENABLED
+#ifdef STA_CAN_BUS_ENABLE
+				  canID_{0},
+				  CAN_queue_{STA_RTOS_CAN_BUS_QUEUE_LENGTH},
+#endif // STA_CAN_BUS_ENABLE
 				  terminate_{false}
 		{
 			STA_ASSERT(stack_size >= 0);
@@ -165,6 +169,16 @@ namespace sta
 
 		TacosThread::~TacosThread(){}
 
+#ifdef STA_CAN_BUS_ENABLE
+		void TacosThread::setCanID(uint32_t canID){
+			canID_ = canID;
+		}
+
+		uint32_t TacosThread::getCanID(){
+			return canID_;
+		}
+#endif // STA_CAN_BUS_ENABLE
+
 	}  // namespace tacos
 }  // namespace sta