Rebase onto main

src/can_bus.cpp

@@ -0,0 +1,115 @@
+#include <sta/tacos/can_bus.hpp>
+#include <sta/debug/debug.hpp>
+#include <sta/debug/assert.hpp>
+
+namespace sta
+{
+	namespace tacos
+	{
+		CanBus::CanBus(CAN_HandleTypeDef * handle)
+			: TacosThread{"Can Bus", STA_TACOS_CAN_BUS_PRIORITY},
+			  canBusController_{handle},
+              canBusDataQueueBuffer{nullptr},
+              canBusSysQueueBuffer{nullptr},
+			  canSysDataQueue_{STA_RTOS_CAN_BUS_QUEUE_LENGTH},
+			  canBusDataQueue_{STA_RTOS_CAN_BUS_QUEUE_LENGTH},
+              canBus_{&canBusController_, HAL_GetTick, dummy::handleSysMessage, dummy::handleDataMessage}
+		{
+		}
+
+        void CanBus::init()
+	    {
+		    canBusController_.start();
+	    }
+
+        void CanBus::func()
+        {
+            uint32_t flags = osThreadFlagsWait(STA_RTOS_THREAD_FLAGS_VALID_BITS, osFlagsWaitAny, 50);
+
+            if (flags != static_cast<uint32_t>(osErrorTimeout))
+            {
+                STA_ASSERT_MSG((flags & osStatusReserved) == flags, "Unexpected error occurred in wait");
+
+                if (flags & STA_RTOS_CAN_FLAG_SYS_QUEUED)
+                {
+                    // Take messages from queue until empty
+                    CanSysMsg msg;
+                    while (rtos::getCanBusMsg(&msg, 0))
+                    {
+                        canBus_.send(msg);
+                    }
+                }
+
+                if (flags & STA_RTOS_CAN_FLAG_DATA_QUEUED)
+                {
+                    // Take messages from queue until empty
+                    CanDataMsg msg;
+                    while (rtos::getCanBusMsg(&msg, 0))
+                    {
+                        canBus_.send(msg);
+                    }
+                }
+
+                if (flags & STA_RTOS_CAN_FLAG_MSG_AVAIL)
+                {
+                    STA_DEBUG_PRINTLN("[event] CAN INT");
+
+                    canBus_.processRx();
+                }
+
+                if (flags & STA_RTOS_CAN_FLAG_SHOW_STATS)
+                {
+                    canBus_.showStatistics();
+                }
+            }
+
+            // Process ISOTP transmissions
+            canBus.processTx();
+        }
+
+        bool queueCanBusMsg(const CanDataMsg & msg, uint32_t timeout)
+		{
+			STA_ASSERT((msg.header.sid & STA_CAN_SID_SYS_BITS) == 0);
+			STA_ASSERT(msg.header.payloadLength <= sizeof(msg.payload));
+
+			if (canBusDataQueue_.put(&msg, timeout))
+			{
+				// Signal thread
+				notify(STA_RTOS_CAN_FLAG_DATA_QUEUED);
+				return true;
+			}
+			else
+			{
+				return false;
+			}
+		}
+
+		bool queueCanBusMsg(const CanSysMsg & msg, uint32_t timeout)
+		{
+			STA_ASSERT((msg.header.sid & ~STA_CAN_SID_SYS_BITS) == 0);
+
+			if (canBusSysQueue_.put(&msg, timeout))
+			{
+				// Signal thread
+				notify(STA_RTOS_CAN_FLAG_SYS_QUEUED);
+				return true;
+			}
+			else
+			{
+				return false;
+			}
+		}
+
+
+		bool getCanBusMsg(CanDataMsg * msg, uint32_t timeout)
+		{
+			return (osOK == osMessageQueueGet(canBusDataQueueHandle, msg, 0, timeout));
+		}
+
+		bool getCanBusMsg(CanSysMsg * msg, uint32_t timeout)
+		{
+			return (osOK == osMessageQueueGet(canBusSysQueueHandle, msg, 0, timeout));
+		}
+
+    } /* namespace tacos */
+} /* namespace sta */