From ecacb923f2bf9ba1f3033bf53b084ab514ca381b Mon Sep 17 00:00:00 2001 From: Henrik Stickann <4376396-Mithradir@users.noreply.gitlab.com> Date: Sat, 21 Jan 2023 22:18:16 +0100 Subject: [PATCH] Rework CAN bus system task --- include/sta/rtos/system/can_bus.hpp | 35 +- src/system/can_bus.cpp | 659 +++++++++++++++++----------- 2 files changed, 406 insertions(+), 288 deletions(-) diff --git a/include/sta/rtos/system/can_bus.hpp b/include/sta/rtos/system/can_bus.hpp index 0810ac3..63bd4fd 100644 --- a/include/sta/rtos/system/can_bus.hpp +++ b/include/sta/rtos/system/can_bus.hpp @@ -5,8 +5,6 @@ #ifndef STA_RTOS_SYSTEM_CAN_BUS_HPP #define STA_RTOS_SYSTEM_CAN_BUS_HPP -#include - /** * @defgroup STA_RTOS_CanBus CAN driver @@ -25,30 +23,12 @@ # define STA_RTOS_CAN_BUS_ENABLE #endif // DOXYGEN -/** - * @def STA_RTOS_CAN_BUS_TASK_NAME - * @brief Set name of CAN driver task. - * - * @ingroup STA_RTOS_BuildConfig - */ -#ifndef STA_RTOS_CAN_BUS_TASK_NAME -# define STA_RTOS_CAN_BUS_TASK_NAME canBus -#endif // !STA_RTOS_CAN_BUS_TASK_NAME - -/** - * @def STA_RTOS_CAN_BUS_ENTRY_FUNCTION - * @brief Set name of CAN driver task entry function. - * - * @ingroup STA_RTOS_BuildConfig - */ -#ifndef STA_RTOS_CAN_BUS_ENTRY_FUNCTION -# define STA_RTOS_CAN_BUS_ENTRY_FUNCTION STA_RTOS_MAKE_ENTRY_NAME(STA_RTOS_CAN_BUS_TASK_NAME) -#endif // !STA_RTOS_CAN_BUS_ENTRY_FUNCTION - #include #ifdef STA_RTOS_CAN_BUS_ENABLE + +#include #include #include @@ -121,13 +101,18 @@ namespace sta * @{ */ + /** + * @brief Initialize CAN bus. + */ + void initCanBus(); + /** - * @brief Extra initialization run at start of CAN bus task. + * @brief Return CanController for use in CAN system task. * - * May be overridden by application if required. + * Implementation must be provided by application. */ - void setupCanBus(); + extern CanController * getCanController(); diff --git a/src/system/can_bus.cpp b/src/system/can_bus.cpp index 6451d56..c75412d 100644 --- a/src/system/can_bus.cpp +++ b/src/system/can_bus.cpp @@ -6,61 +6,101 @@ #ifdef STA_RTOS_CAN_BUS_ENABLE #include -#include #include +#include #include - -#include -#include - #include #include - #include #include #include -#include - -#include - -#include +#include #include +#include #include -#define STA_RTOS_MAKE_DATA_QUEUE_NAME(name) _STA_RTOS_CONCAT(name, DataQueue) -#define STA_RTOS_MAKE_SYS_QUEUE_NAME(name) _STA_RTOS_CONCAT(name, SysQueue) +namespace +{ + StaticTask_t canBusCB; + StaticQueue_t canBusDataQueueCB; + StaticQueue_t canBusSysQueueCB; + + osThreadId_t canBusTaskHandle = nullptr; + osMessageQueueId_t canBusDataQueueHandle = nullptr; + osMessageQueueId_t canBusSysQueueHandle = nullptr; + + sta::CanController * canBusController = nullptr; + + const size_t queueLength = 8; + + // Static memory buffers + CanDataMsg canBusDataQueueBuffer[queueLength]; + CanSysMsg canBusSysQueueBuffer[queueLength]; + uint32_t canBusStack[256]; +} -#define STA_RTOS_CAN_BUS_THREAD STA_RTOS_MAKE_HANDLE_NAME(STA_RTOS_CAN_BUS_TASK_NAME) -#define STA_RTOS_CAN_BUS_DATA_QUEUE STA_RTOS_MAKE_HANDLE_NAME(STA_RTOS_MAKE_DATA_QUEUE_NAME(STA_RTOS_CAN_BUS_TASK_NAME)) -#define STA_RTOS_CAN_BUS_SYS_QUEUE STA_RTOS_MAKE_HANDLE_NAME(STA_RTOS_MAKE_SYS_QUEUE_NAME(STA_RTOS_CAN_BUS_TASK_NAME)) - - -// Access handles from freertos.c -extern osThreadId_t STA_RTOS_CAN_BUS_THREAD; -extern osMessageQueueId_t STA_RTOS_CAN_BUS_DATA_QUEUE; -extern osMessageQueueId_t STA_RTOS_CAN_BUS_SYS_QUEUE; +extern "C" void canBusTask(void *); namespace sta { namespace rtos { - extern CanController * CanBusController; + void initCanBus() + { + // Create thread using static allocation + const osThreadAttr_t taskAttributes = { + .name = "sysCanBus", + .cb_mem = &canBusCB, + .cb_size = sizeof(canBusCB), + .stack_mem = &canBusStack[0], + .stack_size = sizeof(canBusStack), + .priority = (osPriority_t) osPriorityLow, + }; + + canBusTaskHandle = osThreadNew(canBusTask, NULL, &taskAttributes); + STA_ASSERT_MSG(canBusTaskHandle != nullptr, "System CAN task initialization failed"); - STA_WEAK - void setupCanBus() - {} + // Create message queues using static allocation + const osMessageQueueAttr_t dataQueueAttributes = { + .name = "sysCanDataOut", + .attr_bits = 0, + .cb_mem = &canBusDataQueueCB, + .cb_size = sizeof(canBusDataQueueCB), + .mq_mem = &canBusDataQueueBuffer, + .mq_size = sizeof(canBusDataQueueBuffer) + }; + + canBusDataQueueHandle = osMessageQueueNew(queueLength, sizeof(CanDataMsg), &dataQueueAttributes); + STA_ASSERT_MSG(canBusDataQueueHandle != nullptr, "System CAN data message queue initialization failed"); + + const osMessageQueueAttr_t sysQueueAttributes = { + .name = "sysCanSysOut", + .attr_bits = 0, + .cb_mem = &canBusSysQueueCB, + .cb_size = sizeof(canBusSysQueueCB), + .mq_mem = &canBusSysQueueBuffer, + .mq_size = sizeof(canBusSysQueueBuffer) + }; + + canBusSysQueueHandle = osMessageQueueNew(queueLength, sizeof(CanSysMsg), &sysQueueAttributes); + STA_ASSERT_MSG(canBusSysQueueHandle != nullptr, "System CAN system message queue initialization failed"); + + + // Get initialized CAN controller from application + canBusController = getCanController(); + } void notifyCanBus(uint32_t flags) { // Send flags to thread - osThreadFlagsSet(STA_RTOS_CAN_BUS_THREAD, flags); + osThreadFlagsSet(canBusTaskHandle, flags); } @@ -69,10 +109,10 @@ namespace sta STA_ASSERT((msg.header.sid & STA_CAN_SID_SYS_BITS) == 0); STA_ASSERT(msg.header.payloadLength <= sizeof(msg.payload)); - if (osOK == osMessageQueuePut(STA_RTOS_CAN_BUS_DATA_QUEUE, &msg, 0, timeout)) + if (osOK == osMessageQueuePut(canBusDataQueueHandle, &msg, 0, timeout)) { // Signal thread - osThreadFlagsSet(STA_RTOS_CAN_BUS_THREAD, STA_RTOS_CAN_FLAG_DATA_QUEUED); + osThreadFlagsSet(canBusTaskHandle, STA_RTOS_CAN_FLAG_DATA_QUEUED); return true; } else @@ -85,10 +125,10 @@ namespace sta { STA_ASSERT((msg.header.sid & ~STA_CAN_SID_SYS_BITS) == 0); - if (osOK == osMessageQueuePut(STA_RTOS_CAN_BUS_SYS_QUEUE, &msg, 0, timeout)) + if (osOK == osMessageQueuePut(canBusSysQueueHandle, &msg, 0, timeout)) { // Signal thread - osThreadFlagsSet(STA_RTOS_CAN_BUS_THREAD, STA_RTOS_CAN_FLAG_SYS_QUEUED); + osThreadFlagsSet(canBusTaskHandle, STA_RTOS_CAN_FLAG_SYS_QUEUED); return true; } else @@ -100,70 +140,19 @@ namespace sta bool getCanBusMsg(CanDataMsg * msg, uint32_t timeout) { - return (osOK == osMessageQueueGet(STA_RTOS_CAN_BUS_DATA_QUEUE, msg, 0, timeout)); + return (osOK == osMessageQueueGet(canBusDataQueueHandle, msg, 0, timeout)); } bool getCanBusMsg(CanSysMsg * msg, uint32_t timeout) { - return (osOK == osMessageQueueGet(STA_RTOS_CAN_BUS_SYS_QUEUE, msg, 0, timeout)); + return (osOK == osMessageQueueGet(canBusSysQueueHandle, msg, 0, timeout)); } } // namespace rtos } // namespace sta -using namespace sta; - - -/**< ISOTP CAN transmitter */ -IsotpTransmitter gCanTx(rtos::CanBusController, HAL_GetTick); -/**< ISOTP CAN receiver */ -IsotpReceiver gCanRx(rtos::CanBusController, HAL_GetTick); - - -CanRxCallback filterCallbacks[STA_RTOS_CAN_BUS_MAX_FILTER]; - - namespace debug { - /** - * @brief Display ISOTP TX/RX statistics. - */ - void showStatistics() - { - STA_DEBUG_PRINTLN(); - STA_DEBUG_PRINTLN("# ######################"); - STA_DEBUG_PRINTLN("# ## ISOTP statistics ##"); - STA_DEBUG_PRINTLN("# ######################"); - STA_DEBUG_PRINTLN("#"); - - STA_DEBUG_PRINTLN("# Transmitter"); - STA_DEBUG_PRINT("# messages: "); - STA_DEBUG_PRINTLN(gCanTx.stats().messages); - STA_DEBUG_PRINT("# blocks: "); - STA_DEBUG_PRINTLN(gCanTx.stats().blocks); - STA_DEBUG_PRINT("# frames: "); - STA_DEBUG_PRINTLN(gCanTx.stats().frames); - STA_DEBUG_PRINT("# timeouts: "); - STA_DEBUG_PRINTLN(gCanTx.stats().timeouts); - STA_DEBUG_PRINTLN("#"); - - STA_DEBUG_PRINTLN("# Receiver"); - STA_DEBUG_PRINT("# messages: "); - STA_DEBUG_PRINTLN(gCanRx.stats().messages); - STA_DEBUG_PRINT("# blocks: "); - STA_DEBUG_PRINTLN(gCanRx.stats().blocks); - STA_DEBUG_PRINT("# frames: "); - STA_DEBUG_PRINTLN(gCanRx.stats().frames); - STA_DEBUG_PRINT("# timeouts: "); - STA_DEBUG_PRINTLN(gCanRx.stats().timeouts); - STA_DEBUG_PRINT("# flow control errors: "); - STA_DEBUG_PRINTLN(gCanRx.stats().flowErrors); - STA_DEBUG_PRINT("# overflows: "); - STA_DEBUG_PRINTLN(gCanRx.stats().overflows); - STA_DEBUG_PRINTLN(); - } - - /** * @brief Output CAN frame ID to UART. * @@ -200,213 +189,357 @@ namespace debug } // namespace debug -namespace demo +namespace dummy { - extern void handleRxMessage(const uint8_t * buffer, uint16_t size); -} // namespace demo - - -/** - * @brief Process received ISOTP messages. - * - * @param msg ISOTP message - */ -void handleRxMessage(const sta::IsotpMessage & msg) -{ - STA_DEBUG_PRINTLN("[event] RX message"); - - debug::printFrameID(msg.frameID); - - // TODO Forward message to other threads - demo::handleRxMessage(msg.buffer, msg.size); -} - - -/** - * @brief Handle received data message CAN frames. - * - * @param header CAN frame header - * @param payload Payload buffer - */ -void receiveDataCallback(const sta::CanRxHeader & header, const uint8_t * payload) -{ - // Write frame payload to DebugSerial - STA_DEBUG_PRINTLN("[event] RX data frame"); - debug::printPayloadHex(payload, header.payloadLength); - - // Process RX frame - auto handle = gCanRx.processFrame(header, payload); - - if (handle != sta::IsotpReceiver::INVALID_HANDLE) + void handleSysMessage(const sta::CanRxHeader & header, const uint8_t * payload) { - // Get message if completed - sta::IsotpMessage msg; - if (gCanRx.getMessage(handle, &msg)) - { - handleRxMessage(msg); - } + // Write frame payload to DebugSerial + STA_DEBUG_PRINTLN("[event] RX sys frame"); - // Handle FC responses - gCanRx.processFC(handle); + debug::printFrameID(header.id); + debug::printPayloadHex(payload, header.payloadLength); + + // TODO Forward message to other threads } - // Process TX frame - gCanTx.processFrame(header, payload); -} - -/** - * @brief Handle received system message CAN frames. - * - * @param header CAN frame header - * @param payload Payload buffer - */ -void receiveSysCallback(const sta::CanRxHeader & header, const uint8_t * payload) -{ - // Write frame payload to DebugSerial - STA_DEBUG_PRINTLN("[event] RX sys frame"); - - debug::printFrameID(header.id); - debug::printPayloadHex(payload, header.payloadLength); - - // TODO Forward message to other threads -} - - -/** - * @brief Configure CAN filters. - */ -void setupCanSubscriptions() -{ - // Make sure to receive all messages - CanFilter filter; - - // Clear previous subscriptions - rtos::CanBusController->clearFilters(); - - - // All bits except [0:1] of the SID must be zero for system messages - filter.obj = {0, 0}; - filter.mask = {~STA_CAN_SID_SYS_BITS, 0}; - filter.type = sta::CanFilterIdFormat::ANY; - filter.fifo = 0; - - filterCallbacks[0] = receiveSysCallback; - rtos::CanBusController->configureFilter(0, filter, true); - - - // TODO Limit which data messages are received - // Bits [0:1] of the SID must be zero for data messages - filter.obj = {0, 0}; - filter.mask = {STA_CAN_SID_SYS_BITS, 0}; - filter.type = sta::CanFilterIdFormat::ANY; - filter.fifo = 1; - filterCallbacks[1] = receiveDataCallback; - rtos::CanBusController->configureFilter(1, filter, true); -} - - -/** - * @brief Process received CAN messages. - */ -void processMessages() -{ - for (auto fifo : rtos::CanBusController->getPendingRxFifos()) + void handleDataMessage(const sta::IsotpMessage & msg) { - CanRxHeader header; - uint8_t payload[STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE]; + STA_ASSERT(msg.buffer != nullptr); + STA_ASSERT(msg.size != 0); - if (rtos::CanBusController->receiveFrame(fifo, &header, payload)) + STA_DEBUG_PRINTLN("[event] RX data message"); + + debug::printFrameID(msg.frameID); + + // TODO Forward message to other threads + +// if (buffer[0] == DEMO_BMP_PACKET_ID) +// { +// BmpPacket packet; +// if (unpack(buffer + 1, size - 1, &packet)) +// { +// STA_DEBUG_PRINTLN(); +// STA_DEBUG_PRINTLN("# ############"); +// STA_DEBUG_PRINTLN("# ## BMP380 ##"); +// STA_DEBUG_PRINTLN("# ############"); +// STA_DEBUG_PRINTLN("#"); +// +// STA_DEBUG_PRINT("# temperature: "); +// STA_DEBUG_PRINT(packet.temperature); +// STA_DEBUG_PRINTLN(" *C"); +// STA_DEBUG_PRINT("# pressure: "); +// STA_DEBUG_PRINT(packet.pressure); +// STA_DEBUG_PRINTLN(" Pa"); +// STA_DEBUG_PRINT("# altitude: "); +// STA_DEBUG_PRINT(packet.altitude); +// STA_DEBUG_PRINTLN(" m"); +// STA_DEBUG_PRINTLN(); +// } +// else +// { +// STA_DEBUG_PRINTLN("[error] BMP unpack failed"); +// } +// } +// else { -// debug::displayFrameUART(frame); + STA_DEBUG_PRINT("ID: "); + STA_DEBUG_PRINTLN(msg.buffer[0], sta::IntegerBase::HEX); + STA_DEBUG_PRINT("size: "); + STA_DEBUG_PRINTLN(msg.size); + } + } +} // namespace dummy - // Forward frame to filter callback - if (fifo <= STA_RTOS_CAN_BUS_MAX_FILTER && filterCallbacks[header.filter]) + + +namespace sta +{ + class AlpakaCanBus + { + public: + using SysMsgHandler = void (*)(const CanRxHeader &, const uint8_t *); + using DataMsgHandler = void (*)(const IsotpMessage &); + + static const uint8_t FIFO_SYS = 0; + static const uint8_t FIFO_DATA = 1; + + public: + AlpakaCanBus(CanController * controller, TimeMsFn timeMs, SysMsgHandler sysMsgHandler, DataMsgHandler dataMsgHandler); + + + /** + * @brief Send system message. + * + * @param msg Message + */ + void send(const CanSysMsg & msg); + + /** + * @brief Send data message. + * + * @param msg Message + */ + void send(const CanDataMsg & msg); + + + /** + * @brief Process transmissions. + * + * Call regularly to advance transmission. + */ + void processTx(); + + /** + * @brief Process received CAN messages. + */ + void processRx(); + + /** + * @brief Display ISOTP TX/RX statistics. + */ + void showStatistics(); + + private: + /** + * @brief Configure CAN filters. + */ + void setupSubscriptions(); + + /** + * @brief Handle received data message CAN frames. + * + * @param header CAN frame header + * @param payload Payload buffer + */ + void receiveDataFrame(const CanRxHeader & header, const uint8_t * payload); + + private: + CanController * controller_; + IsotpTransmitter tx_; + IsotpReceiver rx_; + SysMsgHandler handleSysMsg_; + DataMsgHandler handleDataMsg_; + }; + + + AlpakaCanBus::AlpakaCanBus(CanController * controller, TimeMsFn timeMs, SysMsgHandler sysMsgHandler, DataMsgHandler dataMsgHandler) + : controller_{controller}, tx_{controller, timeMs}, rx_{controller, timeMs}, handleSysMsg_{sysMsgHandler}, handleDataMsg_{dataMsgHandler} + { + STA_ASSERT(handleSysMsg_ != nullptr); + STA_ASSERT(handleDataMsg_ != nullptr); + + setupSubscriptions(); + } + + + void AlpakaCanBus::send(const CanSysMsg & msg) + { + CanTxHeader header; + header.id.format = static_cast(msg.header.format); + header.id.sid = msg.header.sid & STA_CAN_SID_SYS_BITS; + header.id.eid = msg.header.eid; + header.payloadLength = msg.header.payloadLength; + + controller_->sendFrame(header, msg.payload); + } + + void AlpakaCanBus::send(const CanDataMsg & msg) + { + CanFrameId frameID; + frameID.format = static_cast(msg.header.format); + frameID.sid = msg.header.sid & ~STA_CAN_SID_SYS_BITS; + frameID.eid = msg.header.eid; + + // Start transmission via ISO-TP + tx_.send(frameID, msg.payload, msg.header.payloadLength); + } + + + inline void AlpakaCanBus::processTx() + { + tx_.process(); + } + + + void AlpakaCanBus::processRx() + { + for (auto fifo : controller_->getPendingRxFifos()) + { + CanRxHeader header; + uint8_t payload[STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE]; + + if (controller_->receiveFrame(fifo, &header, payload)) { - filterCallbacks[header.filter](header, payload); +// debug::displayFrameUART(frame); + + // Forward frame to callback + switch (fifo) + { + case FIFO_SYS: + handleSysMsg_(header, payload); + break; + + case FIFO_DATA: + receiveDataFrame(header, payload); + break; + + default: + STA_ASSERT(false); + } } } } -} - -extern "C" -{ - /** - * @brief CAN driver thread entry function. - */ - void canBusTask(void *) + void AlpakaCanBus::showStatistics() { - using namespace sta; + STA_DEBUG_PRINTLN(); + STA_DEBUG_PRINTLN("# ######################"); + STA_DEBUG_PRINTLN("# ## ISOTP statistics ##"); + STA_DEBUG_PRINTLN("# ######################"); + STA_DEBUG_PRINTLN("#"); - // Initialize CAN controller - rtos::setupCanBus(); + const auto & txStats = tx_.stats(); + STA_DEBUG_PRINTLN("# Transmitter"); + STA_DEBUG_PRINT("# messages: "); + STA_DEBUG_PRINTLN(txStats.messages); + STA_DEBUG_PRINT("# blocks: "); + STA_DEBUG_PRINTLN(txStats.blocks); + STA_DEBUG_PRINT("# frames: "); + STA_DEBUG_PRINTLN(txStats.frames); + STA_DEBUG_PRINT("# timeouts: "); + STA_DEBUG_PRINTLN(txStats.timeouts); + STA_DEBUG_PRINTLN("#"); - // Configure filters for - setupCanSubscriptions(); + const auto & rxStats = rx_.stats(); + STA_DEBUG_PRINTLN("# Receiver"); + STA_DEBUG_PRINT("# messages: "); + STA_DEBUG_PRINTLN(rxStats.messages); + STA_DEBUG_PRINT("# blocks: "); + STA_DEBUG_PRINTLN(rxStats.blocks); + STA_DEBUG_PRINT("# frames: "); + STA_DEBUG_PRINTLN(rxStats.frames); + STA_DEBUG_PRINT("# timeouts: "); + STA_DEBUG_PRINTLN(rxStats.timeouts); + STA_DEBUG_PRINT("# flow control errors: "); + STA_DEBUG_PRINTLN(rxStats.flowErrors); + STA_DEBUG_PRINT("# overflows: "); + STA_DEBUG_PRINTLN(rxStats.overflows); + STA_DEBUG_PRINTLN(); + } - rtos::waitForStartupEvent(); + void AlpakaCanBus::setupSubscriptions() + { + // Make sure to receive all messages + CanFilter filter; - while (true) + // Clear previous subscriptions + controller_->clearFilters(); + + + // All bits except [0:1] of the SID must be zero for system messages + filter.obj = {0, 0}; + filter.mask = {~STA_CAN_SID_SYS_BITS, 0}; + filter.type = sta::CanFilterIdFormat::ANY; + filter.fifo = FIFO_SYS; + controller_->configureFilter(FIFO_SYS, filter, true); + + // TODO Limit which data messages are received + // Bits [0:1] of the SID must be zero for data messages + filter.obj = {0, 0}; + filter.mask = {STA_CAN_SID_SYS_BITS, 0}; + filter.type = sta::CanFilterIdFormat::ANY; + filter.fifo = FIFO_DATA; + controller_->configureFilter(FIFO_DATA, filter, true); + } + + void AlpakaCanBus::receiveDataFrame(const CanRxHeader & header, const uint8_t * payload) + { + // Write frame payload to DebugSerial + STA_DEBUG_PRINTLN("[event] RX data frame"); + debug::printPayloadHex(payload, header.payloadLength); + + // Process RX frame + auto handle = rx_.processFrame(header, payload); + + if (handle != IsotpReceiver::INVALID_HANDLE) { - uint32_t flags = osThreadFlagsWait(STA_RTOS_THREAD_FLAGS_VALID_BITS, osFlagsWaitAny, 50); - - if (flags != static_cast(osErrorTimeout)) + // Get message if completed + IsotpMessage msg; + if (rx_.getMessage(handle, &msg)) { - STA_ASSERT_MSG((flags & osStatusReserved) == flags, "Unexpected error occurred in wait"); + handleDataMsg_(msg); + } - if (flags & STA_RTOS_CAN_FLAG_SYS_QUEUED) + // Handle FC responses + rx_.processFC(handle); + } + + // Process TX frame + tx_.processFrame(header, payload); + } +} // namespace sta + + + +/** + * @brief CAN driver thread entry function. + */ +void canBusTask(void *) +{ + using namespace sta; + + STA_ASSERT_MSG(canBusController != nullptr, "System CAN bus not initialized"); + + // Setup ISO-TP transceiver + AlpakaCanBus canBus(canBusController, HAL_GetTick, dummy::handleSysMessage, dummy::handleDataMessage); + + + rtos::waitForStartupEvent(); + + while (true) + { + uint32_t flags = osThreadFlagsWait(STA_RTOS_THREAD_FLAGS_VALID_BITS, osFlagsWaitAny, 50); + + if (flags != static_cast(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)) { - CanSysMsg msg; - CanTxHeader header; - - // Take messages from queue until empty - while (rtos::getCanBusMsg(&msg, 0)) - { - header.id.format = static_cast(msg.header.format); - header.id.sid = msg.header.sid & STA_CAN_SID_SYS_BITS; - header.id.eid = msg.header.eid; - header.payloadLength = msg.header.payloadLength; - - debug::printPayloadHex(msg.payload, header.payloadLength); - - rtos::CanBusController->sendFrame(header, msg.payload); - } - } - - if (flags & STA_RTOS_CAN_FLAG_DATA_QUEUED) - { - CanDataMsg msg; - CanFrameId frameID; - - // Take messages from queue until empty - while (rtos::getCanBusMsg(&msg, 0)) - { - frameID.format = static_cast(msg.header.format); - frameID.sid = msg.header.sid & ~STA_CAN_SID_SYS_BITS; - frameID.eid = msg.header.eid; - - // Transmit via ISO-TP - gCanTx.send(frameID, msg.payload, msg.header.payloadLength); - } - } - - if (flags & STA_RTOS_CAN_FLAG_MSG_AVAIL) - { - STA_DEBUG_PRINTLN("[event] CAN INT"); - - processMessages(); - } - - if (flags & STA_RTOS_CAN_FLAG_SHOW_STATS) - { - debug::showStatistics(); + canBus.send(msg); } } - // Process ISOTP transmissions - gCanTx.process(); + 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(); } }