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

Merge pull request 'CAN RX fix, avoid buffer overflows' (#39) from fix/canRx into main

Browse Source 
Reviewed-on: https://git.intern.spaceteamaachen.de/ALPAKA/TACOS/pulls/39
This commit is contained in:
carlwachter 2024-11-03 11:45:23 +00:00
commit 2d6faf8abc
7 changed files with 118 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/sta/tacos.hpp
View File

@ -46,7 +46,7 @@ namespace sta
*
* @ingroup tacos_api
*/
void setState(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false);
void setState(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false, bool publish = false);
/**
* @brief Request a state transition after a given time has passed. Invalid state transitions will be dismissed.
@ -58,7 +58,7 @@ namespace sta
*
* @ingroup tacos_api
*/
void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0);
void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0, bool publish = false);
/**
* @brief Register a new thread to be run by TACOS.
@ -97,13 +97,13 @@ namespace sta
*
* @param from The state we want to transition from.
* @param to The state we want to transition to.
* @param lockout An optional timer blocking state transition for a given time.
* @param timeout An optional timeout for the CAN communication
*
* @return bool True if the message was sent successfully.
*
* @ingroup tacos_api
*/
bool publishState(uint32_t from, uint32_t to, uint32_t lockout = 0);
bool publishState(uint32_t from, uint32_t to, uint32_t timeout = 0);
#endif // STA_TACOS_CAN_BUS_ENABLED
} // namespace tacos
}

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

@ -61,7 +61,7 @@ namespace sta
* @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);
bool queueCanBusMsg(const CanSysMsg msg, uint32_t timeout);
/**
* @brief Retrieve system message from CAN driver TX queue.
@ -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_;

8
include/sta/tacos/statemachine.hpp
View File

@ -119,7 +119,9 @@ namespace sta
/// Event that triggered the transition
EventFlags event;
/// Lockout time after transition
uint32_t lockout;
uint32_t lockout;
/// Whether to publish the transition via CAN.
bool publish = false;
};
/**
@ -164,7 +166,7 @@ namespace sta
* @param lockout The minimum number of milliseconds we expect to stay in this state. This is used to block premature transitions.
* @param force If true, the state transition will be executed regardless of the current state.
*/
void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false);
void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0, bool force = false, bool publish = true);
/**
* @brief Request a state transition after a given time has passed.
@ -174,7 +176,7 @@ namespace sta
* @param millis the number of milliseconds to wait before triggering the transition.
* @param lockout The minimum number of milliseconds we expect to stay in this state. This is used to block premature transitions.
*/
void requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0);
void requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0, bool publish = true);
void init() override;
void func() override;

153
src/can_bus.cpp
View File

@ -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, 0);
}
}
}
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

8
src/startup.cpp
View File

