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

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

@ -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> canBusRxQueue_;
AlpakaCanBus canBus_;

51
src/can_bus.cpp
View File

@ -16,12 +16,9 @@ namespace sta
: TacosThread{"Can Bus", STA_TACOS_CAN_BUS_PRIORITY},
canBusController_(new STM32CanController(&STA_STM32_CAN_HANDLE)),
canBusSysQueue_(STA_RTOS_CAN_BUS_QUEUE_LENGTH),
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()
@ -58,18 +55,19 @@ namespace sta
{
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;
while (canBusRxQueue_.get(&sysMsg, 0))
{
#ifndef STA_CAN_BUS_FWD_ENABLE
handleSysMessage(sysMsg.header, sysMsg.payload);
#else
if (!handleSysMessage(sysMsg.header, sysMsg.payload)){
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){
for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads())
{
if (thread->getCanID() == sysMsg.header.sid)
{
thread->CAN_queue_.put(sysMsg);
break;
}
@ -79,8 +77,6 @@ namespace sta
}
}
}
}
}
bool CanBus::queueCanBusMsg(const CanSysMsg &msg, uint32_t timeout)
@ -101,8 +97,10 @@ 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);
@ -111,19 +109,20 @@ namespace sta
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);
}
}
}
@ -140,10 +139,11 @@ namespace sta
} /* namespace tacos */
} /* namespace sta */
namespace sta
{
namespace sta {
void CanBus_RxPendingCallback(uint32_t fifo){
void CanBus_RxPendingCallback(uint32_t fifo)
{
sta::tacos::CanBus::instance()->canCallback(fifo);
}
@ -155,7 +155,8 @@ namespace sta {
// 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