mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/TACOS.git
synced 2025-06-10 16:45:59 +00:00
fix(CAN): RX buffer as queue for stability
This commit is contained in:
CarlWachter
parent
5b532087c8
commit
7997a8a91a
@ -100,10 +100,8 @@ namespace sta
|
||||
|
||||
sta::STM32CanController * canBusController_;
|
||||
|
||||
CanSysMsg* canBusSysQueueBuffer_[STA_RTOS_CAN_BUS_QUEUE_LENGTH];
|
||||
uint8_t bufferIndex;
|
||||
|
||||
RtosQueue<CanSysMsg> canBusSysQueue_;
|
||||
RtosQueue<CanSysMsg> canBusSysQueue_;
|
||||
RtosQueue<CanSysMsg> canBusRxQueue_;
|
||||
|
||||
AlpakaCanBus canBus_;
|
||||
|
||||
|
||||
153
src/can_bus.cpp
153
src/can_bus.cpp
@ -16,77 +16,73 @@ namespace sta
|
||||
: TacosThread{"Can Bus", STA_TACOS_CAN_BUS_PRIORITY},
|
||||
canBusController_(new STM32CanController(&STA_STM32_CAN_HANDLE)),
|
||||
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()
|
||||
{
|
||||
canBusController_->start();
|
||||
void CanBus::func()
|
||||
{
|
||||
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 ||
|
||||
HAL_CAN_ActivateNotification(&STA_STM32_CAN_HANDLE, CAN_IT_RX_FIFO1_MSG_PENDING))
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
}
|
||||
if (flags != static_cast<uint32_t>(osErrorTimeout))
|
||||
{
|
||||
STA_ASSERT_MSG((flags & osStatusReserved) == flags, "Unexpected error occurred in wait");
|
||||
|
||||
void CanBus::func()
|
||||
{
|
||||
messageEvent.clear(STA_RTOS_CAN_ANY);
|
||||
uint32_t flags = messageEvent.wait(STA_RTOS_CAN_ANY, osWaitForever);
|
||||
if (flags & STA_RTOS_CAN_FLAG_SYS_QUEUED)
|
||||
{
|
||||
// Take messages from queue until empty
|
||||
CanSysMsg msg;
|
||||
while (CanBus::_instance->getCanBusMsg(&msg, 0))
|
||||
{
|
||||
canBus_.send(msg);
|
||||
}
|
||||
}
|
||||
|
||||
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 (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;
|
||||
if (flags & STA_RTOS_CAN_FLAG_MSG_AVAIL)
|
||||
{
|
||||
CanSysMsg sysMsg;
|
||||
// Iterate through buffer and set back to nullptr after use
|
||||
while (canBusRxQueue_.get(&sysMsg, 0))
|
||||
{
|
||||
#ifndef STA_CAN_BUS_FWD_ENABLE
|
||||
handleSysMessage(sysMsg.header, sysMsg.payload);
|
||||
#else
|
||||
if (!handleSysMessage(sysMsg.header, sysMsg.payload)){
|
||||
handleSysMessage(sysMsg.header, sysMsg.payload);
|
||||
#else
|
||||
if (!handleSysMessage(sysMsg.header, sysMsg.payload))
|
||||
{
|
||||
|
||||
// Append to the correct thread's queue
|
||||
for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads()){
|
||||
if (thread->getCanID() == sysMsg.header.sid){
|
||||
thread->CAN_queue_.put(sysMsg);
|
||||
break;
|
||||
}
|
||||
// Append to the correct thread's queue
|
||||
for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads())
|
||||
{
|
||||
if (thread->getCanID() == sysMsg.header.sid)
|
||||
{
|
||||
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
|
||||
//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))
|
||||
{
|
||||
@ -101,61 +97,66 @@ namespace sta
|
||||
}
|
||||
}
|
||||
|
||||
void CanBus::canCallback(uint32_t fifo){
|
||||
if(messageEvent.get() != STA_RTOS_CAN_FLAG_MSG_AVAIL){
|
||||
void CanBus::canCallback(uint32_t fifo)
|
||||
{
|
||||
if (messageEvent.get() != STA_RTOS_CAN_FLAG_MSG_AVAIL)
|
||||
{
|
||||
// get here does not work since FreeRTOS is a buggy mess
|
||||
messageEvent.set(STA_RTOS_CAN_FLAG_MSG_AVAIL);
|
||||
|
||||
CanRxHeader rxHeader; //CAN Bus Receive Header
|
||||
uint8_t canRX[8] = {0,0,0,0,0,0,0,0}; //CAN Bus Receive Buffer
|
||||
CanRxHeader rxHeader; // CAN Bus Receive Header
|
||||
uint8_t canRX[8] = {0, 0, 0, 0, 0, 0, 0, 0}; // CAN Bus Receive Buffer
|
||||
|
||||
bool received_ = canBusController_->receiveFrame(fifo, &rxHeader, canRX);
|
||||
|
||||
if(received_){
|
||||
if (received_ && rxHeader.id.sid <= 255)
|
||||
{
|
||||
CanSysMsg sysMsg;
|
||||
sysMsg.header.sid = rxHeader.id.sid;
|
||||
sysMsg.header.format = 0;
|
||||
|
||||
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];
|
||||
}
|
||||
|
||||
canBusSysQueueBuffer_[bufferIndex] = &sysMsg;
|
||||
|
||||
bufferIndex++;
|
||||
if (bufferIndex >= STA_RTOS_CAN_BUS_QUEUE_LENGTH) bufferIndex = 0;
|
||||
canBusRxQueue_.put(sysMsg);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool CanBus::getCanBusMsg(CanSysMsg * msg, uint32_t timeout)
|
||||
bool CanBus::getCanBusMsg(CanSysMsg *msg, uint32_t timeout)
|
||||
{
|
||||
return (canBusSysQueue_.get(msg, timeout));
|
||||
}
|
||||
|
||||
CanBus* CanBus::_instance = nullptr;
|
||||
CanBus *CanBus::_instance = nullptr;
|
||||
|
||||
RtosEvent CanBus::messageEvent;
|
||||
|
||||
} /* namespace tacos */
|
||||
} /* namespace tacos */
|
||||
} /* namespace sta */
|
||||
|
||||
namespace sta
|
||||
{
|
||||
|
||||
namespace sta {
|
||||
|
||||
void CanBus_RxPendingCallback(uint32_t fifo){
|
||||
sta::tacos::CanBus::instance()->canCallback(fifo);
|
||||
void CanBus_RxPendingCallback(uint32_t fifo)
|
||||
{
|
||||
sta::tacos::CanBus::instance()->canCallback(fifo);
|
||||
}
|
||||
|
||||
namespace tacos
|
||||
{
|
||||
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(header.sid == STA_TACOS_CAN_BUS_SYS_MSG_ID){
|
||||
if (header.sid == STA_TACOS_CAN_BUS_SYS_MSG_ID)
|
||||
{
|
||||
STA_ASSERT(header.payloadLength == 2);
|
||||
|
||||
// First byte of payload is the origin state, second byte is the destination state. Transition is forced
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user