diff --git a/stm32h753iitx/boardfiles/model/src/drivers.cpp b/stm32h753iitx/boardfiles/model/src/drivers.cpp
index 427a9efe..e8b890bc 100644
--- a/stm32h753iitx/boardfiles/model/src/drivers.cpp
+++ b/stm32h753iitx/boardfiles/model/src/drivers.cpp
@@ -100,7 +100,6 @@ void initDrivers()
     // Core utilities
     systemUtilsHandle = new (&systemUtilsStorage) SystemUtils();
     iwdgHandle = new (&iwdgStorage) IndependentWatchdog(&hiwdg1);
-    loggerHandle = new (&loggerStorage) Logger(); // Initialized later in RTOS task
 
     // Motors
     leftAileronMotorHandle = new (&leftAileronMotorStorage) MotorControl(&htim1, TIM_CHANNEL_1, 5, 10);
diff --git a/stm32l552xx/.cproject b/stm32l552xx/.cproject
index 36d1a940..f9f18a46 100644
--- a/stm32l552xx/.cproject
+++ b/stm32l552xx/.cproject
@@ -58,6 +58,7 @@
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.1110447383" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
 									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="../../zeropilot3.5/build/l552/include"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32L5xx_HAL_Driver/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32L5xx_HAL_Driver/Inc/Legacy"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32L5xx/Include"/>
@@ -87,7 +88,8 @@
 									<listOptionValue builtIn="false" value="../../zeropilot4.0/build/l552/include"/>
 									<listOptionValue builtIn="false" value="../boardfiles/drivers/iwdg"/>
 									<listOptionValue builtIn="false" value="../boardfiles/drivers/power_module"/>
-									<listOptionValue builtIn="false" value="../boardfiles/drivers/logger"/>
+									<listOptionValue builtIn="false" value="../boardfiles/drivers/systemutils"/>
+									<listOptionValue builtIn="false" value="../boardfiles/drivers/textIO"/>
 									<listOptionValue builtIn="false" value="../boardfiles/drivers/motor"/>
 									<listOptionValue builtIn="false" value="../boardfiles/drivers/queue"/>
 									<listOptionValue builtIn="false" value="../boardfiles/drivers/rc"/>
@@ -147,11 +149,11 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="boardfiles"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="FATFS"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
-						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
+						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="boardfiles"/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
@@ -312,9 +314,242 @@
 					<sourceEntries>
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="boardfiles"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="FATFS"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
+					</sourceEntries>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+		</cconfiguration>
+		<cconfiguration id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.627096135">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.627096135" moduleId="org.eclipse.cdt.core.settings" name="Debug">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" cleanCommand="rm -rf" description="" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.627096135" name="Debug" parent="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug">
+					<folderInfo id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.627096135." name="/" resourcePath="">
+						<toolChain id="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.debug.652822347" name="MCU ARM GCC" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.debug">
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu.2044983602" name="MCU" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu" useByScannerDiscovery="true" value="STM32L552ZETxQ" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid.796786616" name="CPU" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid" useByScannerDiscovery="false" value="0" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.908657610" name="Core" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid" useByScannerDiscovery="false" value="0" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.919811758" name="Floating-point unit" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.value.fpv5-sp-d16" valueType="enumerated"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.57826945" name="Floating-point ABI" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.value.hard" valueType="enumerated"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1860358273" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" useByScannerDiscovery="false" value="NUCLEO-L552ZE-Q" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.354581061" 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 || NUCLEO-L552ZE-Q || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Core/Inc | ../Drivers/STM32L5xx_HAL_Driver/Inc | ../Drivers/STM32L5xx_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_CM33_NTZ/non_secure | ../Drivers/CMSIS/Device/ST/STM32L5xx/Include | ../Drivers/CMSIS/Include ||  ||  || USE_FULL_LL_DRIVER | USE_HAL_DRIVER | STM32L552xx ||  || Drivers | Core/Startup | Middlewares | Core ||  ||  || ${workspace_loc:/${ProjName}/STM32L552ZETXQ_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || None ||  ||  || " valueType="string"/>
+							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.1414707443" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
+							<builder buildPath="${workspace_loc:/stm32l552xx}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.1361190127" 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.1443703284" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel.981801139" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel.value.g3" valueType="enumerated"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols.33968878" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="DEBUG"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.234656488" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/boardfiles/drivers/rfd}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/mavlink2}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/mavlink2/all}&quot;"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1596401737" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1095045838" name="MCU GCC Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.2775132" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.value.g3" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.834661917" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.1355568578" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="DEBUG"/>
+									<listOptionValue builtIn="false" value="USE_FULL_LL_DRIVER"/>
+									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
+									<listOptionValue builtIn="false" value="STM32L552xx"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.767042033" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32L5xx_HAL_Driver/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32L5xx_HAL_Driver/Inc/Legacy"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM33_NTZ/non_secure"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32L5xx/Include"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/boardfiles/drivers/rfd}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/mavlink2}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/mavlink2/all}&quot;"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1561074814" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.600978073" name="MCU G++ Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel.1077561319" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel.value.g3" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.477969106" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level" useByScannerDiscovery="false"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths.1252810566" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32L5xx_HAL_Driver/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32L5xx_HAL_Driver/Inc/Legacy"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM33_NTZ/non_secure"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32L5xx/Include"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/boardfiles/drivers/rfd}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/mavlink2}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/mavlink2/all}&quot;"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols.2067083617" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="DEBUG"/>
+									<listOptionValue builtIn="false" value="USE_FULL_LL_DRIVER"/>
+									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
+									<listOptionValue builtIn="false" value="STM32L552xx"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.1343432666" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.1625969947" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script.1852946825" name="Linker Script (-T)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script" value="${workspace_loc:/${ProjName}/STM32L552ZETXQ_FLASH.ld}" valueType="string"/>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input.1577156318" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.615327291" name="MCU G++ Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls.1032571564" name="System calls" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.systemcalls.value.minimalimplementation" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.script.1475636464" name="Linker Script (-T)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.script" value="${workspace_loc:/${ProjName}/STM32L552ZETXQ_FLASH.ld}" valueType="string"/>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input.680587563" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.archiver.734897881" name="MCU GCC Archiver" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.archiver"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.size.1325575005" name="MCU Size" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.size"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objdump.listfile.1324209062" name="MCU Output Converter list file" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objdump.listfile"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.hex.1551785895" name="MCU Output Converter Hex" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.hex"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.binary.1775795944" name="MCU Output Converter Binary" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.binary"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.verilog.1620251182" name="MCU Output Converter Verilog" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.verilog"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.srec.2139504312" name="MCU Output Converter Motorola S-rec" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.srec"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec.1173187973" name="MCU Output Converter Motorola S-rec with symbols" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec"/>
+						</toolChain>
+					</folderInfo>
+					<sourceEntries>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
+					</sourceEntries>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+		</cconfiguration>
+		<cconfiguration id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.7888931">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.7888931" moduleId="org.eclipse.cdt.core.settings" name="Release">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release" cleanCommand="rm -rf" description="" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.7888931" name="Release" parent="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release">
+					<folderInfo id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.7888931." name="/" resourcePath="">
+						<toolChain id="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.release.728203904" name="MCU ARM GCC" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.release">
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu.77950820" name="MCU" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu" useByScannerDiscovery="true" value="STM32L552ZETxQ" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid.1077678011" name="CPU" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid" useByScannerDiscovery="false" value="0" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.1959054295" name="Core" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid" useByScannerDiscovery="false" value="0" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.1113952796" name="Floating-point unit" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.value.fpv5-sp-d16" valueType="enumerated"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.197395354" name="Floating-point ABI" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.value.hard" valueType="enumerated"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.19225912" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" useByScannerDiscovery="false" value="NUCLEO-L552ZE-Q" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.148911844" 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 || Release || false || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.value.workspace || NUCLEO-L552ZE-Q || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Core/Inc | ../Drivers/STM32L5xx_HAL_Driver/Inc | ../Drivers/STM32L5xx_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_CM33_NTZ/non_secure | ../Drivers/CMSIS/Device/ST/STM32L5xx/Include | ../Drivers/CMSIS/Include ||  ||  || USE_FULL_LL_DRIVER | USE_HAL_DRIVER | STM32L552xx ||  || Drivers | Core/Startup | Middlewares | Core ||  ||  || ${workspace_loc:/${ProjName}/STM32L552ZETXQ_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || None ||  ||  || " valueType="string"/>
+							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.1639273716" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
+							<builder buildPath="${workspace_loc:/stm32l552xx}/Release" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.1451151984" 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.948721132" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel.1288724204" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel.value.g0" valueType="enumerated"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.777458867" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/boardfiles/drivers/rfd}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/mavlink2}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/mavlink2/all}&quot;"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1194334019" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1349238348" name="MCU GCC Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.1034754405" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.value.g0" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.1819330918" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.value.os" valueType="enumerated"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.2112943806" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="USE_FULL_LL_DRIVER"/>
+									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
+									<listOptionValue builtIn="false" value="STM32L552xx"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.229822733" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32L5xx_HAL_Driver/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32L5xx_HAL_Driver/Inc/Legacy"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM33_NTZ/non_secure"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32L5xx/Include"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/boardfiles/drivers/rfd}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/mavlink2}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/mavlink2/all}&quot;"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.3478706" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.1419550440" name="MCU G++ Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel.1900656339" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel.value.g0" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.1183032016" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.value.os" valueType="enumerated"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths.951642822" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32L5xx_HAL_Driver/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32L5xx_HAL_Driver/Inc/Legacy"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/include"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2"/>
+									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM33_NTZ/non_secure"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32L5xx/Include"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/boardfiles/drivers/rfd}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/mavlink2}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/mavlink2/all}&quot;"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols.443515352" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="USE_FULL_LL_DRIVER"/>
+									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
+									<listOptionValue builtIn="false" value="STM32L552xx"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.1586176940" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.1582059483" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script.234442945" name="Linker Script (-T)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script" value="${workspace_loc:/${ProjName}/STM32L552ZETXQ_FLASH.ld}" valueType="string"/>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input.1495215391" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.1352750012" name="MCU G++ Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls.1673325298" name="System calls" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.systemcalls" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.systemcalls.value.minimalimplementation" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.script.1773595205" name="Linker Script (-T)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.script" value="${workspace_loc:/${ProjName}/STM32L552ZETXQ_FLASH.ld}" valueType="string"/>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input.1458641277" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.archiver.1919104319" name="MCU GCC Archiver" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.archiver"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.size.869916" name="MCU Size" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.size"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objdump.listfile.1262436774" name="MCU Output Converter list file" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objdump.listfile"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.hex.268572239" name="MCU Output Converter Hex" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.hex"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.binary.1047756887" name="MCU Output Converter Binary" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.binary"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.verilog.1903812461" name="MCU Output Converter Verilog" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.verilog"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.srec.1581295607" name="MCU Output Converter Motorola S-rec" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.srec"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec.83016208" name="MCU Output Converter Motorola S-rec with symbols" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec"/>
+						</toolChain>
+					</folderInfo>
+					<sourceEntries>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
diff --git a/stm32l552xx/FATFS/Target/user_diskio.c b/stm32l552xx/FATFS/Target/user_diskio.c
index 9816d583..a42985f1 100644
--- a/stm32l552xx/FATFS/Target/user_diskio.c
+++ b/stm32l552xx/FATFS/Target/user_diskio.c
@@ -33,7 +33,6 @@
 /* USER CODE BEGIN DECL */
 
 /* Includes ------------------------------------------------------------------*/
