mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/rtos2-utils.git
synced 2025-06-10 18:15:59 +00:00
Add moved CAN bus sys task
This commit is contained in:
Henrik Stickann
parent
b1539aab39
commit
15f79b0446
184
include/sta/rtos/system/can_bus.hpp
Normal file
184
include/sta/rtos/system/can_bus.hpp
Normal file
@ -0,0 +1,184 @@
|
||||
/**
|
||||
* @file
|
||||
* @brief Public interface for CAN driver thread.
|
||||
*/
|
||||
#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
|
||||
* @ingroup STA_RTOS_API
|
||||
* @brief CAN bus system task.
|
||||
*
|
||||
* Check @ref STA_RTOS_BuildConfig for configuration options.
|
||||
*/
|
||||
|
||||
#ifdef DOXYGEN
|
||||
/**
|
||||
* @brief Enable module.
|
||||
*
|
||||
* @ingroup STA_RTOS_BuildConfig
|
||||
*/
|
||||
# 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/rtos/system/can_msg.h>
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
|
||||
/**
|
||||
* @def STA_RTOS_CAN_BUS_MAX_FILTER
|
||||
* @brief Set maximum number of usable filters.
|
||||
*
|
||||
* @ingroup STA_RTOS_BuildConfig
|
||||
*/
|
||||
#ifndef STA_RTOS_CAN_BUS_MAX_FILTER
|
||||
# error "Must set STA_RTOS_CAN_BUS_MAX_FILTER in <sta/config.hpp>"
|
||||
#endif // STA_RTOS_CAN_BUS_MAX_FILTER
|
||||
|
||||
/**
|
||||
* @def STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE
|
||||
* @brief Set maximum payload size.
|
||||
*
|
||||
* @ingroup STA_RTOS_BuildConfig
|
||||
*/
|
||||
#ifndef STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE
|
||||
# error "Must set STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE in <sta/config.hpp>"
|
||||
#endif // STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE
|
||||
|
||||
|
||||
/**
|
||||
* @ingroup STA_RTOS_CanBus
|
||||
* @{
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* @brief CAN frame available.
|
||||
*/
|
||||
#define STA_RTOS_CAN_FLAG_MSG_AVAIL 0x000010U
|
||||
/**
|
||||
* @brief Send CAN message.
|
||||
*/
|
||||
#define STA_RTOS_CAN_FLAG_MSG_SEND 0x000020U
|
||||
/**
|
||||
* @brief CAN data message in queue.
|
||||
*/
|
||||
#define STA_RTOS_CAN_FLAG_DATA_QUEUED 0x000040U
|
||||
/**
|
||||
* @brief CAN system message in queue.
|
||||
*/
|
||||
#define STA_RTOS_CAN_FLAG_SYS_QUEUED 0x000080U
|
||||
/**
|
||||
* @brief Show ISOTP statistics.
|
||||
*/
|
||||
#define STA_RTOS_CAN_FLAG_SHOW_STATS 0x000100U
|
||||
|
||||
|
||||
/**
|
||||
* @brief CAN SID bits used for system messages.
|
||||
*/
|
||||
#define STA_CAN_SID_SYS_BITS UINT32_C(0x3)
|
||||
|
||||
|
||||
/** @} */
|
||||
|
||||
|
||||
namespace sta
|
||||
{
|
||||
namespace rtos
|
||||
{
|
||||
/**
|
||||
* @ingroup STA_RTOS_CanBus
|
||||
* @{
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* @brief Extra initialization run at start of CAN bus task.
|
||||
*
|
||||
* May be overridden by application if required.
|
||||
*/
|
||||
void setupCanBus();
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Send notification to CAN driver.
|
||||
*
|
||||
* @param flags Event flags
|
||||
*/
|
||||
void notifyCanBus(uint32_t flags);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Place data message in CAN driver TX queue.
|
||||
*
|
||||
* @param msg Message to transmit
|
||||
* @param timeout Timeout for placing message (0 = no wait, osWaitForever = blocking)
|
||||
* @return True if message was queued successfully
|
||||
*/
|
||||
bool queueCanBusMsg(const CanDataMsg & msg, uint32_t timeout);
|
||||
/**
|
||||
* @brief Place system message in CAN driver TX queue.
|
||||
*
|
||||
* @param msg Message to transmit
|
||||
* @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);
|
||||
|
||||
/**
|
||||
* @brief Retrieve data message from CAN driver TX queue.
|
||||
*
|
||||
* @param[out] msg Output address for retrieved message
|
||||
* @param timeout Timeout for retrieving message (0 = no wait, osWaitForever = blocking)
|
||||
* @return True if message was retrieved successfully
|
||||
*/
|
||||
bool getCanBusMsg(CanDataMsg * msg, uint32_t timeout);
|
||||
/**
|
||||
* @brief Retrieve system message from CAN driver TX queue.
|
||||
*
|
||||
* @param[out] msg Destination for retrieved message
|
||||
* @param timeout Timeout for retrieving message (0 = no wait, osWaitForever = blocking)
|
||||
* @return True if message was retrieved successfully
|
||||
*/
|
||||
bool getCanBusMsg(CanSysMsg * msg, uint32_t timeout);
|
||||
|
||||
|
||||
/** @} */
|
||||
} // namespace rtos
|
||||
} // namespace sta
|
||||
|
||||
|
||||
#endif // STA_RTOS_CAN_BUS_ENABLE
|
||||
|
||||
#endif // STA_RTOS_SYSTEM_CAN_BUS_HPP
|
||||
49
include/sta/rtos/system/can_msg.h
Normal file
49
include/sta/rtos/system/can_msg.h
Normal file
@ -0,0 +1,49 @@
|
||||
/**
|
||||
* @file
|
||||
* @brief CAN driver message request types for use in C code.
|
||||
*/
|
||||
#ifndef STA_RTOS_SYSTEM_CAN_MSG_H
|
||||
#define STA_RTOS_SYSTEM_CAN_MSG_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
/**
|
||||
* @brief CAN message header.
|
||||
*
|
||||
* @ingroup STA_RTOS_CanBus
|
||||
*/
|
||||
struct CanMsgHeader
|
||||
{
|
||||
uint32_t sid; /**< Message SID */
|
||||
uint32_t eid; /**< Message EID */
|
||||
uint8_t format; /**< Message ID format */
|
||||
uint8_t payloadLength; /**< Payload length */
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Element type for CAN data message queue.
|
||||
*
|
||||
* @ingroup STA_RTOS_CanBus
|
||||
*/
|
||||
struct CanDataMsg
|
||||
{
|
||||
struct CanMsgHeader header; /**< Message header data */
|
||||
uint8_t payload[64]; /**< Message payload */
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Element type for CAN system message queue.
|
||||
*
|
||||
* @ingroup STA_RTOS_CanBus
|
||||
*/
|
||||
struct CanSysMsg
|
||||
{
|
||||
struct CanMsgHeader header; /**< Message header data */
|
||||
uint8_t payload[8]; /**< Message payload */
|
||||
};
|
||||
|
||||
|
||||
#endif // STA_RTOS_SYSTEM_CAN_MSG_H
|
||||
408
src/system/can_bus.cpp
Normal file
408
src/system/can_bus.cpp
Normal file
@ -0,0 +1,408 @@
|
||||
/**
|
||||
* @file
|
||||
* @brief CAN driver thread.
|
||||
*/
|
||||
#include <sta/assert.hpp>
|
||||
#include <sta/debug_serial.hpp>
|
||||
#include <sta/can/subscribable.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 <cmsis_os2.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)
|
||||
|
||||
|
||||
#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;
|
||||
|
||||
|
||||
namespace sta
|
||||
{
|
||||
namespace rtos
|
||||
{
|
||||
extern CanController * CanBusController;
|
||||
|
||||
|
||||
STA_WEAK
|
||||
void setupCanBus()
|
||||
{}
|
||||
|
||||
|
||||
void notifyCanBus(uint32_t flags)
|
||||
{
|
||||
// Send flags to thread
|
||||
osThreadFlagsSet(STA_RTOS_CAN_BUS_THREAD, flags);
|
||||
}
|
||||
|
||||
|
||||
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 (osOK == osMessageQueuePut(STA_RTOS_CAN_BUS_DATA_QUEUE, &msg, 0, timeout))
|
||||
{
|
||||
// Signal thread
|
||||
osThreadFlagsSet(STA_RTOS_CAN_BUS_THREAD, 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 (osOK == osMessageQueuePut(STA_RTOS_CAN_BUS_SYS_QUEUE, &msg, 0, timeout))
|
||||
{
|
||||
// Signal thread
|
||||
osThreadFlagsSet(STA_RTOS_CAN_BUS_THREAD, STA_RTOS_CAN_FLAG_SYS_QUEUED);
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool getCanBusMsg(CanDataMsg * msg, uint32_t timeout)
|
||||
{
|
||||
return (osOK == osMessageQueueGet(STA_RTOS_CAN_BUS_DATA_QUEUE, msg, 0, timeout));
|
||||
}
|
||||
|
||||
bool getCanBusMsg(CanSysMsg * msg, uint32_t timeout)
|
||||
{
|
||||
return (osOK == osMessageQueueGet(STA_RTOS_CAN_BUS_SYS_QUEUE, 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.
|
||||
*
|
||||
* @param id Frame ID
|
||||
*/
|
||||
void printFrameID(const sta::CanFrameId & id)
|
||||
{
|
||||
STA_DEBUG_PRINT("SID: ");
|
||||
STA_DEBUG_PRINTLN(id.sid, sta::IntegerBase::HEX);
|
||||
if (id.format == sta::CanIdFormat::EXT)
|
||||
{
|
||||
STA_DEBUG_PRINT("EID: ");
|
||||
STA_DEBUG_PRINTLN(id.eid, sta::IntegerBase::HEX);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Output CAN frame payload to UART.
|
||||
*
|
||||
* @param payload Payload buffer
|
||||
* @param size Payload size
|
||||
*/
|
||||
void printPayloadHex(const uint8_t * payload, uint8_t size)
|
||||
{
|
||||
// Write frame payload to DebugSerial
|
||||
STA_DEBUG_PRINT("payload: ");
|
||||
for (uint8_t i = 0; i < size; ++i)
|
||||
{
|
||||
STA_DEBUG_PRINT(payload[i], sta::IntegerBase::HEX);
|
||||
STA_DEBUG_PRINT(' ');
|
||||
}
|
||||
STA_DEBUG_PRINTLN();
|
||||
}
|
||||
} // namespace debug
|
||||
|
||||
|
||||
namespace demo
|
||||
{
|
||||
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)
|
||||
{
|
||||
// Get message if completed
|
||||
sta::IsotpMessage msg;
|
||||
if (gCanRx.getMessage(handle, &msg))
|
||||
{
|
||||
handleRxMessage(msg);
|
||||
}
|
||||
|
||||
// Handle FC responses
|
||||
gCanRx.processFC(handle);
|
||||
}
|
||||
|
||||
// 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())
|
||||
{
|
||||
CanRxHeader header;
|
||||
uint8_t payload[STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE];
|
||||
|
||||
if (rtos::CanBusController->receiveFrame(fifo, &header, payload))
|
||||
{
|
||||
// debug::displayFrameUART(frame);
|
||||
|
||||
// Forward frame to filter callback
|
||||
if (fifo <= STA_RTOS_CAN_BUS_MAX_FILTER && filterCallbacks[header.filter])
|
||||
{
|
||||
filterCallbacks[header.filter](header, payload);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C"
|
||||
{
|
||||
/**
|
||||
* @brief CAN driver thread entry function.
|
||||
*/
|
||||
void canBusTask(void *)
|
||||
{
|
||||
using namespace sta;
|
||||
|
||||
// Initialize CAN controller
|
||||
rtos::setupCanBus();
|
||||
|
||||
// Configure filters for
|
||||
setupCanSubscriptions();
|
||||
|
||||
|
||||
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)
|
||||
{
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
// Process ISOTP transmissions
|
||||
gCanTx.process();
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user