ALPAKA/rtos2-utils
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/rtos2-utils.git synced 2025-06-10 18:15:59 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Rework CAN bus system task

Browse Source
This commit is contained in:
Henrik Stickann 2023-01-21 22:18:16 +01:00
parent d323d2ada5
commit ecacb923f2
2 changed files with 406 additions and 288 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
include/sta/rtos/system/can_bus.hpp
View File

@ -5,8 +5,6 @@
#ifndef STA_RTOS_SYSTEM_CAN_BUS_HPP
#define STA_RTOS_SYSTEM_CAN_BUS_HPP
#include <sta/rtos/system/names.hpp>
/**
* @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 <sta/config.hpp>
#ifdef STA_RTOS_CAN_BUS_ENABLE
#include <sta/can/controller.hpp>
#include <sta/rtos/system/can_msg.h>
#include <cstdint>
@ -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();

659
src/system/can_bus.cpp
View File

@ -6,61 +6,101 @@
#ifdef STA_RTOS_CAN_BUS_ENABLE
#include <sta/assert.hpp>
#include <sta/debug_serial.hpp>
#include <sta/can/subscribable.hpp>
#include <sta/debug_serial.hpp>
#include <sta/lang.hpp>
#include <sta/MCP2518FD/controller.hpp>
#include <sta/MCP2518FD/stm32/interrupt.hpp>
#include <sta/proto/isotp/transmitter.hpp>
#include <sta/proto/isotp/receiver.hpp>
#include <sta/rtos/defs.hpp>
#include <sta/rtos/system/can_bus.hpp>
#include <sta/rtos/system/system_event.hpp>
#include <sta/rtos/thread.hpp>
#include <sta/stm32/spi.hpp>
#include <spi.h>
#include <sta/stm32/hal.hpp>
#include <cmsis_os2.h>
#include <FreeRTOS.h>
#include <cstring>
#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<CanIdFormat>(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<CanIdFormat>(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<uint32_t>(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<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))
{
CanSysMsg msg;
CanTxHeader header;
// Take messages from queue until empty
while (rtos::getCanBusMsg(&msg, 0))
{
header.id.format = static_cast<CanIdFormat>(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<CanIdFormat>(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();
}
}