-#include "logger_config.h"
 #include "user_diskio.h"
 #include "user_diskio_sdmmc.h"
 #include "user_diskio_spi.h"
diff --git a/stm32l552xx/FATFS/Target/user_diskio.h b/stm32l552xx/FATFS/Target/user_diskio.h
index 876b7256..7f075ddf 100644
--- a/stm32l552xx/FATFS/Target/user_diskio.h
+++ b/stm32l552xx/FATFS/Target/user_diskio.h
@@ -29,13 +29,13 @@
 /* USER CODE BEGIN 0 */
 
 /* Includes ------------------------------------------------------------------*/
-#include "logger_config.h"
+//#include "logger_params.h"
 #include "ff_gen_drv.h"
 /* Exported types ------------------------------------------------------------*/
 /* Exported constants --------------------------------------------------------*/
 /* Exported functions ------------------------------------------------------- */
 
-#ifdef SD_CARD_LOGGING
+//#ifdef SD_CARD_LOGGING
 
 //#define SDMMC_INTERFACE
 #define SPI_INTERFACE
@@ -46,7 +46,7 @@
   #error Define a SD Card interface
 #endif
 
-#endif
+//#endif
 
 extern Diskio_drvTypeDef USER_Driver;
 
diff --git a/stm32l552xx/boardfiles/drivers/logger/logger.cpp b/stm32l552xx/boardfiles/drivers/logger/logger.cpp
deleted file mode 100644
index 2565d7f9..00000000
--- a/stm32l552xx/boardfiles/drivers/logger/logger.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-#include <cstdio>
-#include <cstring>
-#include "logger.hpp"
-
-int Logger::init() {
-#if defined(SD_CARD_LOGGING)
-    HAL_Delay(1000);
-
-    FRESULT res;
-
-    res = f_mount(&FatFs, "", 1);
-    if (res != FR_OK) {
-        return res;
-    }
-
-    FRESULT exist = FR_OK;
-    FILINFO fno;
-    int count = 1;
-
-    while (exist == FR_OK) {
-        snprintf(file, 100, "log%d.txt", count);
-        exist = f_stat(file, &fno);
-        count++;
-    }
-
-    return res;
-#elif defined(SWO_LOGGING)
-    return 0;
-#endif
-}
-
-int Logger::log(const char message[100]) {
-    char msgToSend[112]; //10 for timestamp, 100 for message, 2 for new line
-
-    uint32_t ts = (uint32_t)(osKernelGetTickCount() * 1.0 / osKernelGetTickFreq());
-    int tsStrLen = snprintf(msgToSend, 10, "%lus: ", ts);
-
-#if defined(SD_CARD_LOGGING)
-    FRESULT res;
-    res = f_open(&fil, file, FA_WRITE | FA_OPEN_APPEND);
-
-    snprintf(msgToSend + tsStrLen, 100, message);
-    snprintf(msgToSend + tsStrLen + strlen(message), 3, "\r\n");
-    res = (FRESULT)f_puts(msgToSend, &fil);
-
-    res = f_close(&fil);
-
-    return res;
-#elif defined(SWO_LOGGING)
-    snprintf(msgToSend + tsStrLen, 100, message);
-    snprintf(msgToSend + tsStrLen + strlen(message), 3, "\r\n");
-    printf("%s", msgToSend);
-    return 0;
-#endif
-}
-
-int Logger::log(const char message[][100], int count) {
-    char msgToSend[112]; //10 for timestamp, 100 for message, 2 for new line
-
-    uint32_t ts = (uint32_t)(osKernelGetTickCount() * 1.0 / osKernelGetTickFreq());
-    int tsStrLen = snprintf(msgToSend, 10, "%lus: ", ts);
-
-#if defined(SD_CARD_LOGGING)
-    FRESULT res;
-    res = f_open(&fil, file, FA_WRITE | FA_OPEN_APPEND);
-
-    for (int i = 0; i < count; i++) {
-      snprintf(msgToSend + tsStrLen, 100, message[i]);
-      snprintf(msgToSend + tsStrLen + strlen(message[i]), 3, "\r\n");
-      res = (FRESULT)f_puts(msgToSend, &fil);
-    }
-
-    res = f_close(&fil);
-
-    return res;
-#elif defined(SWO_LOGGING)
-    for (int i = 0; i < count; i++) {
-      snprintf(msgToSend + tsStrLen, 100, message[i]);
-      snprintf(msgToSend + tsStrLen + strlen(message[i]), 3, "\r\n");
-      printf("%s", msgToSend);
-    }
-    return 0;
-#endif
-}
diff --git a/stm32l552xx/boardfiles/drivers/logger/logger.hpp b/stm32l552xx/boardfiles/drivers/logger/logger.hpp
deleted file mode 100644
index bef4fafb..00000000
--- a/stm32l552xx/boardfiles/drivers/logger/logger.hpp
+++ /dev/null
@@ -1,35 +0,0 @@
-#pragma once
-
-#include "logger_iface.hpp"
-#include "app_fatfs.h"
-#include "logger_config.h"
-
-class Logger : public ILogger {
-    private:
-        FATFS FatFs;
-        FIL fil;
-        char file[100];
-
-    public:
-        Logger() = default;
-
-        /**
-         * @brief logs a single message to the SD Card
-         * @param message: data to be written
-         * @retval DRESULT: Operation result
-         */
-        int log(const char message[100]);
-
-        /**
-         * @brief logs multiple messages to the SD card
-         * @param messages: data to be written
-         * @retval DRESULT: Operation result
-         */
-        int log(const char messages[][100], int count);
-
-        /**
-         * @brief mounts SD card and selects file to write to, call before starting kernel
-         */
-        int init();
-
-};
diff --git a/stm32l552xx/boardfiles/drivers/logger/logger_config.h b/stm32l552xx/boardfiles/drivers/logger/logger_config.h
deleted file mode 100644
index c37f10f0..00000000
--- a/stm32l552xx/boardfiles/drivers/logger/logger_config.h
+++ /dev/null
@@ -1,10 +0,0 @@
-#pragma once
-
-#define SD_CARD_LOGGING
-//#define SWO_LOGGING
-
-#if defined(SD_CARD_LOGGING) && defined(SWO_LOGGING)
-  #error Only one can be defined
-#elif !defined(SD_CARD_LOGGING) && !defined(SWO_LOGGING)
-  #error Define a Logging interface
-#endif
diff --git a/stm32l552xx/boardfiles/drivers/rfd/rfd.cpp b/stm32l552xx/boardfiles/drivers/rfd/rfd.cpp
index 58492fa6..ae4cf99d 100644
--- a/stm32l552xx/boardfiles/drivers/rfd/rfd.cpp
+++ b/stm32l552xx/boardfiles/drivers/rfd/rfd.cpp
@@ -45,6 +45,7 @@ uint16_t RFD::receive(uint8_t* buffer, uint16_t bufferSize) {
 
 void RFD::receiveCallback(uint16_t size){
     writeIndex = size;
+	HAL_UARTEx_ReceiveToIdle_DMA(huart, rxBuffer, BUFFER_SIZE);
 }
 
 UART_HandleTypeDef* RFD::getHuart() const {
diff --git a/stm32l552xx/boardfiles/drivers/textIo/SDIO.cpp b/stm32l552xx/boardfiles/drivers/textIo/SDIO.cpp
new file mode 100644
index 00000000..3a16e388
--- /dev/null
+++ b/stm32l552xx/boardfiles/drivers/textIo/SDIO.cpp
@@ -0,0 +1,56 @@
+#include "SDIO.hpp"
+#include <cstring>
+
+SDIO::SDIO() {
+  //blank
+}
+
+int SDIO::mountFile() {
+    return f_mount(&FatFs, "", 1);
+}
+
+int SDIO::open(char *file, int modes) {
+    if (f_open(&fil, file, modes) != FR_OK) {
+        return 1; // Error opening file
+    }
+    return 0; // Success
+}
+
+int SDIO::close() {
+    if (f_close(&fil) != FR_OK) {
+        return 1; // Error closing file
+    }
+    return 0; // Success
+}
+
+char* SDIO::read(char *buffer, int bufferSize) {
+    return f_gets(buffer, bufferSize, &fil); // Reads a line from the file into the buffer
+}
+
+int SDIO::write(const char *buffer) {
+    return f_puts(buffer, &fil); // Writes a string to the file
+}
+
+int SDIO::seek(int offset) {
+    if (f_lseek(&fil, offset) != 0) {
+        return 1; // Error seeking in file
+    }
+    return 0; // Success
+}
+
+uint64_t SDIO::fsize() {
+  return f_size(&fil);
+}
+
+uint64_t SDIO::tell() {
+    return f_tell(&fil); // Returns the current position in the file
+}
+
+int SDIO::eof() {
+    return f_eof(&fil); // Returns whether the end of file has been reached
+}
+
+bool SDIO::checkFileExist(char *file) {
+    FILINFO fno;
+    return (f_stat(file, &fno) == FR_OK);
+}
\ No newline at end of file
diff --git a/stm32l552xx/boardfiles/drivers/textIo/SDIO.hpp b/stm32l552xx/boardfiles/drivers/textIo/SDIO.hpp
new file mode 100644
index 00000000..e32a2f24
--- /dev/null
+++ b/stm32l552xx/boardfiles/drivers/textIo/SDIO.hpp
@@ -0,0 +1,23 @@
+#pragma once
+
+#include "textio_iface.hpp"
+#include "app_fatfs.h"
+
+class SDIO : public ITextIO {
+    public:
+        SDIO();
+        int mountFile() override;
+        int open(char *file, int modes) override;
+        int close() override;
+        char* read(char *buffer, int bufferSize) override;
+        int write(const char *buffer) override;
+        int seek(int offset) override;
+        uint64_t fsize() override;
+        uint64_t tell() override;
+        int eof() override;
+        bool checkFileExist(char *file) override;
+
+    private:
+        FATFS FatFs;
+        FIL fil;
+};
diff --git a/stm32l552xx/boardfiles/model/inc/drivers.hpp b/stm32l552xx/boardfiles/model/inc/drivers.hpp
index 66c8f8df..b84ff5b9 100644
--- a/stm32l552xx/boardfiles/model/inc/drivers.hpp
+++ b/stm32l552xx/boardfiles/model/inc/drivers.hpp
@@ -8,6 +8,9 @@
 #include "rc.hpp"
 #include "rc_crsf.hpp"
 #include "rc_motor_control.hpp"
+#include "config_keys.hpp"
+#include "config_msg.hpp"
+#include "SDIO.hpp"
 #include "tm_queue.hpp"
 #include "mavlink.h"
 #include "queue.hpp"
@@ -19,6 +22,7 @@
 extern SystemUtils *systemUtilsHandle;
 
 extern IndependentWatchdog *iwdgHandle;
+extern SDIO *textIOHandle;
 extern Logger *loggerHandle;
 
 extern MotorControl *leftAileronMotorHandle;
@@ -38,7 +42,10 @@ extern PowerModule *pmHandle;
 
 extern MessageQueue<RCMotorControlMessage_t> *amRCQueueHandle;
 extern MessageQueue<char[100]> *smLoggerQueueHandle;
+extern MessageQueue<ConfigMessage_t> *smConfigAttitudeQueueHandle;
+extern IMessageQueue<ConfigMessage_t> *smConfigRouteQueueHandle[static_cast<size_t>(Owner_e::COUNT)];
 extern MessageQueue<TMMessage_t> *tmQueueHandle;
+extern MessageQueue<TMSMMessage_t> *tmSmQueueHandle;
 extern MessageQueue<mavlink_message_t> *messageBufferHandle;
 
 extern MotorGroupInstance_t rollMotors;
diff --git a/stm32l552xx/boardfiles/model/src/drivers.cpp b/stm32l552xx/boardfiles/model/src/drivers.cpp
index 55b9a4aa..b8f3bb82 100644
--- a/stm32l552xx/boardfiles/model/src/drivers.cpp
+++ b/stm32l552xx/boardfiles/model/src/drivers.cpp
@@ -45,6 +45,7 @@ alignas(MessageQueue<mavlink_message_t>) static uint8_t messageBufferStorage[siz
 // ----------------------------------------------------------------------------
 SystemUtils *systemUtilsHandle = nullptr;
 IndependentWatchdog *iwdgHandle = nullptr;
+SDIO *textIOHandle = nullptr;
 Logger *loggerHandle = nullptr;
 
 MotorControl *leftAileronMotorHandle = nullptr;
@@ -64,7 +65,10 @@ PowerModule *pmHandle = nullptr;
 
 MessageQueue<RCMotorControlMessage_t> *amRCQueueHandle = nullptr;
 MessageQueue<char[100]> *smLoggerQueueHandle = nullptr;
+IMessageQueue<ConfigMessage_t> *smConfigRouteQueueHandle[static_cast<size_t>(Owner_e::COUNT)] = {};
+MessageQueue<ConfigMessage_t> *smConfigAttitudeQueueHandle = nullptr;
 MessageQueue<TMMessage_t> *tmQueueHandle = nullptr;
+MessageQueue<TMSMMessage_t> *tmSmQueueHandle = nullptr;
 MessageQueue<mavlink_message_t> *messageBufferHandle = nullptr;
 
 // ----------------------------------------------------------------------------
@@ -97,7 +101,8 @@ void initDrivers()
     // Core utilities
     systemUtilsHandle = new (&systemUtilsStorage) SystemUtils();
     iwdgHandle = new (&iwdgStorage) IndependentWatchdog(&hiwdg);
-    loggerHandle = new (&loggerStorage) Logger(); // Initialized later in RTOS task
+    textIOHandle = new SDIO();
+    loggerHandle = new (&loggerStorage) Logger(textIOHandle, systemUtilsHandle); // Initialized later in RTOS task
 
     // Motors
     leftAileronMotorHandle = new (&leftAileronMotorStorage) MotorControl(&htim3, TIM_CHANNEL_1, 5, 10);
@@ -119,7 +124,13 @@ void initDrivers()
     // Queues
     amRCQueueHandle = new (&amRCQueueStorage) MessageQueue<RCMotorControlMessage_t>(&amQueueId);
     smLoggerQueueHandle = new (&smLoggerQueueStorage) MessageQueue<char[100]>(&smLoggerQueueId);
+    smConfigAttitudeQueueHandle = new MessageQueue<ConfigMessage_t>(&smConfigAttitudeQueueId);
+    smConfigRouteQueueHandle[static_cast<size_t>(Owner_e::ATTITUDE_MANAGER)] = smConfigAttitudeQueueHandle;
+    // Add other manager queues to smConfigRouteQueueHandle as needed
+
+    loggerHandle->init();
     tmQueueHandle = new (&tmQueueStorage) MessageQueue<TMMessage_t>(&tmQueueId);
+    tmSmQueueHandle = new MessageQueue<TMSMMessage_t>(&tmSmQueueId);
     messageBufferHandle = new (&messageBufferStorage) MessageQueue<mavlink_message_t>(&messageBufferId);
 
     // Initialize hardware components
@@ -136,6 +147,7 @@ void initDrivers()
     gpsHandle->init();
     imuHandle->init();
     pmHandle->init();
+    rfdHandle->startReceive();
 
     // Motor instance bindings
     leftAileronMotorInstance = {leftAileronMotorHandle, true};
diff --git a/stm32l552xx/boardfiles/model/src/managers.cpp b/stm32l552xx/boardfiles/model/src/managers.cpp
index fb8a8d0b..d2fb2d1a 100644
--- a/stm32l552xx/boardfiles/model/src/managers.cpp
+++ b/stm32l552xx/boardfiles/model/src/managers.cpp
@@ -11,6 +11,11 @@ alignas(TelemetryManager) static uint8_t tmHandleStorage[sizeof(TelemetryManager
 AttitudeManager *amHandle = nullptr;
 SystemManager *smHandle = nullptr;
 TelemetryManager *tmHandle = nullptr;
+DirectMapping *flightMode = nullptr;
+
+Config *configHandle = nullptr;
+
+
 
 void initManagers()
 {
@@ -21,7 +26,8 @@ void initManagers()
         imuHandle,
         amRCQueueHandle, 
         tmQueueHandle, 
-        smLoggerQueueHandle, 
+        smLoggerQueueHandle,
+        smConfigAttitudeQueueHandle,
         &rollMotors, 
         &pitchMotors, 
         &yawMotors, 
@@ -31,15 +37,19 @@ void initManagers()
     );
 
     // SM initialization
+    configHandle = new Config(textIOHandle);
     smHandle = new (&smHandleStorage) SystemManager(
+    	loggerHandle,
+    	configHandle,
         systemUtilsHandle, 
         iwdgHandle,
-        loggerHandle,
         rcHandle,
 		pmHandle,
         amRCQueueHandle,
         tmQueueHandle,
-        smLoggerQueueHandle
+        tmSmQueueHandle, 
+        smLoggerQueueHandle, 
+        smConfigRouteQueueHandle
     );
 
     // TM initialization
@@ -47,6 +57,7 @@ void initManagers()
         systemUtilsHandle,
         rfdHandle,
         tmQueueHandle,
+        tmSmQueueHandle,
         amRCQueueHandle,
         messageBufferHandle
     );
diff --git a/stm32l552xx/boardfiles/rtos/museq/museq.cpp b/stm32l552xx/boardfiles/rtos/museq/museq.cpp
index 34d05f6a..c2819b63 100644
--- a/stm32l552xx/boardfiles/rtos/museq/museq.cpp
+++ b/stm32l552xx/boardfiles/rtos/museq/museq.cpp
@@ -1,5 +1,6 @@
 #include "museq.hpp"
 #include "rc_motor_control.hpp"
+#include "config_msg.hpp"
 #include "tm_queue.hpp"
 #include "mavlink.h"
 
@@ -8,7 +9,9 @@
 osMutexId_t itmMutex;
 osMessageQueueId_t amQueueId;
 osMessageQueueId_t smLoggerQueueId;
+osMessageQueueId_t smConfigAttitudeQueueId;
 osMessageQueueId_t tmQueueId;
+osMessageQueueId_t tmSmQueueId;
 osMessageQueueId_t messageBufferId;
 
 static const osMutexAttr_t itmMutexAttr = {
@@ -41,6 +44,8 @@ void initQueues()
 {
   amQueueId = osMessageQueueNew(16, sizeof(RCMotorControlMessage_t), NULL);
   smLoggerQueueId = osMessageQueueNew(16, sizeof(char[100]), NULL);
+  smConfigAttitudeQueueId = osMessageQueueNew(16, sizeof(ConfigMessage_t), NULL);
   tmQueueId = osMessageQueueNew(16, sizeof(TMMessage_t), NULL);
+  tmSmQueueId = osMessageQueueNew(16, sizeof(TMSMMessage_t), NULL);
   messageBufferId = osMessageQueueNew(16, sizeof(mavlink_message_t), NULL);
 }
diff --git a/stm32l552xx/boardfiles/rtos/museq/museq.hpp b/stm32l552xx/boardfiles/rtos/museq/museq.hpp
index b50eca44..ff40b4d3 100644
--- a/stm32l552xx/boardfiles/rtos/museq/museq.hpp
+++ b/stm32l552xx/boardfiles/rtos/museq/museq.hpp
@@ -19,7 +19,9 @@ void initSemphrs();
 /* declare queues begin */
 extern osMessageQueueId_t amQueueId;
 extern osMessageQueueId_t smLoggerQueueId;
+extern osMessageQueueId_t smConfigAttitudeQueueId;
 extern osMessageQueueId_t tmQueueId;
+extern osMessageQueueId_t tmSmQueueId;
 extern osMessageQueueId_t messageBufferId;
 /* declare queues end */
 
diff --git a/zeropilot4.0/common.cmake b/zeropilot4.0/common.cmake
index 7a6a425a..49854640 100644
--- a/zeropilot4.0/common.cmake
+++ b/zeropilot4.0/common.cmake
@@ -11,9 +11,12 @@ set(AM_INC
 # system manager files
 set(SM_SRC
     "src/system_manager/system_manager.cpp"
+    "src/system_manager/logger.cpp"
+    "src/system_manager/config.cpp"
 )
 set(SM_INC
     "include/system_manager/"
+    "include/system_manager/config_utils/"
 )
 
 # telemetry manager files
diff --git a/zeropilot4.0/include/attitude_manager/attitude_manager.hpp b/zeropilot4.0/include/attitude_manager/attitude_manager.hpp
index 898bdf36..f214cae8 100644
--- a/zeropilot4.0/include/attitude_manager/attitude_manager.hpp
+++ b/zeropilot4.0/include/attitude_manager/attitude_manager.hpp
@@ -4,6 +4,7 @@
 #include "systemutils_iface.hpp"
 #include "direct_mapping.hpp"
 #include "motor_datatype.hpp"
+#include "config_msg.hpp"
 #include "gps_iface.hpp"
 #include "tm_queue.hpp"
 #include "imu_iface.hpp"
@@ -33,6 +34,7 @@ class AttitudeManager {
             IMessageQueue<RCMotorControlMessage_t> *amQueue,
             IMessageQueue<TMMessage_t> *tmQueue,
             IMessageQueue<char[100]> *smLoggerQueue,
+            IMessageQueue<ConfigMessage_t> *smConfigAttitudeQueue,
             MotorGroupInstance_t *rollMotors,
             MotorGroupInstance_t *pitchMotors,
             MotorGroupInstance_t *yawMotors,
@@ -54,6 +56,7 @@ class AttitudeManager {
         IMessageQueue<RCMotorControlMessage_t> *amQueue;
         IMessageQueue<TMMessage_t> *tmQueue;
         IMessageQueue<char[100]> *smLoggerQueue;
+        IMessageQueue<ConfigMessage_t> *smConfigAttitudeQueue;
 
         DirectMapping controlAlgorithm;
         RCMotorControlMessage_t controlMsg;
@@ -73,6 +76,8 @@ class AttitudeManager {
 
         bool getControlInputs(RCMotorControlMessage_t *pControlMsg);
 
+        void handleConfigChanges();
+
         void outputToMotor(ControlAxis_t axis, uint8_t percent);
 
         void sendGPSDataToTelemetryManager(const GpsData_t &gpsData, const bool &armed);
diff --git a/zeropilot4.0/include/driver_ifaces/logger_iface.hpp b/zeropilot4.0/include/driver_ifaces/logger_iface.hpp
deleted file mode 100644
index 1d88abd8..00000000
--- a/zeropilot4.0/include/driver_ifaces/logger_iface.hpp
+++ /dev/null
@@ -1,12 +0,0 @@
-#pragma once
-
-class ILogger {
-    protected:
-        ILogger() = default;
-    
-    public:
-        virtual ~ILogger() = default;
-
-        virtual int log(const char message[100]) = 0;
-        virtual int log(const char message[][100], int count) = 0;
-};
diff --git a/zeropilot4.0/include/driver_ifaces/textio_iface.hpp b/zeropilot4.0/include/driver_ifaces/textio_iface.hpp
new file mode 100644
index 00000000..c455a8b0
--- /dev/null
+++ b/zeropilot4.0/include/driver_ifaces/textio_iface.hpp
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <cstdint>
+class ITextIO {
+    protected:
+        ITextIO() = default;
+
+    public:
+        ~ITextIO() = default;
+
+        virtual int mountFile() = 0;
+        virtual int open(char *file, int modes) = 0;
+        virtual int close() = 0;
+        virtual char* read(char *buffer, int bufferSize) = 0;
+        virtual int write(const char *buffer) = 0;
+        virtual int seek(int offset) = 0;
+        virtual uint64_t fsize() = 0;
+        virtual uint64_t tell() = 0;
+        virtual int eof() = 0;
+        virtual bool checkFileExist(char *file) = 0;
+};
diff --git a/zeropilot4.0/include/system_manager/config.hpp b/zeropilot4.0/include/system_manager/config.hpp
new file mode 100644
index 00000000..5a85d0a3
--- /dev/null
+++ b/zeropilot4.0/include/system_manager/config.hpp
@@ -0,0 +1,79 @@
+#pragma once
+
+#include "config_keys.hpp"
+#include "textio_iface.hpp"
+
+class Config {
+    private:
+        char configFile[100];
+        ITextIO *textIO;
+
+        std::array<Param_t, NUM_KEYS> config_table = CONFIG_VALUES;
+
+        /**
+         * @brief Moves read/write pointer to parameter
+         * @param param parameter to find
+         * @param val buffer to store value at parameter
+         * @param tableIdx buffer to store config table index of parameter
+         * @retval Operation success
+         */
+        int findParam(const char param[100], float &val, int &tableIdx);
+        int findParam(const char param[100], float &val);
+        int findParam(const char param[100], int &tableIdx);
+
+        void getXCharNum(float num, char final[MAX_VALUE_LENGTH + 1]);
+
+    public:
+        Config(ITextIO *textIO);
+
+        /**
+         * @brief Adds textIO object and reads config file
+         * @retval Operation success
+         */
+        int init();
+
+        /**
+         * @brief reads parameter from config file
+         * @param param parameter to read
+         * @param val buffer to store value
+         * @retval Operation success
+         */
+        int readParam(size_t key, float &val);
+
+        /**
+         * @brief writes parameter from config file
+         * @param param parameter to write too
+         * @param newValue updated value
+         * @retval Operation success
+         */
+        int writeParam(size_t key, float newValue);
+
+        /**
+         * @brief writes parameter from config file by name
+         * @param param parameter to write too
+         * @param newValue updated value
+         * @retval Operation success
+         */
+        int writeParamByName(const char param[MAX_KEY_LENGTH], float newValue);
+
+        /**
+         * @brief gets the index of a parameter by name
+         * @param param parameter to get index for
+         * @retval index of the parameter
+         */
+        size_t getParamConfigKey(const char param[MAX_KEY_LENGTH]);
+
+        /**
+         * @brief gets the owner of a parameter
+         * @param key key of parameter to get owner for
+         * @retval Owner of the parameter
+         */
+        Owner_e getParamOwner(size_t key);
+
+        /**
+         * @brief gets parameter struct by key
+         * @param key parameter key to get
+         * @retval Parameter struct
+         */
+        Param_t getParam(size_t key);
+};
\ No newline at end of file
diff --git a/zeropilot4.0/include/system_manager/config_utils/config_defines.hpp b/zeropilot4.0/include/system_manager/config_utils/config_defines.hpp
new file mode 100644
index 00000000..11d683f9
--- /dev/null
+++ b/zeropilot4.0/include/system_manager/config_utils/config_defines.hpp
@@ -0,0 +1,9 @@
+#pragma once
+
+#define FA_READ             0x01
+#define FA_WRITE            0x02
+#define FA_OPEN_EXISTING    0x00
+#define FA_CREATE_NEW       0x04
+#define FA_CREATE_ALWAYS    0x08
+#define FA_OPEN_ALWAYS      0x10
+#define FA_OPEN_APPEND      0x30
\ No newline at end of file
diff --git a/zeropilot4.0/include/system_manager/config_utils/config_keys.hpp b/zeropilot4.0/include/system_manager/config_utils/config_keys.hpp
new file mode 100644
index 00000000..33dc7c6f
--- /dev/null
+++ b/zeropilot4.0/include/system_manager/config_utils/config_keys.hpp
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <cstddef>
+#include <array>
+
+#define MAX_KEY_LENGTH 17
+#define MAX_VALUE_LENGTH 10 // 1 for comma separator
+#define MAX_LINE_LENGTH (MAX_KEY_LENGTH + MAX_VALUE_LENGTH + 2) // +2 for newline and separator (,)
+
+// Managers that own config keys
+enum class Owner_e : size_t {
+    ATTITUDE_MANAGER = 0,
+    COUNT,
+};
+
+struct Param_t {
+    char key[MAX_KEY_LENGTH] = {};
+    float value;
+    Owner_e owner;
+    bool reboot_on_change = false;
+
+    constexpr Param_t(const char *k, float v, Owner_e o, bool r = false) : value(v), owner(o), reboot_on_change(r) {
+        for (size_t i = 0; i < MAX_KEY_LENGTH - 1; ++i) {
+            key[i] = k[i];
+            if (k[i] == '\0') break;
+        }
+    }
+};
+
+constexpr std::array<Param_t, 4> CONFIG_VALUES = {{
+    {"baud_rate", 0.0, Owner_e::ATTITUDE_MANAGER, false}, // Example
+    {"p", 100.0, Owner_e::ATTITUDE_MANAGER, false},
+	{"i", 25.0, Owner_e::ATTITUDE_MANAGER, false},
+	{"d", 10.0, Owner_e::ATTITUDE_MANAGER, false}
+}};
+
+static constexpr size_t NUM_KEYS = CONFIG_VALUES.size();
diff --git a/stm32h753iitx/boardfiles/drivers/logger/logger.hpp b/zeropilot4.0/include/system_manager/logger.hpp
similarity index 57%
rename from stm32h753iitx/boardfiles/drivers/logger/logger.hpp
rename to zeropilot4.0/include/system_manager/logger.hpp
index b7588fe3..b0b449a2 100644
--- a/stm32h753iitx/boardfiles/drivers/logger/logger.hpp
+++ b/zeropilot4.0/include/system_manager/logger.hpp
@@ -1,28 +1,29 @@
 #pragma once
 
-#include "logger_iface.hpp"
-#include "fatfs.h"
-#include "logger_config.h"
+#include "logger_params.h"
+#include "textio_iface.hpp"
+#include "systemutils_iface.hpp"
 
-class Logger : public ILogger {
+class Logger {
     private:
-        FATFS FatFs;
-        FIL fil;
-        char file[100];
+        char logFile[100];
+        ITextIO *textIO;
+        ISystemUtils *sysUtils;
 
     public:
-        Logger() = default;
+        Logger(ITextIO *textIO, ISystemUtils *sysUtils);
 
         /**
          * @brief logs a single message to the SD Card
-         * @param message: data to be written
+         * @param message data to be written
          * @retval DRESULT: Operation result
          */
         int log(const char message[100]);
 
         /**
          * @brief logs multiple messages to the SD card
-         * @param messages: data to be written
+         * @param messages data to be written
+         * @param count number of messages to write
          * @retval DRESULT: Operation result
          */
         int log(const char messages[][100], int count);
diff --git a/stm32h753iitx/boardfiles/drivers/logger/logger_config.h b/zeropilot4.0/include/system_manager/logger_params.h
similarity index 100%
rename from stm32h753iitx/boardfiles/drivers/logger/logger_config.h
rename to zeropilot4.0/include/system_manager/logger_params.h
diff --git a/zeropilot4.0/include/system_manager/system_manager.hpp b/zeropilot4.0/include/system_manager/system_manager.hpp
index a029eb13..be19e30b 100644
--- a/zeropilot4.0/include/system_manager/system_manager.hpp
+++ b/zeropilot4.0/include/system_manager/system_manager.hpp
@@ -3,9 +3,12 @@
 #include "iwdg_iface.hpp"
 #include "systemutils_iface.hpp"
 #include "mavlink.h"
-#include "logger_iface.hpp"
+#include "logger.hpp"
+#include "config.hpp"
+#include "queue_iface.hpp"
 #include "rc_iface.hpp"
 #include "rc_motor_control.hpp"
+#include "config_msg.hpp"
 #include "iwdg_iface.hpp"
 #include "tm_queue.hpp"
 #include "queue_iface.hpp"
@@ -17,34 +20,45 @@
 class SystemManager {
     public:
         SystemManager(
+        	Logger *logger,
+			Config *config,
             ISystemUtils *systemUtilsDriver,
             IIndependentWatchdog *iwdgDriver,
-            ILogger *loggerDriver,
             IRCReceiver *rcDriver,
 			IPowerModule *pmDriver,
             IMessageQueue<RCMotorControlMessage_t> *amRCQueue,
             IMessageQueue<TMMessage_t> *tmQueue,
-            IMessageQueue<char[100]> *smLoggerQueue
+            IMessageQueue<TMSMMessage_t> *tmSmQueue,
+            IMessageQueue<char[100]> *smLoggerQueue,
+            IMessageQueue<ConfigMessage_t> **smConfigRouteQueue
         );
 
         void smUpdate(); // This function is the main function of SM, it should be called in the main loop of the system.
 
     private:
-        ISystemUtils *systemUtilsDriver; // System utilities instance
+        Logger *logger; // Logger 
+        Config *config; // Config manager
 
+        ISystemUtils *systemUtilsDriver; // System utilities instance
         IIndependentWatchdog *iwdgDriver; // Independent Watchdog driver
-        ILogger *loggerDriver; // Logger driver
+
+
         IRCReceiver *rcDriver; // RC receiver driver
         IPowerModule *pmDriver;
         
         IMessageQueue<RCMotorControlMessage_t> *amRCQueue; // Queue driver for tx communication to the Attitude Manager
         IMessageQueue<TMMessage_t> *tmQueue; // Queue driver for tx communication to the Telemetry Manager
+        IMessageQueue<TMSMMessage_t> *tmSmQueue; // Queue driver for rx communication from Telemetry Manager to System Manager
         IMessageQueue<char[100]> *smLoggerQueue; // Queue driver for rx communication from other modules to the System Manager for logging
+        IMessageQueue<ConfigMessage_t> *smConfigRouteQueue[static_cast<size_t>(Owner_e::COUNT)] = {}; // Array of queues for routing config messages to respective managers
 
         uint8_t smSchedulingCounter;
+        int paramAmountSent = -1; // Counter to keep track of how many params have been sent to TM
 
         void sendRCDataToAttitudeManager(const RCControl &rcData);
         void sendRCDataToTelemetryManager(const RCControl &rcData);
         void sendHeartbeatDataToTelemetryManager(uint8_t baseMode, uint32_t customMode, MAV_STATE systemStatus);
+        void sendParamDataToTelemetryManager();
         void sendMessagesToLogger();
+        void handleMessagesFromTelemetryManager();
 };
diff --git a/zeropilot4.0/include/telemetry_manager/telemetry_manager.hpp b/zeropilot4.0/include/telemetry_manager/telemetry_manager.hpp
index 6c86f163..77890e20 100644
--- a/zeropilot4.0/include/telemetry_manager/telemetry_manager.hpp
+++ b/zeropilot4.0/include/telemetry_manager/telemetry_manager.hpp
@@ -1,7 +1,7 @@
 #pragma once
 #define MAVLINK_MSG_MAX_SIZE 280
 #define MAVLINK_MAX_IDENTIFIER_LEN 17
-#define RX_BUFFER_LEN 8192
+#define RX_BUFFER_LEN 2048
 
 #include "systemutils_iface.hpp"
 #include "mavlink.h"
@@ -13,13 +13,16 @@ class TelemetryManager {
   private:
     ISystemUtils *systemUtilsDriver;			                  // System Utils Driver
     IRFD *rfdDriver;										                    // Driver used to actually send mavlink messages
-    IMessageQueue<TMMessage_t> *tmQueueDriver;				      // Driver that receives messages from other managers
+    IMessageQueue<TMMessage_t> *tmRxQueue;				    // Driver that receives messages from other managers
+    IMessageQueue<TMSMMessage_t> *tmTxQueue;            	// Driver that sends messages to system manager
     IMessageQueue<RCMotorControlMessage_t> *amQueueDriver;	// Driver that currently is only used to set arm/disarm
-    IMessageQueue<mavlink_message_t> *messageBuffer{};	  	// GPOS, Attitude and Heartbeat/Connection Messages
+    IMessageQueue<mavlink_message_t> *mavlinkTxQueue{};	  	// GPOS, Attitude, Heartbeat/Connection, and Param Messages
+
     mavlink_status_t status;
     mavlink_message_t message;
     mavlink_message_t overflowBuf;
     bool overflowMsgPending;
+    bool transmittingParams = false; // Flag will be set when TM receives a param list request and unset when finished
 
     void handleRxMsg(const mavlink_message_t &msg);
     void processMsgQueue();
@@ -27,7 +30,14 @@ class TelemetryManager {
     void reconstructMsg();
 
   public:
-    TelemetryManager(ISystemUtils *systemUtilsDriver, IRFD *rfdDriver, IMessageQueue<TMMessage_t>  *tmQueueDriver,  IMessageQueue<RCMotorControlMessage_t> *amQueueDriver,IMessageQueue<mavlink_message_t> *messageBuffer);
+    TelemetryManager(
+    		ISystemUtils *systemUtilsDriver,
+			IRFD *rfdDriver,
+			IMessageQueue<TMMessage_t> *tmRxQueue,
+			IMessageQueue<TMSMMessage_t> *tmTxQueue,
+			IMessageQueue<RCMotorControlMessage_t> *amQueueDriver,
+			IMessageQueue<mavlink_message_t> *mavlinkTxQueue
+	);
     ~TelemetryManager();
 
     void tmUpdate();
diff --git a/zeropilot4.0/include/thread_msgs/config_msg.hpp b/zeropilot4.0/include/thread_msgs/config_msg.hpp
new file mode 100644
index 00000000..49760338
--- /dev/null
+++ b/zeropilot4.0/include/thread_msgs/config_msg.hpp
@@ -0,0 +1,8 @@
+#pragma once
+
+#include <cstddef>
+
+typedef struct {
+    size_t key;
+    float value;
+} ConfigMessage_t;
\ No newline at end of file
diff --git a/zeropilot4.0/include/thread_msgs/tm_queue.hpp b/zeropilot4.0/include/thread_msgs/tm_queue.hpp
index ba51cadc..a1345053 100644
--- a/zeropilot4.0/include/thread_msgs/tm_queue.hpp
+++ b/zeropilot4.0/include/thread_msgs/tm_queue.hpp
@@ -1,5 +1,11 @@
 #pragma once
 #include <cstdint>
+#include <mavlink.h>
+#include "config_keys.hpp"
+
+enum class TMSMRequest {
+    REQUEST_PARAMS,
+};
 
 typedef union TMMessageData_u {
   struct{
@@ -56,6 +62,13 @@ typedef union TMMessageData_u {
       float pitchspeed;
       float yawspeed;
   } attitudeData;
+  struct{ 
+    char key[MAX_KEY_LENGTH];
+    float value;
+    uint8_t type; // Basically always MAV_PARAM_TYPE_REAL64 which is 64bit float
+    uint16_t count;
+    uint16_t index;
+  } paramData;
 } TMMessageData_t;
 
 typedef struct TMMessage{
@@ -65,7 +78,8 @@ typedef struct TMMessage{
         RC_DATA,
         BM_DATA,
         RAW_IMU_DATA,
-        ATTITUDE_DATA
+        ATTITUDE_DATA,
+        PARAM_VALUE_DATA
     } dataType;
     TMMessageData_t tmMessageData;
     uint32_t timeBootMs = 0;
@@ -123,3 +137,42 @@ inline TMMessage_t attitudeDataPack(uint32_t time_boot_ms, float roll, float pit
     const TMMessageData_t DATA = {.attitudeData ={roll, pitch, yaw, rollspeed, pitchspeed, yawspeed }};
     return TMMessage_t{TMMessage_t::ATTITUDE_DATA, DATA, time_boot_ms};
 }
+
+inline TMMessage_t paramDataPack(uint32_t time_boot_ms, uint16_t index, uint16_t count, Param_t param) {
+    TMMessageData_t DATA = {.paramData = { .value = param.value, .type = MAV_PARAM_TYPE_REAL64, .count = count, .index = index}};
+    strncpy(DATA.paramData.key, param.key, MAX_KEY_LENGTH - 1);
+    DATA.paramData.key[MAX_KEY_LENGTH - 1] = '\0';  // Ensure null termination
+    return TMMessage_t{TMMessage_t::PARAM_VALUE_DATA, DATA, time_boot_ms};
+}
+
+typedef union TMSMMessage_u {
+    struct {
+        char keyId[MAX_KEY_LENGTH];
+        float value;
+    } paramChangeData;
+    struct {
+        TMSMRequest requestType;
+    } requestData;
+} TMSMMessageData_t;
+
+typedef struct TMSMMessage{
+    enum{
+        PARAM_CHANGE_DATA,
+        REQUEST_DATA
+    } dataType;
+    TMSMMessageData_t tmSMMessageData;
+    uint32_t timeBootMs = 0;
+} TMSMMessage_t;
+
+inline TMSMMessage_t paramChangePack(uint32_t time_boot_ms, const char* keyId, float value) {
+    TMSMMessageData_t DATA = {.paramChangeData = {}};
+    strncpy(DATA.paramChangeData.keyId, keyId, MAX_KEY_LENGTH - 1);
+    DATA.paramChangeData.keyId[MAX_KEY_LENGTH - 1] = '\0';  // Ensure null termination
+    DATA.paramChangeData.value = value;
+    return TMSMMessage_t{TMSMMessage_t::PARAM_CHANGE_DATA, DATA, time_boot_ms};
+}
+
+inline TMSMMessage_t requestPack(uint32_t time_boot_ms, TMSMRequest requestType) {
+    const TMSMMessageData_t DATA = {.requestData={.requestType = requestType}};
+    return TMSMMessage_t{TMSMMessage_t::REQUEST_DATA, DATA, time_boot_ms};
+}
diff --git a/zeropilot4.0/src/attitude_manager/attitude_manager.cpp b/zeropilot4.0/src/attitude_manager/attitude_manager.cpp
index 5c0f8d32..877fffb4 100644
--- a/zeropilot4.0/src/attitude_manager/attitude_manager.cpp
+++ b/zeropilot4.0/src/attitude_manager/attitude_manager.cpp
@@ -14,6 +14,7 @@ AttitudeManager::AttitudeManager(
     IMessageQueue<RCMotorControlMessage_t> *amQueue,
     IMessageQueue<TMMessage_t> *tmQueue,
     IMessageQueue<char[100]> *smLoggerQueue,
+    IMessageQueue<ConfigMessage_t> *smConfigAttitudeQueue,
     MotorGroupInstance_t *rollMotors,
     MotorGroupInstance_t *pitchMotors,
     MotorGroupInstance_t *yawMotors,
@@ -27,6 +28,7 @@ AttitudeManager::AttitudeManager(
     amQueue(amQueue),
     tmQueue(tmQueue),
     smLoggerQueue(smLoggerQueue),
+    smConfigAttitudeQueue(smConfigAttitudeQueue),
     controlAlgorithm(),
     droneState(DRONE_STATE_DEFAULT),
     rollMotors(rollMotors),
@@ -123,6 +125,9 @@ void AttitudeManager::amUpdate() {
     outputToMotor(THROTTLE, motorOutputs.throttle);
     outputToMotor(FLAP_ANGLE, motorOutputs.flapAngle);
     outputToMotor(STEERING, motorOutputs.yaw);
+
+    // Handle config changes
+    handleConfigChanges();
 }
 
 bool AttitudeManager::getControlInputs(RCMotorControlMessage_t *pControlMsg) {
@@ -134,6 +139,21 @@ bool AttitudeManager::getControlInputs(RCMotorControlMessage_t *pControlMsg) {
     return true;
 }
 
+void AttitudeManager::handleConfigChanges() {
+    if (smConfigAttitudeQueue->count() == 0) {
+        return;
+    }
+    ConfigMessage_t *pConfigMsg = new ConfigMessage_t;
+    smConfigAttitudeQueue->get(pConfigMsg);
+    // Handle config changes here for whatever config keys the attitude manager owns
+    char logMsg[100];
+    snprintf(logMsg, 100, "AttitudeManager: Config key %zu changed to value %f", pConfigMsg->key, pConfigMsg->value);
+    smLoggerQueue->push(&logMsg);
+
+    delete pConfigMsg;
+    return;
+}
+
 void AttitudeManager::outputToMotor(ControlAxis_t axis, uint8_t percent) {
     MotorGroupInstance_t *motorGroup = nullptr;
 
diff --git a/zeropilot4.0/src/system_manager/config.cpp b/zeropilot4.0/src/system_manager/config.cpp
new file mode 100644
index 00000000..8f559ce0
--- /dev/null
+++ b/zeropilot4.0/src/system_manager/config.cpp
@@ -0,0 +1,197 @@
+#include "config.hpp"
+#include "config_utils/config_defines.hpp"
+#include <cstring>
+#include <cstdio>
+#include <string>
+
+Config::Config(ITextIO* textIO) : textIO(textIO) {
+  //blank
+}
+
+void Config::getXCharNum(float num, char final[MAX_VALUE_LENGTH + 1]) {
+    char temp[128];
+    if (num > 1e-6) {
+      snprintf(temp, 128, "%f", num);
+
+      //Remove trailing zeros after decimal
+      char* dot = strchr(temp, '.');
+      if (dot) {
+          char* end = temp + strlen(temp) - 1;
+          while(end > dot && *end == '0') --end;
+          if (*end == '.') --end;
+          *(end + 1) = '\0';
+      }
+    } else {
+      snprintf(temp, 128, "%g", num);
+    }
+
+    if (strlen(temp) <= MAX_VALUE_LENGTH) {
+        int len = snprintf(final, MAX_VALUE_LENGTH + 1, temp);
+        // Add spaces
+        for (int i = len; i < MAX_VALUE_LENGTH; i++) {
+            final[i] = ' ';
+        }
+        final[MAX_VALUE_LENGTH] = '\0';
+    } else {
+        snprintf(final, MAX_VALUE_LENGTH + 1, "%.4e", num);
+    }
+}
+
+int Config::init() {
+  snprintf(configFile, 100, "config.txt");
+  if (textIO->open(configFile, FA_OPEN_EXISTING) != 0) {
+    return 1; // Error opening file
+  }
+
+  // Table was already initialized with default values so we loop and find all params in file
+  for (size_t i = 0; i < NUM_KEYS; i++) {
+    // findParam sets the value in config_table[i].value with the value read from the file if it exists
+    // If the parameter does not exist in the file, it will not change the value in config_table[i].value
+    findParam(config_table[i].key, config_table[i].value);
+  }
+
+  // Close the file after reading all parameters
+  if (textIO->close() != 0) {
+    return 2; // Error closing file
+  }
+  return 0;
+}
+
+int Config::findParam(const char param[MAX_KEY_LENGTH], float &val, int &tableIdx) {
+  // move read/write pointer to param value
+  char *key = nullptr, *value = nullptr;
+  char msg[MAX_LINE_LENGTH];
+  int length;
+  
+  while (textIO->read(msg, sizeof(msg)) != nullptr) {
+    key = strtok(msg, ",");
+    value = strtok(NULL, ",");
+
+    if (!strcmp(param, key)) break;
+  }
+
+  if (key == nullptr || value == nullptr) return 2;
+  if (strcmp(param, key)) return 2;
+
+  length = (textIO->eof() != 0) ? strlen(value) : strlen(value) + 2; //accounting for \n
+
+  textIO->seek(textIO->tell() - length);
+
+  val = std::atof(value);
+
+  // find idx of param in config table
+  if (tableIdx != -1) {
+    for (size_t i = 0; i < NUM_KEYS; i++) {
+      if (!strcmp(param, config_table[i].key)) {
+        tableIdx = static_cast<int>(i);
+        break;
+      }
+    }
+  }
+
+  return 0;
+}
+
+int Config::findParam(const char param[100], float &val)  {
+  int tableIdx = -1;
+  return findParam(param, val, tableIdx);
+}
+
+int Config::findParam(const char param[100], int &tableIdx)  {
+  float val;
+  return findParam(param, val, tableIdx);
+}
+
+int Config::readParam(size_t key, float &val) {
+  if (textIO->open(configFile, FA_READ | FA_WRITE) != 0) {
+      return 1;
+  }
+  
+  int res = findParam(config_table[static_cast<size_t>(key)].key, val);
+
+  textIO->close();
+
+  if (res != 0) {
+    return 2;
+  }
+
+  return 0;
+}
+
+int Config::writeParam(size_t key, float newValue) {
+  char strValue[MAX_VALUE_LENGTH + 1];
+  getXCharNum(newValue, strValue);
+
+  if (textIO->open(configFile, FA_READ | FA_WRITE) != 0) {
+    return 1;
+  }
+
+  float val;
+  int tblIdx;
+  int writeRes;
+  if (findParam(config_table[key].key, val, tblIdx) == 0) {
+    writeRes = textIO->write(strValue);
+  } else {
+    char keyStr[MAX_KEY_LENGTH + 1]; // 1 extra space for the comma
+    snprintf(keyStr, MAX_KEY_LENGTH + 1, "%s,", config_table[key].key);
+
+    textIO->seek(textIO->fsize());
+    if (textIO->tell() != 0) {
+      textIO->write("\n");
+    }
+
+    textIO->write(keyStr);
+    writeRes = textIO->write(strValue);
+  }
+
+  textIO->close();
+
+  // on success, write to config table
+  if (writeRes > 0) {
+    config_table[tblIdx].value = newValue;
+  }
+  
+  return 0;
+}
+
+int Config::writeParamByName(const char param[MAX_KEY_LENGTH], float newValue) {
+  for (size_t i = 0; i < NUM_KEYS; i++) {
+    if (!strcmp(param, config_table[i].key)) {
+      return writeParam(i, newValue);
+    }
+  }
+  return 2; // Parameter not found
+}
+
+size_t Config::getParamConfigKey(const char param[MAX_KEY_LENGTH]) {
+  for (size_t i = 0; i < NUM_KEYS; i++) {
+    if (!strcmp(param, config_table[i].key)) {
+      return i;
+    }
+  }
+  return NUM_KEYS; // Invalid key
+}
+
+Owner_e Config::getParamOwner(size_t key) {
+  size_t index = static_cast<size_t>(key);
+  if (index >= NUM_KEYS) {
+      return Owner_e::COUNT; // Invalid owner
+  }
+  return config_table[index].owner;
+}
+
+Param_t Config::getParam(size_t key) {
+    size_t index = static_cast<size_t>(key);
+    if (index >= NUM_KEYS) {
+        return Param_t{"ERROR", 0, Owner_e::COUNT, false}; // Return default Param_t if key is invalid for now
+    }
+    return config_table[index];
+}
+
+#if defined(SWO_LOGGING)
+extern "C" {
+  int __io_putchar(int ch) {
+    return ITM_SendChar(ch);
+  }
+}
+#endif
diff --git a/stm32h753iitx/boardfiles/drivers/logger/logger.cpp b/zeropilot4.0/src/system_manager/logger.cpp
similarity index 58%
rename from stm32h753iitx/boardfiles/drivers/logger/logger.cpp
rename to zeropilot4.0/src/system_manager/logger.cpp
index 2565d7f9..ccc75177 100644
--- a/stm32h753iitx/boardfiles/drivers/logger/logger.cpp
+++ b/zeropilot4.0/src/system_manager/logger.cpp
@@ -1,25 +1,30 @@
+#include "logger.hpp"
+#include "config_utils/config_defines.hpp"
 #include <cstdio>
+#include <cstdint>
 #include <cstring>
-#include "logger.hpp"
+
+Logger::Logger(ITextIO *textIO, ISystemUtils *sysUtils) : textIO(textIO), sysUtils(sysUtils) {
+  //blank
+}
 
 int Logger::init() {
 #if defined(SD_CARD_LOGGING)
-    HAL_Delay(1000);
+    sysUtils->delayMs(1000); //wait for SD card to stabilize
 
-    FRESULT res;
+    int res;
 
-    res = f_mount(&FatFs, "", 1);
-    if (res != FR_OK) {
+    res = textIO->mountFile();
+    if (res != 0) {
         return res;
     }
 
-    FRESULT exist = FR_OK;
-    FILINFO fno;
+    int exist = 0;
     int count = 1;
 
-    while (exist == FR_OK) {
-        snprintf(file, 100, "log%d.txt", count);
-        exist = f_stat(file, &fno);
+    while (exist == 0) {
+        snprintf(logFile, 100, "log%d.txt", count);
+        exist = textIO->checkFileExist(logFile);
         count++;
     }
 
@@ -27,23 +32,24 @@ int Logger::init() {
 #elif defined(SWO_LOGGING)
     return 0;
 #endif
+
 }
 
 int Logger::log(const char message[100]) {
     char msgToSend[112]; //10 for timestamp, 100 for message, 2 for new line
 
-    uint32_t ts = (uint32_t)(osKernelGetTickCount() * 1.0 / osKernelGetTickFreq());
-    int tsStrLen = snprintf(msgToSend, 10, "%lus: ", ts);
+    uint32_t ts = sysUtils->getCurrentTimestampMs() / 1000;
+    int tsStrLen = snprintf(msgToSend, 10, "%us: ", ts);
 
 #if defined(SD_CARD_LOGGING)
-    FRESULT res;
-    res = f_open(&fil, file, FA_WRITE | FA_OPEN_APPEND);
+    int res;
+    res = textIO->open(logFile, FA_WRITE | FA_OPEN_APPEND);
 
     snprintf(msgToSend + tsStrLen, 100, message);
     snprintf(msgToSend + tsStrLen + strlen(message), 3, "\r\n");
-    res = (FRESULT)f_puts(msgToSend, &fil);
+    res = textIO->write(msgToSend);
 
-    res = f_close(&fil);
+    res = textIO->close();
 
     return res;
 #elif defined(SWO_LOGGING)
@@ -57,20 +63,20 @@ int Logger::log(const char message[100]) {
 int Logger::log(const char message[][100], int count) {
     char msgToSend[112]; //10 for timestamp, 100 for message, 2 for new line
 
-    uint32_t ts = (uint32_t)(osKernelGetTickCount() * 1.0 / osKernelGetTickFreq());
-    int tsStrLen = snprintf(msgToSend, 10, "%lus: ", ts);
+    uint32_t ts = sysUtils->getCurrentTimestampMs() / 1000;
+    int tsStrLen = snprintf(msgToSend, 10, "%us: ", ts);
 
 #if defined(SD_CARD_LOGGING)
-    FRESULT res;
-    res = f_open(&fil, file, FA_WRITE | FA_OPEN_APPEND);
+    int res;
+    textIO->open(logFile, FA_WRITE | FA_OPEN_APPEND);
 
     for (int i = 0; i < count; i++) {
       snprintf(msgToSend + tsStrLen, 100, message[i]);
       snprintf(msgToSend + tsStrLen + strlen(message[i]), 3, "\r\n");
-      res = (FRESULT)f_puts(msgToSend, &fil);
+      res = textIO->write(msgToSend);
     }
 
-    res = f_close(&fil);
+    res = textIO->close();
 
     return res;
 #elif defined(SWO_LOGGING)
diff --git a/zeropilot4.0/src/system_manager/system_manager.cpp b/zeropilot4.0/src/system_manager/system_manager.cpp
index c5773ca8..5eca45db 100644
--- a/zeropilot4.0/src/system_manager/system_manager.cpp
+++ b/zeropilot4.0/src/system_manager/system_manager.cpp
@@ -5,23 +5,34 @@
 #define SM_TELEMETRY_RC_DATA_RATE_HZ 5
 
 SystemManager::SystemManager(
+	Logger *logger,
+	Config *config,
     ISystemUtils *systemUtilsDriver,
     IIndependentWatchdog *iwdgDriver,
-    ILogger *loggerDriver,
     IRCReceiver *rcDriver,
 	IPowerModule *pmDriver,
     IMessageQueue<RCMotorControlMessage_t> *amRCQueue,
     IMessageQueue<TMMessage_t> *tmQueue,
-    IMessageQueue<char[100]> *smLoggerQueue) :
+    IMessageQueue<TMSMMessage_t> *tmSmQueue,
+    IMessageQueue<char[100]> *smLoggerQueue,
+    IMessageQueue<ConfigMessage_t> **smConfigRouteQueue
+    ) :
+		logger(logger),
+	    config(config),
         systemUtilsDriver(systemUtilsDriver),
         iwdgDriver(iwdgDriver),
-        loggerDriver(loggerDriver),
         rcDriver(rcDriver),
 		pmDriver(pmDriver),
         amRCQueue(amRCQueue),
         tmQueue(tmQueue),
+        tmSmQueue(tmSmQueue),
         smLoggerQueue(smLoggerQueue),
-        smSchedulingCounter(0) {}
+        smConfigRouteQueue()
+         {
+            for (size_t i = 0; i < static_cast<size_t>(Owner_e::COUNT); ++i) {
+                this->smConfigRouteQueue[i] = smConfigRouteQueue[i];
+            }
+        }
 
 void SystemManager::smUpdate() {
     // Kick the watchdog
@@ -37,18 +48,21 @@ void SystemManager::smUpdate() {
         sendRCDataToAttitudeManager(rcData);
 
         if (!rcConnected) {
-            loggerDriver->log("RC Reconnected");
+            logger->log("RC Reconnected");
             rcConnected = true;
         }
     } else {
         oldDataCount += 1;
 
         if ((oldDataCount * SM_CONTROL_LOOP_DELAY > SM_RC_TIMEOUT) && rcConnected) {
-            loggerDriver->log("RC Disconnected");
+            logger->log("RC Disconnected");
             rcConnected = false;
         }
     }
 
+    // Handle messages from TM
+    handleMessagesFromTelemetryManager();
+
     // Send RC data to TM
     if (smSchedulingCounter % (SM_SCHEDULING_RATE_HZ / SM_TELEMETRY_RC_DATA_RATE_HZ) == 0) {
         sendRCDataToTelemetryManager(rcData);
@@ -82,6 +96,11 @@ void SystemManager::smUpdate() {
 		(void)pmDataValid; // TODO: remove when used, this line is to suppress -Wunused-variable
 	}
 
+    // Send Param data to TM if there are params left to send and the process has been initiated by a param request which sets paramAmountSent to 0
+    if (paramAmountSent >= 0 && paramAmountSent < NUM_KEYS) {
+        sendParamDataToTelemetryManager();
+    }
+
     // Log if new messages
     if (smLoggerQueue->count() > 0) {
         sendMessagesToLogger();
@@ -101,6 +120,21 @@ void SystemManager::sendHeartbeatDataToTelemetryManager(uint8_t baseMode, uint32
     tmQueue->push(&hbDataMsg);
 }
 
+void SystemManager::sendParamDataToTelemetryManager() {
+    char buffer[100];
+    snprintf(buffer, sizeof(buffer), "Sending param index %d to TM", paramAmountSent);
+    logger->log(buffer);
+    Param_t param = config->getParam(paramAmountSent);
+    TMMessage_t paramDataMsg = paramDataPack(
+        systemUtilsDriver->getCurrentTimestampMs(),
+        static_cast<uint16_t>(paramAmountSent),
+        static_cast<uint16_t>(NUM_KEYS),
+        param
+    );
+    tmQueue->push(&paramDataMsg);
+    paramAmountSent++;
+}
+
 void SystemManager::sendRCDataToAttitudeManager(const RCControl &rcData) {
     RCMotorControlMessage_t rcDataMessage;
 
@@ -123,5 +157,56 @@ void SystemManager::sendMessagesToLogger() {
         msgCount++;
     }
 
-    loggerDriver->log(messages, msgCount);
+    logger->log(messages, msgCount);
+}
+
+void SystemManager::handleMessagesFromTelemetryManager() {
+    uint16_t count = tmSmQueue->count();
+    while (count-- > 0) {
+        TMSMMessage_t msg;
+        tmSmQueue->get(&msg);
+        switch (msg.dataType) {
+            case TMSMMessage_t::PARAM_CHANGE_DATA: {
+                logger->log("Param change request received from TM");
+                int res = config->writeParamByName(
+                    msg.tmSMMessageData.paramChangeData.keyId,
+                    msg.tmSMMessageData.paramChangeData.value
+                );
+                if (res == 0) {
+                    logger->log("Param updated from TM");
+                    size_t key = config->getParamConfigKey(msg.tmSMMessageData.paramChangeData.keyId);
+                    Owner_e owner = config->getParamOwner(key);
+                    if (owner != Owner_e::COUNT) {
+                        ConfigMessage_t configMsg = {
+                            .key = static_cast<size_t>(key),
+                            .value = msg.tmSMMessageData.paramChangeData.value
+                        };
+                        smConfigRouteQueue[static_cast<size_t>(owner)]->push(&configMsg);
+                        TMMessage_t paramDataMsg = paramDataPack(systemUtilsDriver->getCurrentTimestampMs(),
+                            static_cast<uint16_t>(static_cast<size_t>(key)),
+                            static_cast<uint16_t>(NUM_KEYS),
+                            config->getParam(key)
+                        );
+                        tmQueue->push(&paramDataMsg);
+                    } else {
+                        logger->log("Param owner invalid, cannot route update");
+                    }
+                } else {
+                    logger->log("Failed to write param from TM");
+                }
+                break;
+            }
+
+            case TMSMMessage_t::REQUEST_DATA: {
+                if (msg.tmSMMessageData.requestData.requestType == TMSMRequest::REQUEST_PARAMS) {
+                    paramAmountSent = 0; // Start sending params from the beginning
+                    logger->log("Param request received from TM");
+                }
+                break;
+            }
+
+            default:
+                break;
+        }
+    }
 }
diff --git a/zeropilot4.0/src/telemetry_manager/telemetry_manager.cpp b/zeropilot4.0/src/telemetry_manager/telemetry_manager.cpp
index 277cee75..016d81ca 100644
--- a/zeropilot4.0/src/telemetry_manager/telemetry_manager.cpp
+++ b/zeropilot4.0/src/telemetry_manager/telemetry_manager.cpp
@@ -10,32 +10,35 @@
 TelemetryManager::TelemetryManager(
     ISystemUtils *systemUtilsDriver,
     IRFD *rfdDriver,
-    IMessageQueue<TMMessage_t> *tmQueueDriver,
+    IMessageQueue<TMMessage_t> *tmRxQueue,
+    IMessageQueue<TMSMMessage_t> *tmTxQueue,
     IMessageQueue<RCMotorControlMessage_t> *amQueueDriver,
-    IMessageQueue<mavlink_message_t> *messageBuffer
+    IMessageQueue<mavlink_message_t> *mavlinkTxQueue
 ) :
     systemUtilsDriver(systemUtilsDriver),
     rfdDriver(rfdDriver),
-    tmQueueDriver(tmQueueDriver),
+    tmRxQueue(tmRxQueue),
+    tmTxQueue(tmTxQueue),
     amQueueDriver(amQueueDriver),
-    messageBuffer(messageBuffer),
+    mavlinkTxQueue(mavlinkTxQueue),
     overflowMsgPending(false) {}
 
 TelemetryManager::~TelemetryManager() = default;
 
 void TelemetryManager::tmUpdate() {
+    reconstructMsg();
     processMsgQueue();
     transmit();
 }
 
 void TelemetryManager::processMsgQueue() {
-    uint16_t count = tmQueueDriver->count();
+    uint16_t count = tmRxQueue->count();
     TMMessage rcMsg = {};
     bool rc = false;
 	while (count-- > 0) {
         mavlink_message_t mavlinkMessage = {0};
         TMMessage_t tmqMessage = {};
-        tmQueueDriver->get(&tmqMessage);
+        tmRxQueue->get(&tmqMessage);
 
         switch (tmqMessage.dataType) {
             case TMMessage_t::HEARTBEAT_DATA: {
@@ -78,12 +81,26 @@ void TelemetryManager::processMsgQueue() {
                 break;
             }
 
+            case TMMessage_t::PARAM_VALUE_DATA: {
+                auto paramData = tmqMessage.tmMessageData.paramData;
+                mavlink_msg_param_value_pack(SYSTEM_ID, COMPONENT_ID, &mavlinkMessage,
+                	paramData.key, paramData.value, paramData.type, paramData.count, paramData.index);
+                break;
+            }
+
             default: {
                 continue;
             }
         }
-        
-        messageBuffer->push(&mavlinkMessage);
+        // Check to see if all params have been sent and set flag to false to allow other messages
+        if (tmqMessage.dataType == TMMessage_t::PARAM_VALUE_DATA && tmqMessage.tmMessageData.paramData.index + 1 >= tmqMessage.tmMessageData.paramData.count) {
+            transmittingParams = false;
+        }
+
+        // Allow other messages if not transmitting params, otherwise only allow param and heartbeat messages
+        if (!transmittingParams || (transmittingParams && (tmqMessage.dataType == TMMessage_t::PARAM_VALUE_DATA || tmqMessage.dataType == TMMessage_t::HEARTBEAT_DATA))) {
+            mavlinkTxQueue->push(&mavlinkMessage);
+        }
     }
 
 	if (rc) {
@@ -95,7 +112,7 @@ void TelemetryManager::processMsgQueue() {
 		if (mavlinkMessage.len == 0) {
 			return;
 		}
-		messageBuffer->push(&mavlinkMessage);
+		mavlinkTxQueue->push(&mavlinkMessage);
 	}
 }
 
@@ -111,13 +128,13 @@ void TelemetryManager::transmit() {
         overflowMsgPending = false;
     }
 
-    if (messageBuffer->count() == 0 && txBufIdx == 0) {
+    if (mavlinkTxQueue->count() == 0 && txBufIdx == 0) {
         // Nothing to transmit
         return;
     }
 
-    while (messageBuffer->count() > 0 && txBufIdx < TM_MAX_TRANSMISSION_BYTES) {
-        messageBuffer->get(&msgToTx);
+    while (mavlinkTxQueue->count() > 0 && txBufIdx < TM_MAX_TRANSMISSION_BYTES) {
+        mavlinkTxQueue->get(&msgToTx);
         const uint16_t MSG_LEN = mavlink_msg_to_send_buffer(transmitBuffer + txBufIdx, &msgToTx);
 
         if (txBufIdx + MSG_LEN > TM_MAX_TRANSMISSION_BYTES) {
@@ -149,22 +166,21 @@ void TelemetryManager::reconstructMsg() {
 
 void TelemetryManager::handleRxMsg(const mavlink_message_t &msg) {
     switch (msg.msgid) {
+        case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
+            transmittingParams = true; // Set flag to ignore other messages except heartbeat and param value until all params sent
+            TMSMMessage_t smMsg = requestPack(systemUtilsDriver->getCurrentTimestampMs(), TMSMRequest::REQUEST_PARAMS);
+            tmTxQueue->push(&smMsg);
+        }
+
         case MAVLINK_MSG_ID_PARAM_SET: {
             float valueToSet;
             char paramToSet[MAVLINK_MAX_IDENTIFIER_LEN] = {};
-            uint8_t valueType;
             valueToSet = mavlink_msg_param_set_get_param_value(&msg);
-            valueType = mavlink_msg_param_set_get_param_type(&msg);
+            mavlink_msg_param_set_get_param_id(&msg, paramToSet);
 
-            if(paramToSet[0] == 'A'){ // Would prefer to do this using an ENUM LUT but if this is the only param being set its whatever
-                RCMotorControlMessage_t armDisarmMsg{};
-                armDisarmMsg.arm = valueToSet;
-                amQueueDriver->push(&armDisarmMsg);
-            }
-            mavlink_message_t response = {};
-            mavlink_msg_param_value_pack(SYSTEM_ID, COMPONENT_ID, &response, paramToSet, valueToSet, valueType, 1, 0);
-            messageBuffer->push(&response);
-            break;
+            // Send param change to system manager which will respond back with updated param value
+            TMSMMessage_t smMsg = paramChangePack(systemUtilsDriver->getCurrentTimestampMs(), paramToSet, valueToSet);
+            tmTxQueue->push(&smMsg);
         }
 
         default: {