ALPAKA/TACOS
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/TACOS.git synced 2025-08-06 09:37:34 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Cut out isotp and added CAN ID for state transition

Browse Source
This commit is contained in:
CarlWachter
2024-04-30 15:24:28 +02:00
parent 5ecc5f9211
commit 104d2931be
3 changed files with 26 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
src/can_bus.cpp
View File

@@ -5,8 +5,8 @@
#include <sta/debug/assert.hpp>
#include <sta/tacos/manager.hpp>
// 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