From 07b38f8701723afe4284878575155f4dcbd3deb2 Mon Sep 17 00:00:00 2001
From: Gabriel Lima <gabrielmartinslima@gmail.com>
Date: Sat, 27 Dec 2025 23:54:34 -0300
Subject: [PATCH] Firmware: Implemented TelemetryDispatcher skeleton

---
 Firmware/.cproject                            |  5 ++-
 .../TelemetryDispatcher.cpp                   | 35 ++++++++++++++++
 .../TelemetryDispatcher.hpp                   | 27 ++++++++++++
 .../{Components => }/TelemetryPacket.hpp      |  0
 Firmware/Application/start.cpp                | 42 +++++++------------
 Firmware/BrushedPIDDriver.ioc                 | 11 +++--
 Firmware/Core/Inc/FreeRTOSConfig.h            |  7 ++--
 Firmware/Core/Src/app_freertos.c              |  2 +-
 8 files changed, 92 insertions(+), 37 deletions(-)
 create mode 100644 Firmware/Application/Components/TelemetryDispatcher/TelemetryDispatcher.cpp
 create mode 100644 Firmware/Application/Components/TelemetryDispatcher/TelemetryDispatcher.hpp
 rename Firmware/Application/{Components => }/TelemetryPacket.hpp (100%)

diff --git a/Firmware/.cproject b/Firmware/.cproject
index 312f829..ce8b266 100644
--- a/Firmware/.cproject
+++ b/Firmware/.cproject
@@ -26,8 +26,8 @@
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.499271190" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Debug || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.value.workspace || STM32G474RBTx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Core/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc/Legacy | ../Drivers/CMSIS/Device/ST/STM32G4xx/Include | ../Drivers/CMSIS/Include | ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../Core/ThreadSafe | ../USB_Device/App | ../USB_Device/Target | ../Middlewares/ST/STM32_USB_Device_Library/Core/Inc | ../Middlewares/ST/STM32_USB_Device_Library/Class/CustomHID/Inc || ../Core/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc/Legacy | ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F | ../Drivers/CMSIS/Device/ST/STM32G4xx/Include | ../Drivers/CMSIS/Include | ../USB_Device/App | ../USB_Device/Target | ../Middlewares/ST/STM32_USB_Device_Library/Core/Inc | ../Middlewares/ST/STM32_USB_Device_Library/Class/CustomHID/Inc ||  || USE_HAL_DRIVER | STM32G474xx | STM32_THREAD_SAFE_STRATEGY=4 ||  || USB_Device | Core/ThreadSafe | Drivers | Core/Startup | Middlewares | Core ||  ||  || ${workspace_loc:/${ProjName}/STM32G474RBTX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || FreeRTOS_AllowLockFromIRQ ||  ||  || " valueType="string"/>
 							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.1811138434" name="Cpu clock frequence" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="48" valueType="string"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.converthex.1736150988" name="Convert to Intel Hex file (-O ihex)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.converthex" useByScannerDiscovery="false" value="true" valueType="boolean"/>
-							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.1821079540" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat" value="true" valueType="boolean"/>
-							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoscanffloat.557990240" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoscanffloat" value="true" valueType="boolean"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.1821079540" name="Use float with printf from newlib-nano (-u _printf_float)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoscanffloat.557990240" name="Use float with scanf from newlib-nano (-u _scanf_float)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoscanffloat" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.45455421" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
 							<builder buildPath="${workspace_loc:/BrushedPIDDriver}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.2074713205" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.622430708" name="MCU/MPU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
@@ -99,6 +99,7 @@
 									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"/>
 									<listOptionValue builtIn="false" value="../Application/Shared/RoboFramework/Inc"/>
 									<listOptionValue builtIn="false" value="../Application/Shared/RoboFramework_STM32/Inc"/>
