Merge pull request 'Handling CAN sys message' (#34) from feature/can_system_msg into main

Reviewed-on: https://git.intern.spaceteamaachen.de/ALPAKA/TACOS/pulls/34
Reviewed-by: dario <dario@noreply.git.intern.spaceteamaachen.de>

include/sta/tacos.hpp

@@ -42,10 +42,11 @@ namespace sta
 		 * @param from The start 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 force If true, the state transition will be executed regardless of the current state.
 		 * 
 		 * @ingroup tacos_api
 		 */
-		void setState(uint32_t from, uint32_t to, uint32_t lockout = 0);
+		void setState(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false);
 
 		/**
 		 * @brief Request a state transition after a given time has passed. Invalid state transitions will be dismissed.

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 */
 

include/sta/tacos/statemachine.hpp

@@ -162,8 +162,9 @@ namespace sta
 			 * @param from The state which we want to leave. This is used to filter out obsolete transitions.
 			 * @param to The state to transition to.
 			 * @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);
+			void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false);
 
 			/**
 			 * @brief Request a state transition after a given time has passed.

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 == STA_TACOS_CAN_BUS_SYS_MSG_ID){
+				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]);
+				// 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

src/statemachine.cpp

@@ -64,7 +64,7 @@ namespace sta
 			return currentState_;
 		}
 
-		void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */)
+		void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = 0 */)
 		{
 			StateTransition transition;
 			transition.from = from;
@@ -72,9 +72,28 @@ namespace sta
 			transition.event = EventFlags::NORMAL;
 			transition.lockout = lockout;
 
-			// 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);
+			if (force){
+				// Perform the transition and notify the threads. The event flags are set
+				// here in order to allow threads to react immediately.
+				currentState_ = transition.to;
+				Statemachine::stateChangeEvent.set(transition.event);
+				Statemachine::stateChangeEvent.clear(EventFlags::ALL);
+
+				if (failsafeTimer_.isRunning())
+				{
+					failsafeTimer_.stop();
+				}
+
+				// Start the lockout timer if requested.
+				if (transition.lockout != 0)
+				{
+					setLockoutTimer(transition.lockout);
+				}
+			}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 */)

src/tacos.cpp

@@ -16,9 +16,9 @@ namespace sta
             return Statemachine::instance()->getCurrentState();
         }
 
-		void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */)
+		void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = false */)
         {
-            Statemachine::instance()->requestStateTransition(from, to, lockout);
+            Statemachine::instance()->requestStateTransition(from, to, lockout, force);
         }
 
 		void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */)