Integration rebase

Components/DataBroker/Inc/DataBroker.hpp

@@ -175,17 +175,35 @@ class DataBroker {
    * @brief Returns the correct Publisher object for a template type
    */
   template <typename T>
-  static constexpr auto getPublisher(void) {
+  static constexpr Publisher<T>* getPublisher(void) {
     if constexpr (matchType<T, IMUData>()) {
       return &IMU_Data_publisher;
-    } else {
+    }
+    else if constexpr(matchType<T, BaroData>()){
+    	return &Baro_Data_publisher;
+    }
+    else if constexpr(matchType<T, MagData>()){
+    	return &Mag_Data_publisher;
+    }
+    else if constexpr(matchType<T, FilterData>()){
+    	return &Filter_Data_publisher;
+    }
+    else if constexpr(matchType<T, GPSData>()){
+    	return &GPS_Data_publisher;
+    }
+    else {
       SOAR_ASSERT(false, "This publisher type does not exist, you must create it");
       return (Publisher<T>*)nullptr;
     }
   }
 
   // List of Publishers
   inline static Publisher<IMUData> IMU_Data_publisher{DataBrokerMessageTypes::IMU_DATA};
+  inline static Publisher<MagData> Mag_Data_publisher{DataBrokerMessageTypes::MAG_DATA};
+  inline static Publisher<BaroData> Baro_Data_publisher{DataBrokerMessageTypes::BARO_DATA};
+  inline static Publisher<FilterData> Filter_Data_publisher{DataBrokerMessageTypes::FILTER_DATA};
+  inline static Publisher<GPSData> GPS_Data_publisher{DataBrokerMessageTypes::GPS_DATA};
+
 
 };
 /************************************

Components/DataBroker/Inc/Publisher.hpp

@@ -83,7 +83,7 @@ class Publisher {
         uint8_t* messsageData = reinterpret_cast<uint8_t*>(dataToPublish);
 
         // copy data to command
-        brokerData.CopyDataToCommand(messsageData, sizeof(T));
+        brokerData.CopyDataToCommandFixed(messsageData, sizeof(T));
 
         subscriber.getSubscriberQueueHandle()->Send(brokerData);
       }

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: {

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_ */

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
 */

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

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);

CubeDefines.hpp

@@ -16,6 +16,7 @@
 #include <cstdint>        // For uint32_t, etc.
 #include <cstdio>        // 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

CubeTask.cpp

@@ -36,6 +36,7 @@ void CubeTask::InitTask()
 void CubeTask::Run(void * pvParams)
 {
     //UART Task loop
+
     while(1) {
         Command cm;
 

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_ */