diff --git a/Components/DataBroker/Inc/DataBroker.hpp b/Components/DataBroker/Inc/DataBroker.hpp index 0605787..7622710 100644 --- a/Components/DataBroker/Inc/DataBroker.hpp +++ b/Components/DataBroker/Inc/DataBroker.hpp @@ -175,10 +175,23 @@ class DataBroker { * @brief Returns the correct Publisher object for a template type */ template - static constexpr auto getPublisher(void) { + static constexpr Publisher* getPublisher(void) { if constexpr (matchType()) { return &IMU_Data_publisher; - } else { + } + else if constexpr(matchType()){ + return &Baro_Data_publisher; + } + else if constexpr(matchType()){ + return &Mag_Data_publisher; + } + else if constexpr(matchType()){ + return &Filter_Data_publisher; + } + else if constexpr(matchType()){ + return &GPS_Data_publisher; + } + else { SOAR_ASSERT(false, "This publisher type does not exist, you must create it"); return (Publisher*)nullptr; } @@ -186,6 +199,11 @@ class DataBroker { // List of Publishers inline static Publisher IMU_Data_publisher{DataBrokerMessageTypes::IMU_DATA}; + inline static Publisher Mag_Data_publisher{DataBrokerMessageTypes::MAG_DATA}; + inline static Publisher Baro_Data_publisher{DataBrokerMessageTypes::BARO_DATA}; + inline static Publisher Filter_Data_publisher{DataBrokerMessageTypes::FILTER_DATA}; + inline static Publisher GPS_Data_publisher{DataBrokerMessageTypes::GPS_DATA}; + }; /************************************ diff --git a/Components/DataBroker/Inc/Publisher.hpp b/Components/DataBroker/Inc/Publisher.hpp index c664c44..5f1aef4 100644 --- a/Components/DataBroker/Inc/Publisher.hpp +++ b/Components/DataBroker/Inc/Publisher.hpp @@ -83,7 +83,7 @@ class Publisher { uint8_t* messsageData = reinterpret_cast(dataToPublish); // copy data to command - brokerData.CopyDataToCommand(messsageData, sizeof(T)); + brokerData.CopyDataToCommandFixed(messsageData, sizeof(T)); subscriber.getSubscriberQueueHandle()->Send(brokerData); } diff --git a/Components/SystemTypes/DataBrokerMessageTypes.hpp b/Components/SystemTypes/DataBrokerMessageTypes.hpp index 23f4965..6cefc63 100644 --- a/Components/SystemTypes/DataBrokerMessageTypes.hpp +++ b/Components/SystemTypes/DataBrokerMessageTypes.hpp @@ -45,7 +45,7 @@ std::string ToString(DataBrokerMessageTypes messageType); inline std::string ToString(DataBrokerMessageTypes messageType) { switch (messageType) { case DataBrokerMessageTypes::IMU_DATA: { - std::string type{"IMU_DATA"}; + std::string type{"IMU32G_DATA"}; return type; } case DataBrokerMessageTypes::GPS_DATA: { diff --git a/Components/SystemTypes/SensorDataTypes.hpp b/Components/SystemTypes/SensorDataTypes.hpp index 21cc20a..3476058 100644 --- a/Components/SystemTypes/SensorDataTypes.hpp +++ b/Components/SystemTypes/SensorDataTypes.hpp @@ -26,33 +26,74 @@ * @param accelY The acceleration in the Y axis relative to the sensor * @param accelZ The acceleration in the Z axis relative to the sensor */ +struct ACCEL_t { + int16_t x; + int16_t y; + int16_t z; +}; + +struct GYRO_t { + int16_t x; + int16_t y; + int16_t z; +}; + struct IMUData { - uint32_t accelX; - uint32_t accelY; - uint32_t accelZ; + ACCEL_t accel; + GYRO_t gyro; + int16_t temp; + uint8_t id; + uint32_t gyroX; + uint32_t gyroY; + uint32_t gyroZ; + + uint32_t accelX; + uint32_t accelY; + uint32_t accelZ; }; /** * @param Temperature. Can be any where from -2147483648 to 2147483647 */ struct ThermocoupleData { - int32_t temperature; + int32_t temperature; }; struct GPSData{ uint32_t gps; }; -struct BaroData{ + +struct BaroData { uint32_t baro; + int16_t temp; + uint32_t pressure; + uint8_t id; }; -struct FilterData{ - uint32_t filter; +struct FilterData { + uint32_t alt; + uint32_t velo; + uint32_t acc; + + // predictions can be published later + /* + uint32_t alt_predict; + uint32_t velo_predict; + uint32_t acc_predict; + */ }; -struct MagData{ - uint32_t mag; +struct MagData { + uint32_t rawX; + uint32_t rawY; + uint32_t rawZ; + float scaledX; + float scaledY; + float scaledZ; + uint32_t magX; + uint32_t magY; + uint32_t magZ; }; #endif /* SENSORDATATYPES_HPP_ */ diff --git a/Core/Command.cpp b/Core/Command.cpp index 9b8f8ab..6535f6b 100644 --- a/Core/Command.cpp +++ b/Core/Command.cpp @@ -106,11 +106,13 @@ uint8_t* Command::AllocateData(uint16_t dataSize) return nullptr; } + /** * @brief Sets the internal command data pointer to the given data pointer and given size * Only use this for static buffers in which the data does NOT need to be freed. * This may be subject to require synchronization across different threads as the memory * is not indepedent to the Command object. + * * @param existingPtr byte pointer to the data * @param size Size of the given data address * @return TRUE on success, FALSE on failure (mem already allocated) @@ -142,6 +144,24 @@ bool Command::CopyDataToCommand(uint8_t* dataSrc, uint16_t size) return false; } + +/** +* @brief Copy data with fixed size. Used to avoid heap fragmentation +* @param dataSize Size of array to allocate +*/ +bool Command::CopyDataToCommandFixed(uint8_t* dataSrc, uint16_t size) +{ + // If we successfully allocate, copy the data and return success + // command should be less than 512 bytes. this is just an arbitrary number. + if(size < 512 + && this->data != nullptr) { + memcpy(this->data, dataSrc, size); + return true; + } + + return false; +} + /** * @brief Resets command, equivalent of a destructor that must be called, counts allocations and deallocations, asserts an error if the allocation count is too high */ diff --git a/Core/Inc/Command.hpp b/Core/Inc/Command.hpp index f6c81f4..06a511a 100644 --- a/Core/Inc/Command.hpp +++ b/Core/Inc/Command.hpp @@ -38,6 +38,7 @@ class Command // Functions uint8_t* AllocateData(uint16_t dataSize); // Dynamically allocates data for the command bool CopyDataToCommand(uint8_t* dataSrc, uint16_t size); // Copies the data into the command, into newly allocated memory + bool CopyDataToCommandFixed(uint8_t* dataSrc, uint16_t size); bool SetCommandToStaticExternalBuffer(uint8_t* existingPtr, uint16_t size); // Set data pointer to a pre-allocated buffer, if bFreeMemory is set to true, responsibility for freeing memory will fall on Command void Reset(); // Reset the command, equivalent of a destructor that must be called, counts allocations and deallocations, asserts an error if the allocation count is too high diff --git a/CubeDefines.cpp b/CubeDefines.cpp index 43cdb65..4e013d5 100644 --- a/CubeDefines.cpp +++ b/CubeDefines.cpp @@ -45,7 +45,7 @@ void cube_print(const char* str, ...) Command cmd(DATA_COMMAND, (uint16_t)CUBE_TASK_COMMAND_SEND_DEBUG); // Set the UART channel to send data on //Copy data into the command - cmd.CopyDataToCommand(str_buffer, buflen); + cmd.CopyDataToCommandFixed(str_buffer, buflen); //Send this packet off to the UART Task CubeTask::Inst().GetEventQueue()->Send(cmd, false); diff --git a/CubeDefines.hpp b/CubeDefines.hpp index 80b53a4..8c03c9e 100644 --- a/CubeDefines.hpp +++ b/CubeDefines.hpp @@ -16,6 +16,7 @@ #include // For uint32_t, etc. #include // Standard c printf, vsnprintf, etc. #include "cmsis_os.h" // CMSIS RTOS definitions +#include "Mutex.hpp" /* Global Functions ------------------------------------------------------------------*/ void cube_print(const char* format, ...); @@ -55,7 +56,7 @@ constexpr uint16_t ASSERT_TAKE_MAX_TIME_MS = 500; // Max time in ms to ta #define SOAR_ASSERT(expr, ...) ((expr) ? (void)0U : cube_assert_debug(false, (const char *)__FILE__, __LINE__, ##__VA_ARGS__)) // SOAR_PRINT macro, acts as an interface to the print function which sends a packet to the UART Task to print data -#define SOAR_PRINT(str, ...) (cube_print(str, ##__VA_ARGS__)) +#define SOAR_PRINT(str, ...) /** * @brief Malloc inline function, wraps malloc for multi-platform support, asserts successful allocation diff --git a/CubeTask.cpp b/CubeTask.cpp index 894c093..b10a922 100644 --- a/CubeTask.cpp +++ b/CubeTask.cpp @@ -36,6 +36,7 @@ void CubeTask::InitTask() void CubeTask::Run(void * pvParams) { //UART Task loop + while(1) { Command cm; diff --git a/Drivers/Inc/UARTTask.hpp b/Drivers/Inc/UARTTask.hpp new file mode 100644 index 0000000..9216fe3 --- /dev/null +++ b/Drivers/Inc/UARTTask.hpp @@ -0,0 +1,78 @@ +/* + * UARTTask.hpp + * + * Created on: Feb 4, 2026 + * Author: jaddina + */ + +#ifndef DRIVERS_INC_UARTTASK_HPP_ +#define DRIVERS_INC_UARTTASK_HPP_ + +/** + ****************************************************************************** + * File Name : UARTTask.hpp + * Description : + ****************************************************************************** +*/ +#ifndef SOAR_COMMS_UARTTASK_HPP_ +#define SOAR_COMMS_UARTTASK_HPP_ +/* Includes ------------------------------------------------------------------*/ +#include "Task.hpp" +#include "SystemDefines.hpp" +#include "UARTDriver.hpp" + + + +/* Macros ------------------------------------------------------------------*/ +enum UART_TASK_COMMANDS { + UART_TASK_COMMAND_NONE = 0, + UART_TASK_COMMAND_SEND_DEBUG, + UART_TASK_COMMAND_SEND_RADIO, + UART_TASK_COMMAND_SEND_PBB, + UART_TASK_COMMAND_MAX +}; + + +/* Class ------------------------------------------------------------------*/ +class UARTTask : public Task +{ +public: + static UARTTask& Inst() { + static UARTTask inst; + return inst; + } + + void InitTask(); + +protected: + static void RunTask(void* pvParams) { UARTTask::Inst().Run(pvParams); } // Static Task Interface, passes control to the instance Run(); + + void Run(void* pvParams); // Main run code + + void ConfigureUART(); + void HandleCommand(Command& cm); + +private: + UARTTask() : Task(UART_TASK_QUEUE_DEPTH_OBJS) {} // Private constructor + UARTTask(const UARTTask&); // Prevent copy-construction + UARTTask& operator=(const UARTTask&); // Prevent assignment +}; + + +/* Utility Functions ------------------------------------------------------------------*/ +namespace UARTUtils +{ + +} + + +#endif // SOAR_COMMS_UARTTASK_HPP_ + + + + + + + + +#endif /* DRIVERS_INC_UARTTASK_HPP_ */ diff --git a/Drivers/UARTTask.cpp b/Drivers/UARTTask.cpp new file mode 100644 index 0000000..cd7a193 --- /dev/null +++ b/Drivers/UARTTask.cpp @@ -0,0 +1,100 @@ +/* + * UARTTask.cpp + * + * Created on: Feb 4, 2026 + * Author: jaddina + */ +/** + ****************************************************************************** + * File Name : UARTTask.cpp + * Description : UART + ****************************************************************************** +*/ + +#include "UARTTask.hpp" +#include "UARTDriver.hpp" + +/** + * TODO: Currently not used, would be used for DMA buffer configuration or interrupt setup + * @brief Configures UART DMA buffers and interrupts + * +*/ +void UARTTask::ConfigureUART() +{ + // UART 5 - Uses polling for now (switch to DMA or interrupts once SOAR-Protocol is defined) +} + +/** + * @brief Initializes UART task with the RTOS scheduler +*/ +void UARTTask::InitTask() +{ + // Make sure the task is not already initialized + SOAR_ASSERT(rtTaskHandle == nullptr, "Cannot initialize UART task twice"); + + // Start the task + BaseType_t rtValue = + xTaskCreate((TaskFunction_t)UARTTask::RunTask, + (const char*)"UARTTask", + (uint16_t)UART_TASK_STACK_DEPTH_WORDS, + (void*)this, + (UBaseType_t)UART_TASK_RTOS_PRIORITY, + (TaskHandle_t*)&rtTaskHandle); + + //Ensure creation succeded + SOAR_ASSERT(rtValue == pdPASS, "UARTTask::InitTask() - xTaskCreate() failed"); + + // Configure DMA + +} + +/** + * @brief Instance Run loop for the UART Task, runs on scheduler start as long as the task is initialized. + * @param pvParams RTOS Passed void parameters, contains a pointer to the object instance, should not be used +*/ +void UARTTask::Run(void * pvParams) +{ + //UART Task loop + while(1) { + Command cm; + + //Wait forever for a command + qEvtQueue->ReceiveWait(cm); + + //Process the command + HandleCommand(cm); + } +} + +/** + * @brief HandleCommand handles any command passed to the UART task primary event queue. Responsible for + * handling all commands, even if unsupported. (Unexpected commands must still be reset) + * @param cm Reference to the command object to handle +*/ +void UARTTask::HandleCommand(Command& cm) +{ + //Switch for the GLOBAL_COMMAND + switch (cm.GetCommand()) { + case DATA_COMMAND: { + //Switch for task specific command within DATA_COMMAND + switch (cm.GetTaskCommand()) { + case UART_TASK_COMMAND_SEND_DEBUG: + UART::Debug->Transmit(cm.GetDataPointer(), cm.GetDataSize()); + break; + + default: + SOAR_PRINT("UARTTask - Received Unsupported DATA_COMMAND {%d}\n", cm.GetTaskCommand()); + break; + } + } + case TASK_SPECIFIC_COMMAND: { + break; + } + default: + SOAR_PRINT("UARTTask - Received Unsupported Command {%d}\n", cm.GetCommand()); + break; + } + + //No matter what we happens, we must reset allocated data + cm.Reset(); +}