@ -124,7 +124,7 @@ namespace sta
}
#endif // STA_TACOS_WATCHDOG_ENABLED
#ifdef STA_CAN_BUS_ENABLE
#ifdef STA_TACOS_CAN_BUS_ENABLED
/**
* @brief Function that is called before the Can Bus task is started. Override it to adjust
* the Can bus to your specifications.
@ -141,7 +141,7 @@ namespace sta
CanBus::instance()->start();
}
#endif //STA_CAN_BUS_ENABLE
#endif //STA_TACOS_CAN_BUS_ENABLED
} // namespace tacos
@ -162,9 +162,9 @@ namespace sta
tacos::initWatchdog();
#endif // STA_TACOS_WATCHDOG_ENABLED
#ifdef STA_CAN_BUS_ENABLE
#ifdef STA_TACOS_CAN_BUS_ENABLED
tacos::initCanBus();
#endif // STA_CAN_BUS_ENABLE
#endif // STA_TACOS_CAN_BUS_ENABLED
}
} // namespace rtos
} // namespace sta

24
src/statemachine.cpp
View File

@ -5,6 +5,7 @@
* Author: Dario
*/
#include <sta/tacos.hpp>
#include <sta/tacos/statemachine.hpp>
#include <sta/debug/debug.hpp>
@ -40,9 +41,16 @@ namespace sta
{
STA_ASSERT(transition.to < STA_TACOS_NUM_STATES);
#ifdef STA_TACOS_CAN_BUS_ENABLED
// Publish the state via CAN bus.
tacos::publishState(transition.from, transition.to, 0);
#endif // STA_TACOS_CAN_BUS_ENABLED
// Perform the transition and notify the threads. The event flags are set
// here in order to allow threads to react immediately.
currentState_ = transition.to;
// Send a system-wide notification for the state transition.
Statemachine::stateChangeEvent.set(transition.event);
Statemachine::stateChangeEvent.clear(EventFlags::ALL);
@ -64,19 +72,25 @@ namespace sta
return currentState_;
}
void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = 0 */)
void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = 0 */, bool publish /* = true */)
{
StateTransition transition;
transition.from = from;
transition.to = to;
transition.event = EventFlags::NORMAL;
transition.lockout = lockout;
transition.publish = publish;
// Force the transition if requested, but only if the requested state is different from the current one.
if (force && transition.to != currentState_){
// Perform the transition and notify the threads. The event flags are set
// here in order to allow threads to react immediately.
currentState_ = transition.to;
#ifdef STA_TACOS_CAN_BUS_ENABLED
tacos::publishState(transition.from, transition.to, transition.lockout);
#endif // STA_TACOS_CAN_BUS_ENABLED
Statemachine::stateChangeEvent.set(transition.event);
Statemachine::stateChangeEvent.clear(EventFlags::ALL);
@ -90,19 +104,19 @@ namespace sta
{
setLockoutTimer(transition.lockout);
}
}else{
} else {
// Try to add a state transition request to the queue. Don't wait if another
// thread is already requesting a state change.
queue_.put(transition, 0);
}
}
void Statemachine::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */)
void Statemachine::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */, bool publish /* = true */)
{
STA_ASSERT(to < STA_TACOS_NUM_STATES);
failsafeTimer_.setCallback([from, to, lockout](void* arg) {
Statemachine::instance()->requestStateTransition(from, to, lockout);
failsafeTimer_.setCallback([from, to, lockout, publish](void* arg) {
Statemachine::instance()->requestStateTransition(from, to, lockout, false, publish);
}, NULL);
failsafeTimer_.start(millis);

12
src/tacos.cpp
View File

@ -16,14 +16,14 @@ namespace sta
return Statemachine::instance()->getCurrentState();
}
void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = false */)
void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = false */, bool publish /* = false */)
{
Statemachine::instance()->requestStateTransition(from, to, lockout, force);
Statemachine::instance()->requestStateTransition(from, to, lockout, force, publish);
}
void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */)
void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */, bool publish /* = false */)
{
Statemachine::instance()->requestTimedStateTransition(from, to, millis, lockout);
Statemachine::instance()->requestTimedStateTransition(from, to, millis, lockout, publish);
}
#ifdef STA_TACOS_CAN_BUS_ENABLED
@ -31,7 +31,7 @@ namespace sta
return CanBus::instance()->queueCanBusMsg(msg, timeout);
}
bool publishState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */){
bool publishState(uint32_t from, uint32_t to, uint32_t timeout /* = 0 */){
CanSysMsg msg;
msg.header.sid = STA_TACOS_CAN_BUS_SYS_MSG_ID;
msg.header.payloadLength = 2;
@ -41,7 +41,7 @@ namespace sta
msg.header.eid = 0;
msg.header.format = 0;
return CanBus::instance()->queueCanBusMsg(msg, lockout);
return CanBus::instance()->queueCanBusMsg(msg, timeout);
}
#endif // STA_TACOS_CAN_BUS_ENABLED
} // namespace tacos