From dd4c5f34a21003658d5334bf094a9623e023ff66 Mon Sep 17 00:00:00 2001
From: CarlWachter <carlwachter@web.de>
Date: Fri, 23 Aug 2024 15:59:35 +0200
Subject: [PATCH] feat: handleSysMessage checks allows user to overwrite CAN
 system behavior

---
 include/sta/tacos/can_bus.hpp | 13 ++++++++++---
 src/can_bus.cpp               | 33 +++++++++++++++++++--------------
 2 files changed, 29 insertions(+), 17 deletions(-)

diff --git a/include/sta/tacos/can_bus.hpp b/include/sta/tacos/can_bus.hpp
index 750292e..27b0086 100644
--- a/include/sta/tacos/can_bus.hpp
+++ b/include/sta/tacos/can_bus.hpp
@@ -111,10 +111,17 @@ namespace sta
         };
 
 	/**
-	 * @brief Callback function for handling received messages. Intended for state transitions.
-	*/
+	 * @brief Handle system messages received over the CAN bus. Called as soon as a message is received. If
+	 * the message is a system message, it will be handled here. If the message is not a system message, it will be
+	 * passed to the appropriate thread's queue.
+	 * 
+	 * @param header The header of the received message.
+	 * @param payload The payload of the received message.
+	 * 
+	 * @return True if the message was a system message.
+	 */
 	STA_WEAK
-	void handleSysMessage(CanMsgHeader & header, uint8_t * payload);
+	bool handleSysMessage(CanMsgHeader & header, uint8_t * payload);
 
     } /* namespace tacos */
 
diff --git a/src/can_bus.cpp b/src/can_bus.cpp
index 2eca8af..f870201 100644
--- a/src/can_bus.cpp
+++ b/src/can_bus.cpp
@@ -63,17 +63,15 @@ namespace sta
                     		sysMsg = *canBusSysQueueBuffer_[i];
                     		canBusSysQueueBuffer_[i] = nullptr;
 
-                    		// Append to the correct thread's queue
-							for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads()){
-								if (thread->getCanID() == sysMsg.header.sid){
-									thread->CAN_queue_.put(sysMsg);
-									break;
-								}
-							}
+							if (!handleSysMessage(sysMsg.header, sysMsg.payload)){
 
-							if(sysMsg.header.sid == STA_TACOS_CAN_BUS_SYS_MSG_ID){
-								// Handle system message
-								handleSysMessage(sysMsg.header, sysMsg.payload);
+								// Append to the correct thread's queue
+								for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads()){
+									if (thread->getCanID() == sysMsg.header.sid){
+										thread->CAN_queue_.put(sysMsg);
+										break;
+									}
+								}
 							}
 						}
                     }
@@ -148,15 +146,22 @@ namespace sta {
 
 	namespace tacos
 	{
-		void handleSysMessage(CanMsgHeader & header, uint8_t * payload)
+		STA_WEAK
+		bool handleSysMessage(CanMsgHeader & header, uint8_t * payload)
 		{
 			// This is a weak function that can be overridden by the user, 
 			// if they want to handle system messages in a different way, i.e. ignore them
 
-			STA_ASSERT(header.payloadLength == 2);
+			if(header.sid == 0x0){
+				STA_ASSERT(header.payloadLength == 2);
 
-			// First byte of payload is the origin state, second byte is the destination state. Transition is forced
-			tacos::setState(payload[0], payload[1], 0, true);
+				// First byte of payload is the origin state, second byte is the destination state. Transition is forced
+				tacos::setState(payload[0], payload[1], 0, true);
+
+				return true;
+			}
+
+			return false;
 		}
 	} // namespace tacos
 } // namespace sta