From 3af6656e0f122d0a32fe5407fd32c3fa7bfd2a19 Mon Sep 17 00:00:00 2001
From: CarlWachter <carlwachter@web.de>
Date: Tue, 30 Apr 2024 17:33:52 +0200
Subject: [PATCH] Added Publishing and handling of CAN state transition

---
 include/sta/tacos.hpp | 24 ++++++++++++++++++++++++
 src/can_bus.cpp       |  9 ++++++++-
 src/tacos.cpp         | 13 +++++++++++++
 3 files changed, 45 insertions(+), 1 deletion(-)

diff --git a/include/sta/tacos.hpp b/include/sta/tacos.hpp
index 55f30f8..4672dbf 100644
--- a/include/sta/tacos.hpp
+++ b/include/sta/tacos.hpp
@@ -78,7 +78,31 @@ namespace sta
             return thread_ptr;
         }
 
+		/**
+		 * @brief Queue a message to be sent over the CAN bus.
+		 * 
+		 * @param msg The message to be sent.
+		 * @param timeout The time to wait for the message to be sent.
+		 * 
+		 * @return bool True if the message was sent successfully.
+		 * 
+		 * @ingroup tacos_api
+		 */
 		bool queueCanBusMsg(CanSysMsg & msg, uint32_t timeout);
+
+		/**
+		 * @brief Publish a state transition message to the CAN bus.
+		 * 
+		 * @param from The state we want to transition from.
+		 * @param to The state we want to transition to.
+		 * @param lockout An optional timer blocking state transition for a given time.
+		 * 
+		 * @return bool True if the message was sent successfully.
+		 * 
+		 * @ingroup tacos_api
+		 */
+		bool publishState(uint32_t from, uint32_t to, uint32_t lockout = 0);
+
 	}  // namespace tacos
 }
 
diff --git a/src/can_bus.cpp b/src/can_bus.cpp
index a9767fe..52656c6 100644
--- a/src/can_bus.cpp
+++ b/src/can_bus.cpp
@@ -4,6 +4,7 @@
 #include <sta/tacos/can_bus.hpp>
 #include <sta/debug/assert.hpp>
 #include <sta/tacos/manager.hpp>
+#include <sta/tacos.hpp>
 
 extern CAN_HandleTypeDef STA_STM32_CAN_HANDLE;
 
@@ -149,7 +150,13 @@ namespace sta {
 	{
 		void handleSysMessage(CanMsgHeader & header, uint8_t * payload)
 		{
-			// This is a weak function that can be overridden by the user
+			// 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);
+
+			// First byte of payload is the origin state, second byte is the destination state
+			tacos::setState(payload[0], payload[1]);
 		}
 	} // namespace tacos
 } // namespace sta
diff --git a/src/tacos.cpp b/src/tacos.cpp
index 6860a51..b4e4ba3 100644
--- a/src/tacos.cpp
+++ b/src/tacos.cpp
@@ -29,6 +29,19 @@ namespace sta
         bool queueCanBusMsg(CanSysMsg & msg, uint32_t timeout){
 			return CanBus::instance()->queueCanBusMsg(msg, timeout);
 		}
+
+        bool publishState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */){
+            CanSysMsg msg;
+            msg.header.sid = STA_TACOS_CAN_BUS_SYS_MSG_ID;
+            msg.header.payloadLength = 2;
+            msg.payload[0] = from;
+            msg.payload[1] = to;
+
+            msg.header.eid = 0;
+            msg.header.format = 0;
+
+            return CanBus::instance()->queueCanBusMsg(msg, lockout);
+        }
     } // namespace tacos
     
 } // namespace sta