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

fix(CAN): RX buffer as queue for stability

Browse Source
This commit is contained in:
CarlWachter 2024-09-25 17:21:52 +02:00
parent 5b532087c8
commit 7997a8a91a
2 changed files with 79 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/sta/tacos/can_bus.hpp
View File

@ -100,10 +100,8 @@ namespace sta
sta::STM32CanController * canBusController_; 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_; AlpakaCanBus canBus_;

153
src/can_bus.cpp
View File

@ -16,77 +16,73 @@ namespace sta
: TacosThread{"Can Bus", STA_TACOS_CAN_BUS_PRIORITY}, : TacosThread{"Can Bus", STA_TACOS_CAN_BUS_PRIORITY},
canBusController_(new STM32CanController(&STA_STM32_CAN_HANDLE)), canBusController_(new STM32CanController(&STA_STM32_CAN_HANDLE)),
canBusSysQueue_(STA_RTOS_CAN_BUS_QUEUE_LENGTH), 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 #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 // 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)) 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 // get here does not work since FreeRTOS is a buggy mess
messageEvent.set(STA_RTOS_CAN_FLAG_MSG_AVAIL); 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); bool received_ = canBusController_->receiveFrame(fifo, &rxHeader, canRX);
if(received_){ if (received_ && rxHeader.id.sid <= 255)
{
CanSysMsg sysMsg; CanSysMsg sysMsg;
sysMsg.header.sid = rxHeader.id.sid; sysMsg.header.sid = rxHeader.id.sid;
sysMsg.header.format = 0;
sysMsg.header.payloadLength = rxHeader.payloadLength; 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]; sysMsg.payload[i] = canRX[i];
} }
canBusSysQueueBuffer_[bufferIndex] = &sysMsg; canBusRxQueue_.put(sysMsg);
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)); return (canBusSysQueue_.get(msg, timeout));
} }
CanBus* CanBus::_instance = nullptr; CanBus *CanBus::_instance = nullptr;
RtosEvent CanBus::messageEvent; RtosEvent CanBus::messageEvent;
} /* namespace tacos */ } /* namespace tacos */
} /* namespace sta */ } /* 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 namespace tacos
{ {
STA_WEAK 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 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); STA_ASSERT(header.payloadLength == 2);
// First byte of payload is the origin state, second byte is the destination state. Transition is forced // First byte of payload is the origin state, second byte is the destination state. Transition is forced