Added Publishing and handling of CAN state transition

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
 }
 

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

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