+									<listOptionValue builtIn="false" value="../Application"/>
 									<listOptionValue builtIn="false" value="../USB_Device/App"/>
 									<listOptionValue builtIn="false" value="../USB_Device/Target"/>
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Core/Inc"/>
diff --git a/Firmware/Application/Components/TelemetryDispatcher/TelemetryDispatcher.cpp b/Firmware/Application/Components/TelemetryDispatcher/TelemetryDispatcher.cpp
new file mode 100644
index 0000000..97e0244
--- /dev/null
+++ b/Firmware/Application/Components/TelemetryDispatcher/TelemetryDispatcher.cpp
@@ -0,0 +1,35 @@
+/*
+ * TelemetryDispatcher.cpp
+ *
+ *  Created on: Dec 27, 2025
+ *      Author: Gabriel
+ */
+
+#include "TelemetryDispatcher.hpp"
+
+TelemetryDispatcher::TelemetryDispatcher(Queue<TelemetryPacket>* inQueue, HIDRAW* pHid) :
+		Executable(Log::Level::Debug, "TelemetryDispatcher"),
+		inQueue(inQueue),
+		pHid(pHid)
+{
+
+}
+
+TelemetryDispatcher::~TelemetryDispatcher() {
+
+}
+
+int32_t TelemetryDispatcher::init() {
+	int32_t errors = 0;
+	log(Log::Level::Informational, "Initalizing");
+	log(Log::Level::Informational, "Initalzed with %d errors", errors);
+	return errors;
+}
+
+int32_t TelemetryDispatcher::execute() {
+	log(Log::Level::Informational, "Started execution");
+	while(true){
+		vTaskDelay(1);
+	}
+	return 0;
+}
diff --git a/Firmware/Application/Components/TelemetryDispatcher/TelemetryDispatcher.hpp b/Firmware/Application/Components/TelemetryDispatcher/TelemetryDispatcher.hpp
new file mode 100644
index 0000000..e41aca0
--- /dev/null
+++ b/Firmware/Application/Components/TelemetryDispatcher/TelemetryDispatcher.hpp
@@ -0,0 +1,27 @@
+/*
+ * TelemetryDispatcher.hpp
+ *
+ *  Created on: Dec 27, 2025
+ *      Author: Gabriel
+ */
+
+#ifndef COMPONENTS_TELEMETRYDISPATCHER_TELEMETRYDISPATCHER_HPP_
+#define COMPONENTS_TELEMETRYDISPATCHER_TELEMETRYDISPATCHER_HPP_
+
+#include "Executable.hpp"
+#include "HIDRAW.hpp"
+#include "Queue.hpp"
+#include "TelemetryPacket.hpp"
+
+class TelemetryDispatcher : public Executable {
+public:
+	TelemetryDispatcher(Queue<TelemetryPacket>* inQueue, HIDRAW* pHid);
+	virtual ~TelemetryDispatcher();
+	int32_t init();
+	int32_t execute();
+private:
+	Queue<TelemetryPacket>* inQueue;
+	HIDRAW* pHid;
+};
+
+#endif /* COMPONENTS_TELEMETRYDISPATCHER_TELEMETRYDISPATCHER_HPP_ */
diff --git a/Firmware/Application/Components/TelemetryPacket.hpp b/Firmware/Application/TelemetryPacket.hpp
similarity index 100%
rename from Firmware/Application/Components/TelemetryPacket.hpp
rename to Firmware/Application/TelemetryPacket.hpp
diff --git a/Firmware/Application/start.cpp b/Firmware/Application/start.cpp
index 873dd79..3841f49 100644
--- a/Firmware/Application/start.cpp
+++ b/Firmware/Application/start.cpp
@@ -8,21 +8,16 @@
 #include "start.h"
 #include <cstdio>
 #include "FreeRTOS.h"
-#include "GPIO_Pin_STM32.hpp"
 #include "LogDriver.hpp"
-#include "GPIO_Pin_STM32.hpp"
-#include "PWM_Pin_STM32.hpp"
 #include "Queue_STM32.hpp"
-#include "CDC_STM32.hpp"
-#include "Encoder_STM32.hpp"
-#include "SoftTimer_STM32.hpp"
-
-#include "Components/TelemetryPacket.hpp"
+#include "HIDRAW_STM32.hpp"
 
 #include "main.h"
