Added state publishing to TACOS state transition

include/sta/tacos.hpp

@@ -46,7 +46,7 @@ namespace sta
 		 * 
 		 * @ingroup tacos_api
 		 */
-		void setState(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false);
+		void setState(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false, bool publish = true);
 
 		/**
 		 * @brief Request a state transition after a given time has passed. Invalid state transitions will be dismissed.
@@ -58,7 +58,7 @@ namespace sta
 		 * 
 		 * @ingroup tacos_api
 		 */
-		void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0);
+		void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0, bool publish = true);
 
 		/**
 		 * @brief Register a new thread to be run by TACOS.
@@ -97,13 +97,13 @@ namespace sta
 		 * 
 		 * @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.
+		 * @param timeout An optional timeout for the CAN communication
 		 * 
 		 * @return bool True if the message was sent successfully.
 		 * 
 		 * @ingroup tacos_api
 		 */
-		bool publishState(uint32_t from, uint32_t to, uint32_t lockout = 0);
+		bool publishState(uint32_t from, uint32_t to, uint32_t timeout = 0);
 #endif // STA_TACOS_CAN_BUS_ENABLED
 	}  // namespace tacos
 }

include/sta/tacos/statemachine.hpp

@@ -120,6 +120,8 @@ namespace sta
 			EventFlags event; 	
 			/// Lockout time after transition	
 			uint32_t lockout;
+			/// Whether to publish the transition via CAN.
+			bool publish = false;
 		};
 
 		/**
@@ -164,7 +166,7 @@ namespace sta
 			 * @param lockout The minimum number of milliseconds we expect to stay in this state. This is used to block premature transitions.
 			 * @param force If true, the state transition will be executed regardless of the current state.
 			 */
-			void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false);
+			void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false, bool publish = true);
 
 			/**
 			 * @brief Request a state transition after a given time has passed.
@@ -174,7 +176,7 @@ namespace sta
 			 * @param millis the number of milliseconds to wait before triggering the transition.
 			 * @param lockout The minimum number of milliseconds we expect to stay in this state. This is used to block premature transitions.
 			 */
-			void requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0);
+			void requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0, bool publish = true);
 
 			void init() override;
 			void func() override;

src/statemachine.cpp

@@ -5,6 +5,7 @@
  *      Author: Dario
  */
 
+#include <sta/tacos.hpp>
 #include <sta/tacos/statemachine.hpp>
 
 #include <sta/debug/debug.hpp>
@@ -40,9 +41,16 @@ namespace sta
 			{
 				STA_ASSERT(transition.to < STA_TACOS_NUM_STATES);
 
+#ifdef STA_TACOS_CAN_BUS_ENABLED
+				// Publish the state via CAN bus.
+				tacos::publishState(transition.from, transition.to, 0);
+#endif // STA_TACOS_CAN_BUS_ENABLED
+
 				// Perform the transition and notify the threads. The event flags are set
 				// here in order to allow threads to react immediately.
 				currentState_ = transition.to;
+
+				// Send a system-wide notification for the state transition.
 				Statemachine::stateChangeEvent.set(transition.event);
 				Statemachine::stateChangeEvent.clear(EventFlags::ALL);
 
@@ -64,19 +72,25 @@ namespace sta
 			return currentState_;
 		}
 
-		void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = 0 */)
+		void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = 0 */, bool publish /* = true */)
 		{
 			StateTransition transition;
 			transition.from = from;
 			transition.to = to;
 			transition.event = EventFlags::NORMAL;
 			transition.lockout = lockout;
+			transition.publish = publish;
 
 			// Force the transition if requested, but only if the requested state is different from the current one.
 			if (force && transition.to != currentState_){
 				// Perform the transition and notify the threads. The event flags are set
 				// here in order to allow threads to react immediately.
 				currentState_ = transition.to;
+
+#ifdef STA_TACOS_CAN_BUS_ENABLED
+				tacos::publishState(transition.from, transition.to, transition.lockout);
+#endif // STA_TACOS_CAN_BUS_ENABLED
+
 				Statemachine::stateChangeEvent.set(transition.event);
 				Statemachine::stateChangeEvent.clear(EventFlags::ALL);
 
@@ -90,19 +104,19 @@ namespace sta
 				{
 					setLockoutTimer(transition.lockout);
 				}
-			}else{
+			} else {
 				// Try to add a state transition request to the queue. Don't wait if another
 				// thread is already requesting a state change.
 				queue_.put(transition, 0);
 			}
 		}
 
-		void Statemachine::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */)
+		void Statemachine::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */, bool publish /* = true */)
 		{
 			STA_ASSERT(to < STA_TACOS_NUM_STATES);
 
-			failsafeTimer_.setCallback([from, to, lockout](void* arg) {
+			failsafeTimer_.setCallback([from, to, lockout, publish](void* arg) {
-				Statemachine::instance()->requestStateTransition(from, to, lockout);
+				Statemachine::instance()->requestStateTransition(from, to, lockout, false, publish);
 			}, NULL);
 
 			failsafeTimer_.start(millis);

src/tacos.cpp

@@ -16,14 +16,14 @@ namespace sta
             return Statemachine::instance()->getCurrentState();
         }
 
-		void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = false */)
+		void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = false */, bool publish /* = false */)
         {
-            Statemachine::instance()->requestStateTransition(from, to, lockout, force);
+            Statemachine::instance()->requestStateTransition(from, to, lockout, force, publish);
         }
 
-		void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */)
+		void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */, bool publish /* = false */)
         {
-            Statemachine::instance()->requestTimedStateTransition(from, to, millis, lockout);
+            Statemachine::instance()->requestTimedStateTransition(from, to, millis, lockout, publish);
         }
 
 #ifdef STA_TACOS_CAN_BUS_ENABLED
@@ -31,7 +31,7 @@ namespace sta
 			return CanBus::instance()->queueCanBusMsg(msg, timeout);
 		}
 
-        bool publishState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */){
+        bool publishState(uint32_t from, uint32_t to, uint32_t timeout /* = 0 */){
             CanSysMsg msg;
             msg.header.sid = STA_TACOS_CAN_BUS_SYS_MSG_ID;
             msg.header.payloadLength = 2;
@@ -41,7 +41,7 @@ namespace sta
             msg.header.eid = 0;
             msg.header.format = 0;
 
-            return CanBus::instance()->queueCanBusMsg(msg, lockout);
+            return CanBus::instance()->queueCanBusMsg(msg, timeout);
         }
 #endif // STA_TACOS_CAN_BUS_ENABLED
     } // namespace tacos