diff --git a/include/sta/tacos.hpp b/include/sta/tacos.hpp index 8d6fa36..7f9ef4a 100644 --- a/include/sta/tacos.hpp +++ b/include/sta/tacos.hpp @@ -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 } diff --git a/include/sta/tacos/can_bus.hpp b/include/sta/tacos/can_bus.hpp index 27b0086..21e684e 100644 --- a/include/sta/tacos/can_bus.hpp +++ b/include/sta/tacos/can_bus.hpp @@ -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 canBusSysQueue_; + RtosQueue canBusSysQueue_; + RtosQueue canBusRxQueue_; AlpakaCanBus canBus_; diff --git a/include/sta/tacos/statemachine.hpp b/include/sta/tacos/statemachine.hpp index afa095b..452809b 100644 --- a/include/sta/tacos/statemachine.hpp +++ b/include/sta/tacos/statemachine.hpp @@ -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; diff --git a/src/can_bus.cpp b/src/can_bus.cpp index 87e147f..78686d3 100644 --- a/src/can_bus.cpp +++ b/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(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(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 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 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 diff --git a/src/startup.cpp b/src/startup.cpp index a841f00..6934df5 100644 --- a/src/startup.cpp +++ b/src/startup.cpp @@ -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 diff --git a/src/statemachine.cpp b/src/statemachine.cpp index 6df4e93..d970a57 100644 --- a/src/statemachine.cpp +++ b/src/statemachine.cpp @@ -5,6 +5,7 @@ * Author: Dario */ +#include #include #include @@ -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); diff --git a/src/tacos.cpp b/src/tacos.cpp index 99a9380..f1c615f 100644 --- a/src/tacos.cpp +++ b/src/tacos.cpp @@ -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