diff --git a/include/sta/tacos/can_bus.hpp b/include/sta/tacos/can_bus.hpp index e265109..b7e8c88 100644 --- a/include/sta/tacos/can_bus.hpp +++ b/include/sta/tacos/can_bus.hpp @@ -5,11 +5,12 @@ #ifdef STA_CAN_BUS_ENABLE -#include #include #include #include +#include +#include #include @@ -50,14 +51,6 @@ namespace sta return _instance; } - /** - * @brief Place data message in CAN driver TX queue. - * - * @param msg Message to transmit - * @param timeout Timeout for placing message (0 = no wait, osWaitForever = blocking) - * @return True if message was queued successfully - */ - bool queueCanBusMsg(const CanDataMsg & msg, uint32_t timeout); /** * @brief Place system message in CAN driver TX queue. * @@ -67,15 +60,6 @@ namespace sta */ bool queueCanBusMsg(const CanSysMsg & msg, uint32_t timeout); - /** - * @brief Retrieve data message from CAN driver TX queue. - * - * @param[out] msg Output address for retrieved message - * @param timeout Timeout for retrieving message (0 = no wait, osWaitForever = blocking) - * @return True if message was retrieved successfully - */ - bool getCanBusMsg(CanDataMsg * msg, uint32_t timeout); - /** * @brief Retrieve system message from CAN driver TX queue. * @@ -113,12 +97,10 @@ namespace sta sta::STM32CanController * canBusController_; - //CanDataMsg canBusDataQueueBuffer_[STA_RTOS_CAN_BUS_QUEUE_LENGTH]; CanSysMsg* canBusSysQueueBuffer_[STA_RTOS_CAN_BUS_QUEUE_LENGTH]; uint8_t bufferIndex; RtosQueue canBusSysQueue_; - RtosQueue canBusDataQueue_; uint32_t canBusStack[256]; @@ -127,10 +109,11 @@ namespace sta static RtosEvent messageEvent; }; - - void handleSysMessage(const sta::CanRxHeader & header, const uint8_t * payload); - - void handleDataMessage(const sta::IsotpMessage & msg); + /** + * @brief Callback function for handling received messages. Intended for state transitions. + */ + STA_WEAK + void handleSysMessage(CanMsgHeader & header, uint8_t * payload); } /* namespace tacos */ diff --git a/include/sta/tacos/configs/default.hpp b/include/sta/tacos/configs/default.hpp index a5967f7..b19db1a 100644 --- a/include/sta/tacos/configs/default.hpp +++ b/include/sta/tacos/configs/default.hpp @@ -15,4 +15,7 @@ #define STA_RTOS_CAN_BUS_MAX_FILTER 14 #define STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE 8 +// State transition message define with highest CAN priority +#define STA_TACOS_CAN_BUS_SYS_MSG_ID 0x0 + #endif // STA_TACOS_CONFIGS_DEFAULT_HPP diff --git a/src/can_bus.cpp b/src/can_bus.cpp index e26a8b2..a9767fe 100644 --- a/src/can_bus.cpp +++ b/src/can_bus.cpp @@ -5,8 +5,8 @@ #include #include -// TODO fix this shit -extern CAN_HandleTypeDef hcan1; +extern CAN_HandleTypeDef STA_STM32_CAN_HANDLE; + namespace sta { namespace tacos @@ -15,8 +15,7 @@ namespace sta : TacosThread{"Can Bus", STA_TACOS_CAN_BUS_PRIORITY}, canBusController_(new STM32CanController(&STA_STM32_CAN_HANDLE)), canBusSysQueue_(STA_RTOS_CAN_BUS_QUEUE_LENGTH), - canBusDataQueue_(STA_RTOS_CAN_BUS_QUEUE_LENGTH), - canBus_{canBusController_, HAL_GetTick, sta::tacos::handleSysMessage, sta::tacos::handleDataMessage} + canBus_{canBusController_, HAL_GetTick} { bufferIndex = 0; for(int i = 0; i < STA_RTOS_CAN_BUS_QUEUE_LENGTH; i++){ @@ -28,8 +27,8 @@ namespace sta { canBusController_->start(); - if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK || - HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO1_MSG_PENDING)) + 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(); } @@ -70,37 +69,18 @@ namespace sta break; } } + + if(sysMsg.header.sid == STA_TACOS_CAN_BUS_SYS_MSG_ID){ + // Handle system message + handleSysMessage(sysMsg.header, sysMsg.payload); + } } } - //canBus_.processRx(); - } - - if (flags & STA_RTOS_CAN_FLAG_SHOW_STATS) - { - canBus_.showStatistics(); } } } - bool CanBus::queueCanBusMsg(const CanDataMsg& msg, uint32_t timeout) - { - STA_ASSERT((msg.header.sid & STA_CAN_SID_SYS_BITS) == 0); - STA_ASSERT(msg.header.payloadLength <= sizeof(msg.payload)); - - if (canBusDataQueue_.put(msg, timeout)) - { - // Signal task - messageEvent.set(STA_RTOS_CAN_FLAG_DATA_QUEUED); - messageEvent.clear(STA_RTOS_CAN_ANY); - return true; - } - else - { - return false; - } - } - 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 @@ -133,7 +113,10 @@ namespace sta CanSysMsg sysMsg; sysMsg.header.sid = rxHeader.id.sid; sysMsg.header.payloadLength = rxHeader.payloadLength; - std::memcpy(sysMsg.payload, canRX, rxHeader.payloadLength); + + for(int i = 0; i < rxHeader.payloadLength; i++){ + sysMsg.payload[i] = canRX[i]; + } canBusSysQueueBuffer_[bufferIndex] = &sysMsg; @@ -143,11 +126,6 @@ namespace sta } } - bool CanBus::getCanBusMsg(CanDataMsg * msg, uint32_t timeout) - { - return (canBusDataQueue_.get(msg, timeout)); - } - bool CanBus::getCanBusMsg(CanSysMsg * msg, uint32_t timeout) { return (canBusSysQueue_.get(msg, timeout)); @@ -169,74 +147,9 @@ namespace sta { namespace tacos { - void handleSysMessage(const sta::CanRxHeader & header, const uint8_t * payload) + void handleSysMessage(CanMsgHeader & header, uint8_t * payload) { - // Write frame payload to DebugSerial - STA_DEBUG_PRINTLN("[event] RX sys frame"); - - debug::printFrameID(header.id); - debug::printPayloadHex(payload, header.payloadLength); - - // Sysmessage is mainly only state change from GRSM - // TODO add other cases - - // 0 is from state, 1 is to state, 2 is lockout, 3 is failsafe (-1 if inactive) - if(payload[1] > 0 && payload[1] < STA_TACOS_NUM_STATES){ - - if(payload[3] == -1){ - sta::tacos::Statemachine::instance()->requestStateTransition(payload[0], payload[1], payload[2]); - } - else{ - sta::tacos::Statemachine::instance()->requestTimedStateTransition(payload[0], payload[1], payload[2], payload[3]); - } - } - } - - void handleDataMessage(const sta::IsotpMessage & msg) - { - STA_ASSERT(msg.buffer != nullptr); - STA_ASSERT(msg.size != 0); - - STA_DEBUG_PRINTLN("[event] RX data message"); - - debug::printFrameID(msg.frameID); - - // TODO Forward message to other threads - - // if (buffer[0] == DEMO_BMP_PACKET_ID) - // { - // BmpPacket packet; - // if (unpack(buffer + 1, size - 1, &packet)) - // { - // STA_DEBUG_PRINTLN(); - // STA_DEBUG_PRINTLN("# ############"); - // STA_DEBUG_PRINTLN("# ## BMP380 ##"); - // STA_DEBUG_PRINTLN("# ############"); - // STA_DEBUG_PRINTLN("#"); - // - // STA_DEBUG_PRINT("# temperature: "); - // STA_DEBUG_PRINT(packet.temperature); - // STA_DEBUG_PRINTLN(" *C"); - // STA_DEBUG_PRINT("# pressure: "); - // STA_DEBUG_PRINT(packet.pressure); - // STA_DEBUG_PRINTLN(" Pa"); - // STA_DEBUG_PRINT("# altitude: "); - // STA_DEBUG_PRINT(packet.altitude); - // STA_DEBUG_PRINTLN(" m"); - // STA_DEBUG_PRINTLN(); - // } - // else - // { - // STA_DEBUG_PRINTLN("[error] BMP unpack failed"); - // } - // } - // else - { - STA_DEBUG_PRINT("ID: "); - STA_DEBUG_PRINTLN(msg.buffer[0], sta::IntegerBase::HEX); - STA_DEBUG_PRINT("size: "); - STA_DEBUG_PRINTLN(msg.size); - } + // This is a weak function that can be overridden by the user } } // namespace tacos } // namespace sta