-#include "usb_device.h"
-#include "usbd_customhid.h"
-extern USBD_HandleTypeDef hUsbDeviceFS;
+
+#include <Components/TelemetryDispatcher/TelemetryDispatcher.hpp>
+
+Queue_STM32<TelemetryPacket, 1> telemetryQueue("telemetryQueue");
+TelemetryDispatcher componentTelemetryDispatcher(&telemetryQueue, HIDRAW_STM32::getInstance());
 
 void executableDispatch(void* _executable){
 	Executable* executable = static_cast<Executable*>(_executable);
@@ -48,27 +43,20 @@ void start(){
 	// Init log facility
 	LogDriver::getInstance()->init();
 	TaskHandle_t hTaskLogDriver;
-	xTaskCreate(executableDispatch, "LogDriver", 256, LogDriver::getInstance(), 5, &hTaskLogDriver);
+	xTaskCreate(executableDispatch, "LogDriver", 512, LogDriver::getInstance(), 5, &hTaskLogDriver);
 
 	// Init queues
+	telemetryQueue.init();
 
 	// Init shared resources
+	HIDRAW_STM32::getInstance()->init();
 
-	//Init components
-	pkt.counter = 0;
-	for(uint32_t i=0; i<32; i++){
-		pkt.message[i] = 33 + i;
-	}
-	for(uint32_t i=0; i<23; i++){
-		pkt.padding[i] = 0;
-	}
-	while(true){
-		pkt.counter++;
-		pkt.timestamp_ms = xTaskGetTickCount();
-		uint8_t result = USBD_CUSTOM_HID_SendReport(&hUsbDeviceFS, reinterpret_cast<uint8_t*>(&pkt), 64);
-		vTaskDelay(1000);
-	}
+	// Init components
+	componentTelemetryDispatcher.init();
+
+	// Init tasks
+	TaskHandle_t hTaskTelemetryDispatcher;
+	xTaskCreate(executableDispatch, "TelemetryDispatcher", 512, &componentTelemetryDispatcher, 20, &hTaskTelemetryDispatcher);
 
-	//Init tasks
 	vTaskDelete(nullptr);
 }
diff --git a/Firmware/BrushedPIDDriver.ioc b/Firmware/BrushedPIDDriver.ioc
index 6d465fa..758d18e 100644
--- a/Firmware/BrushedPIDDriver.ioc
+++ b/Firmware/BrushedPIDDriver.ioc
@@ -39,6 +39,7 @@ FDCAN1.CalculateBaudRateNominal=1000000
 FDCAN1.CalculateTimeBitNominal=1000
 FDCAN1.CalculateTimeQuantumNominal=333.3333333333333
 FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal
+FREERTOS.FootprintOK=true
 FREERTOS.INCLUDE_pcTaskGetTaskName=1
 FREERTOS.INCLUDE_uxTaskGetStackHighWaterMark=1
 FREERTOS.INCLUDE_uxTaskGetStackHighWaterMark2=0
@@ -49,11 +50,13 @@ FREERTOS.INCLUDE_xEventGroupSetBitFromISR=1
 FREERTOS.INCLUDE_xSemaphoreGetMutexHolder=1
 FREERTOS.INCLUDE_xTaskGetHandle=1
 FREERTOS.INCLUDE_xTaskResumeFromISR=0
-FREERTOS.IPParameters=Tasks01,configUSE_APPLICATION_TASK_TAG,configTOTAL_HEAP_SIZE,configTIMER_TASK_PRIORITY,INCLUDE_xSemaphoreGetMutexHolder,INCLUDE_pcTaskGetTaskName,INCLUDE_xEventGroupSetBitFromISR,INCLUDE_xTaskGetHandle,INCLUDE_uxTaskGetStackHighWaterMark2,configUSE_NEWLIB_REENTRANT,INCLUDE_uxTaskGetStackHighWaterMark,INCLUDE_uxTaskPriorityGet,INCLUDE_vTaskPrioritySet,INCLUDE_xTaskResumeFromISR,INCLUDE_vTaskSuspend,configENABLE_FPU
-FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
+FREERTOS.IPParameters=Tasks01,configUSE_APPLICATION_TASK_TAG,configTOTAL_HEAP_SIZE,configTIMER_TASK_PRIORITY,INCLUDE_xSemaphoreGetMutexHolder,INCLUDE_pcTaskGetTaskName,INCLUDE_xEventGroupSetBitFromISR,INCLUDE_xTaskGetHandle,INCLUDE_uxTaskGetStackHighWaterMark2,configUSE_NEWLIB_REENTRANT,INCLUDE_uxTaskGetStackHighWaterMark,INCLUDE_uxTaskPriorityGet,INCLUDE_vTaskPrioritySet,INCLUDE_xTaskResumeFromISR,INCLUDE_vTaskSuspend,configENABLE_FPU,configRECORD_STACK_HIGH_ADDRESS,FootprintOK,configMAX_TASK_NAME_LEN
+FREERTOS.Tasks01=defaultTask,24,256,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
 FREERTOS.configENABLE_FPU=1
-FREERTOS.configTIMER_TASK_PRIORITY=2
-FREERTOS.configTOTAL_HEAP_SIZE=8192
+FREERTOS.configMAX_TASK_NAME_LEN=32
+FREERTOS.configRECORD_STACK_HIGH_ADDRESS=1
+FREERTOS.configTIMER_TASK_PRIORITY=48
+FREERTOS.configTOTAL_HEAP_SIZE=32768
 FREERTOS.configUSE_APPLICATION_TASK_TAG=1
 FREERTOS.configUSE_NEWLIB_REENTRANT=1
 File.Version=6
diff --git a/Firmware/Core/Inc/FreeRTOSConfig.h b/Firmware/Core/Inc/FreeRTOSConfig.h
index 65a242c..e924a08 100644
--- a/Firmware/Core/Inc/FreeRTOSConfig.h
+++ b/Firmware/Core/Inc/FreeRTOSConfig.h
@@ -69,8 +69,8 @@
 #define configTICK_RATE_HZ                       ((TickType_t)1000)
 #define configMAX_PRIORITIES                     ( 56 )
 #define configMINIMAL_STACK_SIZE                 ((uint16_t)128)
-#define configTOTAL_HEAP_SIZE                    ((size_t)8192)
-#define configMAX_TASK_NAME_LEN                  ( 16 )
+#define configTOTAL_HEAP_SIZE                    ((size_t)32768)
+#define configMAX_TASK_NAME_LEN                  ( 32 )
 #define configUSE_TRACE_FACILITY                 1
 #define configUSE_16_BIT_TICKS                   0
 #define configUSE_MUTEXES                        1
@@ -79,6 +79,7 @@
 #define configUSE_APPLICATION_TASK_TAG           1
 #define configUSE_COUNTING_SEMAPHORES            1
 #define configUSE_PORT_OPTIMISED_TASK_SELECTION  0
+#define configRECORD_STACK_HIGH_ADDRESS          1
 /* USER CODE BEGIN MESSAGE_BUFFER_LENGTH_TYPE */
 /* Defaults to size_t for backward compatibility, but can be changed
    if lengths will always be less than the number of bytes in a size_t. */
@@ -91,7 +92,7 @@
 
 /* Software timer definitions. */
 #define configUSE_TIMERS                         1
-#define configTIMER_TASK_PRIORITY                ( 2 )
+#define configTIMER_TASK_PRIORITY                ( 48 )
 #define configTIMER_QUEUE_LENGTH                 10
 #define configTIMER_TASK_STACK_DEPTH             256
 
diff --git a/Firmware/Core/Src/app_freertos.c b/Firmware/Core/Src/app_freertos.c
index 0ff903c..2569e83 100644
--- a/Firmware/Core/Src/app_freertos.c
+++ b/Firmware/Core/Src/app_freertos.c
@@ -54,7 +54,7 @@ osThreadId_t defaultTaskHandle;
 const osThreadAttr_t defaultTask_attributes = {
   .name = "defaultTask",
   .priority = (osPriority_t) osPriorityNormal,
-  .stack_size = 128 * 4
+  .stack_size = 256 * 4
 };
 
 /* Private function prototypes -----------------------------------------------*/