Merge pull request 'CAN RX fix, avoid buffer overflows' (#39) from fix/canRx into main

Reviewed-on: https://git.intern.spaceteamaachen.de/ALPAKA/TACOS/pulls/39

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 = false);
 
 		/**
 		 * @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 = false);
 
 		/**
 		 * @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/can_bus.hpp

@@ -61,7 +61,7 @@ namespace sta
 			 * @param timeout   Timeout for placing message (0 = no wait, osWaitForever = blocking)
 			 * @return          True if message was queued successfully
 			 */
-			bool queueCanBusMsg(const CanSysMsg & msg, uint32_t timeout);
+			bool queueCanBusMsg(const CanSysMsg msg, uint32_t timeout);
 
 			/**
 			 * @brief Retrieve system message from CAN driver TX queue.
@@ -100,10 +100,8 @@ namespace sta
 
 			sta::STM32CanController * canBusController_;
 
-            CanSysMsg* canBusSysQueueBuffer_[STA_RTOS_CAN_BUS_QUEUE_LENGTH];
+			RtosQueue<CanSysMsg> canBusSysQueue_;
-            uint8_t bufferIndex;
+            RtosQueue<CanSysMsg> canBusRxQueue_;
-
-            RtosQueue<CanSysMsg> canBusSysQueue_;
 
             AlpakaCanBus canBus_;
 

include/sta/tacos/statemachine.hpp

@@ -119,7 +119,9 @@ namespace sta
 			/// Event that triggered the transition
 			EventFlags event; 	
 			/// Lockout time after transition	
-			uint32_t lockout; 		
+			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/can_bus.cpp

@@ -16,77 +16,73 @@ namespace sta
 			: TacosThread{"Can Bus", STA_TACOS_CAN_BUS_PRIORITY},
 			  canBusController_(new STM32CanController(&STA_STM32_CAN_HANDLE)),
 			  canBusSysQueue_(STA_RTOS_CAN_BUS_QUEUE_LENGTH),
-              canBus_{canBusController_, HAL_GetTick}
+			  canBusRxQueue_(STA_RTOS_CAN_BUS_QUEUE_LENGTH),
+			  canBus_{canBusController_, HAL_GetTick}
 		{
-			bufferIndex = 0;
+		}
-			for(int i = 0; i < STA_RTOS_CAN_BUS_QUEUE_LENGTH; i++){
+
-				canBusSysQueueBuffer_[i] = nullptr;
+		void CanBus::init()
+		{
+			canBusController_->start();
+
+			if (HAL_CAN_ActivateNotification(&STA_STM32_CAN_HANDLE, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK ||
+				HAL_CAN_ActivateNotification(&STA_STM32_CAN_HANDLE, CAN_IT_RX_FIFO1_MSG_PENDING))
+			{
+				Error_Handler();
 			}
 		}
 
-        void CanBus::init()
+		void CanBus::func()
-	    {
+		{
-		    canBusController_->start();
+			messageEvent.clear(STA_RTOS_CAN_ANY);
+			uint32_t flags = messageEvent.wait(STA_RTOS_CAN_ANY, osWaitForever);
 
-		    if (HAL_CAN_ActivateNotification(&STA_STM32_CAN_HANDLE, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK ||
+			if (flags != static_cast<uint32_t>(osErrorTimeout))
-		    		HAL_CAN_ActivateNotification(&STA_STM32_CAN_HANDLE, CAN_IT_RX_FIFO1_MSG_PENDING))
+			{
-		    {
+				STA_ASSERT_MSG((flags & osStatusReserved) == flags, "Unexpected error occurred in wait");
-		      	 Error_Handler();
-		    }
-	    }
 
-        void CanBus::func()
+				if (flags & STA_RTOS_CAN_FLAG_SYS_QUEUED)
-        {
+				{
-        	messageEvent.clear(STA_RTOS_CAN_ANY);
+					// Take messages from queue until empty
-            uint32_t flags = messageEvent.wait(STA_RTOS_CAN_ANY, osWaitForever);
+					CanSysMsg msg;
+					while (CanBus::_instance->getCanBusMsg(&msg, 0))
+					{
+						canBus_.send(msg);
+					}
+				}
 
-            if (flags != static_cast<uint32_t>(osErrorTimeout))
+				if (flags & STA_RTOS_CAN_FLAG_MSG_AVAIL)
-            {
+				{
-                STA_ASSERT_MSG((flags & osStatusReserved) == flags, "Unexpected error occurred in wait");
+					CanSysMsg sysMsg;
-
+					// Iterate through buffer and set back to nullptr after use
-                if (flags & STA_RTOS_CAN_FLAG_SYS_QUEUED)
+					while (canBusRxQueue_.get(&sysMsg, 0))
-                {
+					{
-                    // Take messages from queue until empty
-                    CanSysMsg msg;
-                    while (CanBus::_instance->getCanBusMsg(&msg, 0))
-                    {
-                        canBus_.send(msg);
-                    }
-                }
-
-                if (flags & STA_RTOS_CAN_FLAG_MSG_AVAIL)
-                {
-                    CanSysMsg sysMsg;
-                    // Iterate through buffer and set back to nullptr after use
-                    for(int i = 0; i < STA_RTOS_CAN_BUS_QUEUE_LENGTH; i++){
-                    	if(canBusSysQueueBuffer_[i] != nullptr){
-                    		sysMsg = *canBusSysQueueBuffer_[i];
-                    		canBusSysQueueBuffer_[i] = nullptr;
 #ifndef STA_CAN_BUS_FWD_ENABLE
-							handleSysMessage(sysMsg.header, sysMsg.payload);
+						handleSysMessage(sysMsg.header, sysMsg.payload);
-#else 
+#else
-							if (!handleSysMessage(sysMsg.header, sysMsg.payload)){
+						if (!handleSysMessage(sysMsg.header, sysMsg.payload))
+						{
 
-								// Append to the correct thread's queue
+							// Append to the correct thread's queue
-								for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads()){
+							for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads())
-									if (thread->getCanID() == sysMsg.header.sid){
+							{
-										thread->CAN_queue_.put(sysMsg);
+								if (thread->getCanID() == sysMsg.header.sid)
-										break;
+								{
-									}
+									thread->CAN_queue_.put(sysMsg);
+									break;
 								}
 							}
-#endif // STA_CAN_BUS_FWD_ENABLE
 						}
-                    }
+#endif // STA_CAN_BUS_FWD_ENABLE
-                }
+					}
-            }
+				}
+			}
+		}
 
-        }
+		bool CanBus::queueCanBusMsg(const CanSysMsg msg, uint32_t timeout)
-
-		bool CanBus::queueCanBusMsg(const CanSysMsg& msg, uint32_t timeout)
 		{
 			// This technically should check if we are using a system message, but we just pretending that everything is one of those rn
-			//STA_ASSERT((msg.header.sid & ~STA_CAN_SID_SYS_BITS) == 0);
+			// STA_ASSERT((msg.header.sid & ~STA_CAN_SID_SYS_BITS) == 0);
 
 			if (canBusSysQueue_.put(msg, timeout))
 			{
@@ -101,61 +97,66 @@ namespace sta
 			}
 		}
 
-		void CanBus::canCallback(uint32_t fifo){
+		void CanBus::canCallback(uint32_t fifo)
-			if(messageEvent.get() != STA_RTOS_CAN_FLAG_MSG_AVAIL){
+		{
+			if (messageEvent.get() != STA_RTOS_CAN_FLAG_MSG_AVAIL)
+			{
 				// get here does not work since FreeRTOS is a buggy mess
 				messageEvent.set(STA_RTOS_CAN_FLAG_MSG_AVAIL);
 
-				CanRxHeader rxHeader;					//CAN Bus Receive Header
+				CanRxHeader rxHeader;						 // CAN Bus Receive Header
-				uint8_t canRX[8] = {0,0,0,0,0,0,0,0};  	//CAN Bus Receive Buffer
+				uint8_t canRX[8] = {0, 0, 0, 0, 0, 0, 0, 0}; // CAN Bus Receive Buffer
 
 				bool received_ = canBusController_->receiveFrame(fifo, &rxHeader, canRX);
 
-				if(received_){
+				if (received_ && rxHeader.id.sid <= 255)
+				{
 					CanSysMsg sysMsg;
 					sysMsg.header.sid = rxHeader.id.sid;
+					sysMsg.header.format = 0;
+
 					sysMsg.header.payloadLength = rxHeader.payloadLength;
 
-					for(int i = 0; i < rxHeader.payloadLength; i++){
+					for (int i = 0; i < rxHeader.payloadLength; i++)
+					{
 						sysMsg.payload[i] = canRX[i];
 					}
 
-					canBusSysQueueBuffer_[bufferIndex] = &sysMsg;
+					canBusRxQueue_.put(sysMsg, 0);
-
-					bufferIndex++;
-					if (bufferIndex >= STA_RTOS_CAN_BUS_QUEUE_LENGTH) bufferIndex = 0;
 				}
 			}
 		}
 
-		bool CanBus::getCanBusMsg(CanSysMsg * msg, uint32_t timeout)
+		bool CanBus::getCanBusMsg(CanSysMsg *msg, uint32_t timeout)
 		{
 			return (canBusSysQueue_.get(msg, timeout));
 		}
 
-		CanBus* CanBus::_instance = nullptr;
+		CanBus *CanBus::_instance = nullptr;
 
 		RtosEvent CanBus::messageEvent;
 
-    } /* namespace tacos */
+	} /* namespace tacos */
 } /* namespace sta */
 
+namespace sta
+{
 
-namespace sta {
+	void CanBus_RxPendingCallback(uint32_t fifo)
-
+	{
-	void CanBus_RxPendingCallback(uint32_t fifo){
+		sta::tacos::CanBus::instance()->canCallback(fifo);
-			sta::tacos::CanBus::instance()->canCallback(fifo);
 	}
 
 	namespace tacos
 	{
 		STA_WEAK
-		bool handleSysMessage(CanMsgHeader & header, uint8_t * payload)
+		bool 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
 
-			if(header.sid == STA_TACOS_CAN_BUS_SYS_MSG_ID){
+			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. Transition is forced

src/startup.cpp

@@ -124,7 +124,7 @@ namespace sta
 		}
 #endif // STA_TACOS_WATCHDOG_ENABLED
 
-#ifdef STA_CAN_BUS_ENABLE
+#ifdef STA_TACOS_CAN_BUS_ENABLED
 		/**
 		 * @brief Function that is called before the Can Bus task is started. Override it to adjust 
 		 * the Can bus to your specifications.
@@ -141,7 +141,7 @@ namespace sta
 
 			CanBus::instance()->start();
 		}
-#endif //STA_CAN_BUS_ENABLE
+#endif //STA_TACOS_CAN_BUS_ENABLED
 	}  // namespace tacos
 
 
@@ -162,9 +162,9 @@ namespace sta
 			tacos::initWatchdog();
 #endif // STA_TACOS_WATCHDOG_ENABLED
 
-#ifdef STA_CAN_BUS_ENABLE
+#ifdef STA_TACOS_CAN_BUS_ENABLED
 			tacos::initCanBus();
-#endif // STA_CAN_BUS_ENABLE
+#endif // STA_TACOS_CAN_BUS_ENABLED
 		}
 	} // namespace rtos
 } // namespace sta

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