diff --git a/.DS_Store b/.DS_Store
index 727bd4c..c4a0f9c 100644
Binary files a/.DS_Store and b/.DS_Store differ
diff --git a/.cproject b/.cproject
index 60fc330..e6207cb 100644
--- a/.cproject
+++ b/.cproject
@@ -54,7 +54,7 @@
 									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2"/>
 									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/CAN-Demo/Libs/rtos2-utils/include}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/CAN-Demo/TACOS/include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Libs/TACOS/include}&quot;"/>
 								</option>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1002361924" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
 							</tool>
@@ -72,9 +72,9 @@
 									<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_CM4F"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/CAN-Demo/TACOS/include}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/CAN-Demo/App/Inc/sta}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/CAN-Demo/Libs/rtos2-utils/include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Libs/TACOS/include}&quot;"/>
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols.2087867384" 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"/>
@@ -114,7 +114,6 @@
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="Libs"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
-						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="TACOS/src"/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
diff --git a/.gitmodules b/.gitmodules
index b26bad9..2f2a29d 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -6,6 +6,6 @@
 	path = Libs/rtos2-utils
 	url = ssh://git@git.intern.spaceteamaachen.de:22222/ALPAKA/rtos2-utils.git
 	branch = libs-rework
-[submodule "TACOS"]
-	path = TACOS
+[submodule "Libs/TACOS"]
+	path = Libs/TACOS
 	url = ssh://git@git.intern.spaceteamaachen.de:22222/ALPAKA/TACOS.git
diff --git a/Debug/App/Src/subdir.mk b/Debug/App/Src/subdir.mk
index 707663f..8644e31 100644
--- a/Debug/App/Src/subdir.mk
+++ b/Debug/App/Src/subdir.mk
@@ -19,7 +19,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 App/Src/%.o App/Src/%.su App/Src/%.cyclo: ../App/Src/%.cpp App/Src/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-App-2f-Src
 
diff --git a/Debug/Core/Src/subdir.mk b/Debug/Core/Src/subdir.mk
index d11b3bc..d6cf749 100644
--- a/Debug/Core/Src/subdir.mk
+++ b/Debug/Core/Src/subdir.mk
@@ -43,7 +43,7 @@ OBJS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Core/Src/%.o Core/Src/%.su Core/Src/%.cyclo: ../Core/Src/%.c Core/Src/subdir.mk
-	arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -O0 -ffunction-sections -fdata-sections -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Core-2f-Src
 
diff --git a/Debug/Drivers/STM32F4xx_HAL_Driver/Src/subdir.mk b/Debug/Drivers/STM32F4xx_HAL_Driver/Src/subdir.mk
index 237d216..62f982b 100644
--- a/Debug/Drivers/STM32F4xx_HAL_Driver/Src/subdir.mk
+++ b/Debug/Drivers/STM32F4xx_HAL_Driver/Src/subdir.mk
@@ -64,7 +64,7 @@ OBJS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Drivers/STM32F4xx_HAL_Driver/Src/%.o Drivers/STM32F4xx_HAL_Driver/Src/%.su Drivers/STM32F4xx_HAL_Driver/Src/%.cyclo: ../Drivers/STM32F4xx_HAL_Driver/Src/%.c Drivers/STM32F4xx_HAL_Driver/Src/subdir.mk
-	arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -O0 -ffunction-sections -fdata-sections -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Drivers-2f-STM32F4xx_HAL_Driver-2f-Src
 
diff --git a/Debug/Libs/sta-core/src/atomic/subdir.mk b/Debug/Libs/sta-core/src/atomic/subdir.mk
index 794de5c..f1313c6 100644
--- a/Debug/Libs/sta-core/src/atomic/subdir.mk
+++ b/Debug/Libs/sta-core/src/atomic/subdir.mk
@@ -19,7 +19,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/atomic/%.o Libs/sta-core/src/atomic/%.su Libs/sta-core/src/atomic/%.cyclo: ../Libs/sta-core/src/atomic/%.cpp Libs/sta-core/src/atomic/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-atomic
 
diff --git a/Debug/Libs/sta-core/src/bus/i2c/subdir.mk b/Debug/Libs/sta-core/src/bus/i2c/subdir.mk
index 4bfdb43..d91d88c 100644
--- a/Debug/Libs/sta-core/src/bus/i2c/subdir.mk
+++ b/Debug/Libs/sta-core/src/bus/i2c/subdir.mk
@@ -19,7 +19,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/bus/i2c/%.o Libs/sta-core/src/bus/i2c/%.su Libs/sta-core/src/bus/i2c/%.cyclo: ../Libs/sta-core/src/bus/i2c/%.cpp Libs/sta-core/src/bus/i2c/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-bus-2f-i2c
 
diff --git a/Debug/Libs/sta-core/src/bus/spi/subdir.mk b/Debug/Libs/sta-core/src/bus/spi/subdir.mk
index 0ea49ad..bb218a7 100644
--- a/Debug/Libs/sta-core/src/bus/spi/subdir.mk
+++ b/Debug/Libs/sta-core/src/bus/spi/subdir.mk
@@ -22,7 +22,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/bus/spi/%.o Libs/sta-core/src/bus/spi/%.su Libs/sta-core/src/bus/spi/%.cyclo: ../Libs/sta-core/src/bus/spi/%.cpp Libs/sta-core/src/bus/spi/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-bus-2f-spi
 
diff --git a/Debug/Libs/sta-core/src/bus/subdir.mk b/Debug/Libs/sta-core/src/bus/subdir.mk
index dd6a813..292318a 100644
--- a/Debug/Libs/sta-core/src/bus/subdir.mk
+++ b/Debug/Libs/sta-core/src/bus/subdir.mk
@@ -19,7 +19,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/bus/%.o Libs/sta-core/src/bus/%.su Libs/sta-core/src/bus/%.cyclo: ../Libs/sta-core/src/bus/%.cpp Libs/sta-core/src/bus/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-bus
 
diff --git a/Debug/Libs/sta-core/src/bus/uart/subdir.mk b/Debug/Libs/sta-core/src/bus/uart/subdir.mk
index d7a187e..caeb385 100644
--- a/Debug/Libs/sta-core/src/bus/uart/subdir.mk
+++ b/Debug/Libs/sta-core/src/bus/uart/subdir.mk
@@ -19,7 +19,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/bus/uart/%.o Libs/sta-core/src/bus/uart/%.su Libs/sta-core/src/bus/uart/%.cyclo: ../Libs/sta-core/src/bus/uart/%.cpp Libs/sta-core/src/bus/uart/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-bus-2f-uart
 
diff --git a/Debug/Libs/sta-core/src/can/subdir.mk b/Debug/Libs/sta-core/src/can/subdir.mk
index 743a9d1..debb99b 100644
--- a/Debug/Libs/sta-core/src/can/subdir.mk
+++ b/Debug/Libs/sta-core/src/can/subdir.mk
@@ -19,7 +19,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/can/%.o Libs/sta-core/src/can/%.su Libs/sta-core/src/can/%.cyclo: ../Libs/sta-core/src/can/%.cpp Libs/sta-core/src/can/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-can
 
diff --git a/Debug/Libs/sta-core/src/debug/printing/subdir.mk b/Debug/Libs/sta-core/src/debug/printing/subdir.mk
index a4644c9..8d3bb68 100644
--- a/Debug/Libs/sta-core/src/debug/printing/subdir.mk
+++ b/Debug/Libs/sta-core/src/debug/printing/subdir.mk
@@ -22,7 +22,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/debug/printing/%.o Libs/sta-core/src/debug/printing/%.su Libs/sta-core/src/debug/printing/%.cyclo: ../Libs/sta-core/src/debug/printing/%.cpp Libs/sta-core/src/debug/printing/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-debug-2f-printing
 
diff --git a/Debug/Libs/sta-core/src/debug/subdir.mk b/Debug/Libs/sta-core/src/debug/subdir.mk
index 68bf5e0..0a12c53 100644
--- a/Debug/Libs/sta-core/src/debug/subdir.mk
+++ b/Debug/Libs/sta-core/src/debug/subdir.mk
@@ -16,7 +16,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/debug/%.o Libs/sta-core/src/debug/%.su Libs/sta-core/src/debug/%.cyclo: ../Libs/sta-core/src/debug/%.cpp Libs/sta-core/src/debug/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-debug
 
diff --git a/Debug/Libs/sta-core/src/devices/arduino/bus/subdir.mk b/Debug/Libs/sta-core/src/devices/arduino/bus/subdir.mk
index 57a1385..839d4bb 100644
--- a/Debug/Libs/sta-core/src/devices/arduino/bus/subdir.mk
+++ b/Debug/Libs/sta-core/src/devices/arduino/bus/subdir.mk
@@ -19,7 +19,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/devices/arduino/bus/%.o Libs/sta-core/src/devices/arduino/bus/%.su Libs/sta-core/src/devices/arduino/bus/%.cyclo: ../Libs/sta-core/src/devices/arduino/bus/%.cpp Libs/sta-core/src/devices/arduino/bus/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-devices-2f-arduino-2f-bus
 
diff --git a/Debug/Libs/sta-core/src/devices/arduino/subdir.mk b/Debug/Libs/sta-core/src/devices/arduino/subdir.mk
index 30673af..ea8480d 100644
--- a/Debug/Libs/sta-core/src/devices/arduino/subdir.mk
+++ b/Debug/Libs/sta-core/src/devices/arduino/subdir.mk
@@ -19,7 +19,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/devices/arduino/%.o Libs/sta-core/src/devices/arduino/%.su Libs/sta-core/src/devices/arduino/%.cyclo: ../Libs/sta-core/src/devices/arduino/%.cpp Libs/sta-core/src/devices/arduino/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-devices-2f-arduino
 
diff --git a/Debug/Libs/sta-core/src/devices/raspi/bus/subdir.mk b/Debug/Libs/sta-core/src/devices/raspi/bus/subdir.mk
index 4fedd53..90d90dc 100644
--- a/Debug/Libs/sta-core/src/devices/raspi/bus/subdir.mk
+++ b/Debug/Libs/sta-core/src/devices/raspi/bus/subdir.mk
@@ -19,7 +19,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/devices/raspi/bus/%.o Libs/sta-core/src/devices/raspi/bus/%.su Libs/sta-core/src/devices/raspi/bus/%.cyclo: ../Libs/sta-core/src/devices/raspi/bus/%.cpp Libs/sta-core/src/devices/raspi/bus/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-devices-2f-raspi-2f-bus
 
diff --git a/Debug/Libs/sta-core/src/devices/raspi/subdir.mk b/Debug/Libs/sta-core/src/devices/raspi/subdir.mk
index ae55ec7..8e82af3 100644
--- a/Debug/Libs/sta-core/src/devices/raspi/subdir.mk
+++ b/Debug/Libs/sta-core/src/devices/raspi/subdir.mk
@@ -19,7 +19,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/devices/raspi/%.o Libs/sta-core/src/devices/raspi/%.su Libs/sta-core/src/devices/raspi/%.cyclo: ../Libs/sta-core/src/devices/raspi/%.cpp Libs/sta-core/src/devices/raspi/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-devices-2f-raspi
 
diff --git a/Debug/Libs/sta-core/src/devices/stm32/bus/subdir.mk b/Debug/Libs/sta-core/src/devices/stm32/bus/subdir.mk
index e8c0ff7..48a7712 100644
--- a/Debug/Libs/sta-core/src/devices/stm32/bus/subdir.mk
+++ b/Debug/Libs/sta-core/src/devices/stm32/bus/subdir.mk
@@ -22,7 +22,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/devices/stm32/bus/%.o Libs/sta-core/src/devices/stm32/bus/%.su Libs/sta-core/src/devices/stm32/bus/%.cyclo: ../Libs/sta-core/src/devices/stm32/bus/%.cpp Libs/sta-core/src/devices/stm32/bus/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-devices-2f-stm32-2f-bus
 
diff --git a/Debug/Libs/sta-core/src/devices/stm32/subdir.mk b/Debug/Libs/sta-core/src/devices/stm32/subdir.mk
index 61ffb21..bbc373e 100644
--- a/Debug/Libs/sta-core/src/devices/stm32/subdir.mk
+++ b/Debug/Libs/sta-core/src/devices/stm32/subdir.mk
@@ -28,7 +28,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/devices/stm32/%.o Libs/sta-core/src/devices/stm32/%.su Libs/sta-core/src/devices/stm32/%.cyclo: ../Libs/sta-core/src/devices/stm32/%.cpp Libs/sta-core/src/devices/stm32/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-devices-2f-stm32
 
diff --git a/Debug/Libs/sta-core/src/devices/template/subdir.mk b/Debug/Libs/sta-core/src/devices/template/subdir.mk
index 0bb9da6..f6fbccf 100644
--- a/Debug/Libs/sta-core/src/devices/template/subdir.mk
+++ b/Debug/Libs/sta-core/src/devices/template/subdir.mk
@@ -19,7 +19,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/devices/template/%.o Libs/sta-core/src/devices/template/%.su Libs/sta-core/src/devices/template/%.cyclo: ../Libs/sta-core/src/devices/template/%.cpp Libs/sta-core/src/devices/template/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src-2f-devices-2f-template
 
diff --git a/Debug/Libs/sta-core/src/subdir.mk b/Debug/Libs/sta-core/src/subdir.mk
index 62d25b7..99b4ed3 100644
--- a/Debug/Libs/sta-core/src/subdir.mk
+++ b/Debug/Libs/sta-core/src/subdir.mk
@@ -16,7 +16,7 @@ CPP_DEPS += \
 
 # Each subdirectory must supply rules for building sources it contributes
 Libs/sta-core/src/%.o Libs/sta-core/src/%.su Libs/sta-core/src/%.cyclo: ../Libs/sta-core/src/%.cpp Libs/sta-core/src/subdir.mk
-	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/TACOS/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
+	arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F407xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/sta-core/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc" -I../Middlewares/Third_Party/FreeRTOS/Source/include -I../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/App/Inc/sta" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/rtos2-utils/include" -I"/Users/carlos/Documents/RWTH/space_team/CAN-Demo/Libs/TACOS/include" -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@"
 
 clean: clean-Libs-2f-sta-2d-core-2f-src
 
diff --git a/Debug/makefile b/Debug/makefile
index 3c57810..ae2fd0f 100644
--- a/Debug/makefile
+++ b/Debug/makefile
@@ -9,7 +9,6 @@ RM := rm -rf
 
 # All of the sources participating in the build are defined here
 -include sources.mk
--include TACOS/src/subdir.mk
 -include Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/subdir.mk
 -include Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/subdir.mk
 -include Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/subdir.mk
@@ -33,6 +32,7 @@ RM := rm -rf
 -include Libs/rtos2-utils/src/system/subdir.mk
 -include Libs/rtos2-utils/src/debug/subdir.mk
 -include Libs/rtos2-utils/src/subdir.mk
+-include Libs/TACOS/src/subdir.mk
 -include Drivers/STM32F4xx_HAL_Driver/Src/subdir.mk
 -include Core/Startup/subdir.mk
 -include Core/Src/subdir.mk
diff --git a/Debug/objects.list b/Debug/objects.list
index 820e63c..0ae75c9 100644
--- a/Debug/objects.list
+++ b/Debug/objects.list
@@ -29,6 +29,10 @@
 "./Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.o"
 "./Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.o"
 "./Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.o"
+"./Libs/TACOS/src/manager.o"
+"./Libs/TACOS/src/startup.o"
+"./Libs/TACOS/src/statemachine.o"
+"./Libs/TACOS/src/thread.o"
 "./Libs/rtos2-utils/src/debug/heap_stats.o"
 "./Libs/rtos2-utils/src/debug/runtime_stats.o"
 "./Libs/rtos2-utils/src/debug/stack_overflow.o"
@@ -88,7 +92,3 @@
 "./Middlewares/Third_Party/FreeRTOS/Source/timers.o"
 "./Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.o"
 "./Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.o"
-"./TACOS/src/manager.o"
-"./TACOS/src/startup.o"
-"./TACOS/src/statemachine.o"
-"./TACOS/src/thread.o"
diff --git a/Debug/sources.mk b/Debug/sources.mk
index 45aa89f..ac092b3 100644
--- a/Debug/sources.mk
+++ b/Debug/sources.mk
@@ -43,6 +43,7 @@ App/Src/tasks \
 Core/Src \
 Core/Startup \
 Drivers/STM32F4xx_HAL_Driver/Src \
+Libs/TACOS/src \
 Libs/rtos2-utils/src/debug \
 Libs/rtos2-utils/src \
 Libs/rtos2-utils/src/system \
@@ -66,5 +67,4 @@ Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 \
 Middlewares/Third_Party/FreeRTOS/Source \
 Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F \
 Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang \
-TACOS/src \
 
diff --git a/Libs/TACOS b/Libs/TACOS
new file mode 160000
index 0000000..7ebc2e5
--- /dev/null
+++ b/Libs/TACOS
@@ -0,0 +1 @@
+Subproject commit 7ebc2e5d1e7c66aa386c48d845c219cf5513d943
diff --git a/Tacos/.gitignore b/Tacos/.gitignore
deleted file mode 100644
index b9309ce..0000000
--- a/Tacos/.gitignore
+++ /dev/null
@@ -1,15 +0,0 @@
-# IDE settings
-.settings/
-
-# Contain local paths
-.mxproject
-*.launch
-
-# Build artifacts
-Debug/
-Release/
-
-# Doxygen
-docs/html
-docs/latex
-
diff --git a/Tacos/README.md b/Tacos/README.md
deleted file mode 100644
index 152f808..0000000
--- a/Tacos/README.md
+++ /dev/null
@@ -1,66 +0,0 @@
-# TACOS
-
-This is the Trajectory Analysis Control OS (TACOS) that serves as a starting point for developing TACOS-based projects, by implementing threads in the App directory. TACOS is a subset of the features provided by ALPAKA. In particular, TACOS consists of the STM32-specific components of sta-core, rtos-2 and rtos2-utils.
-
-## Setting Up TACOS
-
-Add this as a library an existing CubeIDE project using FreeRTOS. Generally, we advise you to add it as a submodule. Make sure that you add the include paths for Tacos, i.e. sta-core and rtos2-utils, to the project with the following steps:    
-``` 
-Properties -> C/C++ General -> Paths and Symbols -> Includes -> GNU C -> Add... 
-Properties -> C/C++ General -> Paths and Symbols -> Includes -> GNU C++ -> Add... 
-Properties -> C/C++ General -> Paths and Symbols -> Source Location -> Add Folder... 
-```
-
-Create a new thread via the project's IOC and call `startALPAKA()` from this thread. If your thread is called `defaultTask`, the corresponding function `StartDefaultTask` generated in `Core/Src/freertos.c` should look like this:
-```
-void StartDefaultTask(void *argument)
-{
-  /* USER CODE BEGIN StartDefaultTask */
-	extern void startALPAKA(void *);
-	startALPAKA(argument);
-
-  /* Infinite loop */
-  for(;;)
-  {
-    osDelay(1);
-  }
-  /* USER CODE END StartDefaultTask */
-}
-```
-
-## Configuring TACOS
-Be sure to add a "config.hpp" to the App/Inc/sta directory. This file could look like this:
-```
-#ifndef INC_STA_CONFIG_HPP_
-#define INC_STA_CONFIG_HPP_
-
-// Use the STM32F411 microprocessor.
-#include <sta/devices/stm32/mcu/STM32F411xE.hpp>
-
-// Doesn't really do too much right now. Has to be added for successful compilation.
-#define STA_PRINTF_USE_STDLIB
-
-// Enable debug serial output and assertions.
-#define STA_ASSERT_FORCE
-#define STA_DEBUGGING_ENABLED
-
-// Activate the timer for microsecond delays.
-// #define STA_STM32_DELAY_ENABLE
-// #define STA_STM32_DELAY_US_TIM htim1
-
-// Settings for the rtos-utils
-#define STA_RTOS_SYSTEM_EVENTS_ENABLE
-// #define STA_RTOS_SYSTEM_WATCHDOG_ENABLE
-// #define STA_RTOS_WATCHDOG_ENABLE
-
-
-// Settings for TACOS
-#define STA_TACOS_MANAGER_PRIORITY osPriorityNormal
-#define STA_TACOS_STATEMACHINE_PRIORITY osPriorityNormal
-
-// Statemachine settings. Here, we only have a single state which is also the initial state.
-#define STA_TACOS_NUM_STATES 3
-#define STA_TACOS_INITIAL_STATE 0
-
-#endif /* INC_STA_CONFIG_HPP_ */
-```
\ No newline at end of file
diff --git a/Tacos/include/sta/tacos/manager.hpp b/Tacos/include/sta/tacos/manager.hpp
deleted file mode 100644
index c11ab3d..0000000
--- a/Tacos/include/sta/tacos/manager.hpp
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * manager.hpp
- *
- *  Created on: Sep 19, 2023
- *      Author: Dario
- */
-
-#ifndef INCLUDE_STA_TACOS_MANAGER_HPP_
-#define INCLUDE_STA_TACOS_MANAGER_HPP_
-
-
-#include <sta/config.hpp>
-
-#ifndef STA_TACOS_MANAGER_PRIORITY
-#	error "Manger task priority not specified in config.hpp"
-#else
-
-#include <set>
-#include <list>
-#include <memory>
-#include <sta/tacos/thread.hpp>
-
-namespace sta
-{
-	namespace tacos
-	{
-		class Manager : public TacosThread
-		{
-		public:
-			static Manager* instance()
-			{
-				static CGuard g;
-
-				if (!_instance)
-				{
-					// Create a the manager singleton instance.
-					Manager::_instance = new Manager();
-				}
-
-				return _instance;
-			}
-
-			/**
-			 *
-			 */
-			void registerThread(std::shared_ptr<TacosThread> thread, std::list<uint16_t> states);
-
-			void init() override;
-
-			void func() override;
-
-		private:
-			static Manager* _instance;
-
-			class CGuard
-			{
-			public:
-			   ~CGuard()
-			   {
-				  if( NULL != Manager::_instance )
-				  {
-					 delete Manager::_instance;
-					 Manager::_instance = NULL;
-				  }
-			   }
-			};
-
-			Manager();
-
-			Manager(const Manager&);
-
-			//~Manager();
-
-			void updateThreads();
-
-			void startThreads(uint16_t state);
-
-			void stopThreads(uint16_t state);
-
-			std::set<std::shared_ptr<TacosThread>> threads_[STA_TACOS_NUM_STATES];
-		};
-	}  // namespace tacos
-}  // namespace sta
-
-#endif // STA_TACOS_MANAGER_PRIORITY
-
-#endif /* INCLUDE_STA_TACOS_MANAGER_HPP_ */
diff --git a/Tacos/include/sta/tacos/startup.hpp b/Tacos/include/sta/tacos/startup.hpp
deleted file mode 100644
index f108f85..0000000
--- a/Tacos/include/sta/tacos/startup.hpp
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * startup.hpp
- *
- *  Created on: 22 Sep 2023
- *      Author: Dario
- */
-
-#ifndef INCLUDE_STA_TACOS_STARTUP_HPP_
-#define INCLUDE_STA_TACOS_STARTUP_HPP_
-
-namespace sta
-{
-	namespace tacos
-	{
-		/**
-		 * @brief Function that is called before the statemachine task is started.
-		 * Override it to adjust the statemachine to your specifications.
-		 */
-		void onStatemachineInit();
-
-		/**
-		 * @brief Function that is called before the manager task is started.
-		 * Override it to adjust the manager to your specifications.
-		 */
-		void onManagerInit();
-	}  // namespace tacos
-}  // namespace sta
-
-
-
-#endif /* INCLUDE_STA_TACOS_STARTUP_HPP_ */
diff --git a/Tacos/include/sta/tacos/statemachine.hpp b/Tacos/include/sta/tacos/statemachine.hpp
deleted file mode 100644
index 99b444a..0000000
--- a/Tacos/include/sta/tacos/statemachine.hpp
+++ /dev/null
@@ -1,158 +0,0 @@
-/*
- * statemachine.hpp
- *
- *  Created on: Sep 14, 2023
- *      Author: Dario
- */
-
-#ifndef INCLUDE_TACOS_STATEMACHINE_HPP_
-#define INCLUDE_TACOS_STATEMACHINE_HPP_
-
-#include <sta/config.hpp>
-
-
-#ifndef STA_TACOS_NUM_STATES
-#	error "Number of states wasn't defined in config.hpp"
-#else
-
-
-#ifndef STA_TACOS_INITIAL_STATE
-#	define STA_TACOS_INITIAL_STATE 0
-#endif
-
-#define STA_TACOS_STATEMACHINE_QUEUE_LENGTH		4
-
-// State transition happened because of
-#define STA_TACOS_STATE_CHANGE_NORMAL_FLAG		( 0x1U )
-#define STA_TACOS_STATE_CHANGE_FORCED_FLAG		( 0x1U << 1)
-#define STA_TACOS_STATE_CHANGE_TIMEOUT			( 0x1U << 2)
-#define STA_TACOS_STATE_CHANGE_STARTUP_FLAG			( 0x1U << 3)
-#define STA_TACOS_STATE_CHANGE_ALL_FLAG			( 0x15U )
-
-
-#include <sta/tacos/thread.hpp>
-#include <sta/rtos/queue.hpp>
-#include <sta/rtos/timer.hpp>
-#include <sta/rtos/event.hpp>
-#include <sta/event.hpp>
-#include <sta/debug/assert.hpp>
-
-#include <functional>
-#include <tuple>
-
-
-namespace sta
-{
-	namespace tacos
-	{
-		enum EventFlags
-		{
-			NORMAL 				= 0x1U,
-			FORCED				= 0x1U << 1,
-			TIMEOUT				= 0x1U << 2,
-			STARTUP				= 0x1U << 3,
-			ALL					= NORMAL | FORCED | TIMEOUT | STARTUP
-		};
-
-		struct StateTransition
-		{
-			uint16_t from;
-			uint16_t to;
-			EventFlags event;
-			uint32_t lockout;
-		};
-
-
-		class Statemachine : public TacosThread
-		{
-		public:
-			/**
-			 * @brief The global event signaling a state change.
-			 */
-			static RtosEvent stateChangeEvent;
-
-			/**
-			 * @brief Getter function for the singleton instance.
-			 */
-			static Statemachine* instance()
-			{
-				static CGuard g;
-
-				if (!_instance)
-				{
-					// Create the manager singleton instance.
-					Statemachine::_instance = new Statemachine();
-				}
-
-				return _instance;
-			}
-
-			/**
-			 * @brief Returns the statemachine's current state.
-			 */
-			uint16_t getCurrentState() const;
-
-			/**
-			 * @brief Request a state transition from a state to another.
-			 *
-			 * @param from The state which we want to leave. This is used to filter out obsolete transitions.
-			 * @param to The state to transition to.
-			 * @param lockout The minimum number of milliseconds we expect to stay in this state. This is used to block premature transitions.
-			 */
-			void requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout = 0);
-
-			/**
-			 * @brief Request a state transition after a given time has passed.
-			 *
-			 * @param from The state which we want to leave. This is used to filter out obsolete transitions.
-			 * @param to The state to transition to.
-			 * @param millis the number of milliseconds to wait before triggering the transition.
-			 * @param lockout The minimum number of milliseconds we expect to stay in this state. This is used to block premature transitions.
-			 */
-			void requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout = 0);
-
-			void init() override;
-			void func() override;
-
-		private:
-			static Statemachine * _instance;
-
-			class CGuard
-			{
-			public:
-			   ~CGuard()
-			   {
-				  if( NULL != Statemachine::_instance )
-				  {
-					 delete Statemachine::_instance;
-					 Statemachine::_instance = NULL;
-				  }
-			   }
-			};
-
-			Statemachine();
-
-			Statemachine(const Statemachine&);
-
-			~Statemachine() {}
-
-		private:
-			/**
-			 * @brief Starts the lockoutTimer for the desired duration.
-			 */
-			void setLockoutTimer(uint32_t millis);
-
-		private:
-			uint16_t currentState_;
-
-			RtosTimer lockoutTimer_;
-			RtosTimer failsafeTimer_;
-
-			RtosQueue<StateTransition> queue_;
-		};
-	}  // namespace tacos
-}  // namespace sta
-
-#endif // STA_TACOS_NUM_STATES
-
-#endif /* INCLUDE_TACOS_STATEMACHINE_HPP_ */
diff --git a/Tacos/include/sta/tacos/thread.hpp b/Tacos/include/sta/tacos/thread.hpp
deleted file mode 100644
index 54e66f2..0000000
--- a/Tacos/include/sta/tacos/thread.hpp
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
- * task.hpp
- *
- *  Created on: Sep 14, 2023
- *      Author: Dario
- */
-
-#ifndef INCLUDE_TACOS_TASK_HPP_
-#define INCLUDE_TACOS_TASK_HPP_
-
-#include <cmsis_os2.h>
-
-#include <sta/rtos/thread.hpp>
-
-
-namespace sta
-{
-	namespace tacos
-	{
-		class TacosThread : public RtosThread
-		{
-		public:
-			/**
-			 *
-			 */
-			TacosThread(const char* name, osPriority_t prio);
-
-			TacosThread();
-
-			virtual ~TacosThread();
-
-			/**
-			 * @brief Start the execution of this thread.
-			 */
-			void start();
-
-			/**
-			 * @brief Checks if this thread is currently running.
-			 */
-			bool isRunning();
-
-			/**
-			 * @brief Get the currently running instance.
-			 */
-			osThreadId_t getInstance();
-
-			/**
-			 *
-			 */
-			const char* getName() const;
-
-			bool operator==(const TacosThread& other) const;
-			bool operator<(const TacosThread& other) const;
-
-			/**
-			 * @brief A function that wraps this task's functionality in a loop. This loop will run until
-			 * termination is requested.
-			 */
-			void loop();
-
-			/**
-			 * @brief This function is executed first when this thread is started.
-			 */
-			virtual void init();
-
-			/**
-			 * @brief The body of the thread's loop. Has to be implemented by the user.
-			 */
-			virtual void func() = 0;
-
-
-			virtual void cleanup();
-
-		private:
-
-			/**
-			 * @brief Static function to pass to RTOS to run as a thread. Calls the loop function implemented here.
-			 */
-			static void entry_point(void* arg);
-
-		private:
-			osThreadId_t instance_;
-			osThreadAttr_t attribs_;
-			bool running_;
-		};
-	}
-}
-
-
-#endif /* INCLUDE_TACOS_TASK_HPP_ */
diff --git a/Tacos/src/manager.cpp b/Tacos/src/manager.cpp
deleted file mode 100644
index 468c5fb..0000000
--- a/Tacos/src/manager.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-/*
- * manager.cpp
- *
- *  Created on: Sep 19, 2023
- *      Author: Dario
- */
-
-#include <sta/tacos/manager.hpp>
-#include <sta/tacos/statemachine.hpp>
-#include <sta/debug/debug.hpp>
-
-#include <algorithm>
-
-#include <FreeRTOS.h>
-
-
-namespace sta
-{
-	namespace tacos
-	{
-		void Manager::registerThread(std::shared_ptr<TacosThread> thread, std::list<uint16_t> states)
-		{
-			for (uint16_t state : states)
-			{
-				STA_ASSERT(state < STA_TACOS_NUM_STATES);
-
-				threads_[state].emplace(thread);
-			}
-		}
-
-		void Manager::startThreads(uint16_t state)
-		{
-			STA_ASSERT(state < STA_TACOS_NUM_STATES);
-
-			for (std::shared_ptr<TacosThread> thread : threads_[state])
-			{
-				if (!thread->isRunning())
-				{
-					thread->start();
-				}
-			}
-		}
-
-		void Manager::stopThreads(uint16_t state)
-		{
-			std::set<std::shared_ptr<TacosThread>> terminated;
-
-			for (uint16_t other = 0; other < STA_TACOS_NUM_STATES; ++other)
-			{
-				if (other == state)
-				{
-					continue;
-				}
-
-				for (std::shared_ptr<TacosThread> thread : threads_[other])
-				{
-					// If the thread is currently running but not part of the set of threads that should be running...
-					if (thread->isRunning() && terminated.count(thread) == 0 && threads_[state].count(thread) == 0)
-					{
-						// ...politely request termination.
-						thread->requestTermination();
-						terminated.emplace(thread);
-					}
-				}
-			}
-		}
-
-		void Manager::updateThreads()
-		{
-			uint16_t state = Statemachine::instance()->getCurrentState();
-
-			stopThreads(state);
-			startThreads(state);
-		}
-
-		void Manager::init()
-		{
-			startThreads(Statemachine::instance()->getCurrentState());
-		}
-
-		void Manager::func()
-		{
-			Statemachine::stateChangeEvent.wait(EventFlags::ALL, osWaitForever);
-
-			HeapStats_t stats;
-			vPortGetHeapStats(&stats);
-
-			// Start all new tasks and stop all the tasks that aren't supposed to be running.
-			updateThreads();
-		}
-
-		Manager::Manager()
-			: TacosThread{"Manager", STA_TACOS_MANAGER_PRIORITY}
-		{
-
-		}
-
-		//Manager::~Manager(){}
-
-		Manager* Manager::_instance = nullptr;
-
-	}  // namespace tacos
-}  // namespace sta
-
-
diff --git a/Tacos/src/startup.cpp b/Tacos/src/startup.cpp
deleted file mode 100644
index e0c550a..0000000
--- a/Tacos/src/startup.cpp
+++ /dev/null
@@ -1,91 +0,0 @@
-/*
- * startup.cpp
- *
- *  Created on: 22 Sep 2023
- *      Author: Dario
- */
-
-
-#include <cmsis_os2.h>
-#include <usart.h>
-#include <sta/rtos/mutex.hpp>
-
-// sta-core-specific imports.
-#include <sta/devices/stm32/bus/uart.hpp>
-#include <sta/debug/printing/printable_uart.hpp>
-#include <sta/debug/debug.hpp>
-#include <sta/lang.hpp>
-
-// Tacos-specific includes.
-#include <sta/tacos/manager.hpp>
-#include <sta/tacos/statemachine.hpp>
-#include <sta/tacos/startup.hpp>
-
-
-// The UART mutex defined in freertos.c
-extern osMutexId_t uartMutexHandle;
-
-
-namespace sta
-{
-	// Here the printable used for debugging is defined.
-	Printable * Debug;
-
-	namespace tacos
-	{
-		void initPrintable()
-		{
-			// Initialize the mutex for UART communication.
-			RtosMutex * mutex = new RtosMutex(&uartMutexHandle);
-
-			// Initialize the UART interface and printable object.
-			UARTSettings settings = { .mode =  UARTMode::RX_TX };
-			STM32UART * intf_ptr = new STM32UART(&huart4, settings, mutex);
-			Debug = new PrintableUART(intf_ptr);
-
-			STA_DEBUG_PRINTLN("UART SUCCESSFULLY INITIALIZED");
-		}
-
-
-		STA_WEAK
-		void onStatemachineInit()
-		{}
-
-
-		void initStatemachine()
-		{
-			onStatemachineInit();
-
-			Statemachine::instance()->start();
-		}
-
-
-		STA_WEAK
-		void onManagerInit()
-		{}
-
-
-		void initManager()
-		{
-			onManagerInit();
-
-			Manager::instance()->start();
-		}
-	}  // namespace tacos
-
-
-	namespace rtos
-	{
-		// Override the weak implementation of startupExtras provided in rtos2-utils.
-		void startupExtras(void * argument)
-		{
-			tacos::initPrintable();
-
-			tacos::initStatemachine();
-
-			tacos::initManager();
-		}
-	} // namespace rtos
-} // namespace sta
-
-
diff --git a/Tacos/src/statemachine.cpp b/Tacos/src/statemachine.cpp
deleted file mode 100644
index a5c4609..0000000
--- a/Tacos/src/statemachine.cpp
+++ /dev/null
@@ -1,107 +0,0 @@
-/*
- * statemachine.cpp
- *
- *  Created on: 21 Sep 2023
- *      Author: Dario
- */
-
-#include <sta/tacos/statemachine.hpp>
-
-#include <sta/debug/debug.hpp>
-
-
-namespace sta
-{
-	namespace tacos
-	{
-		Statemachine::Statemachine()
-			: TacosThread{"Statemachine", STA_TACOS_STATEMACHINE_PRIORITY},
-			  currentState_{STA_TACOS_INITIAL_STATE},
-			  lockoutTimer_{[](void *){}, nullptr},
-			  failsafeTimer_{[](void *){}, nullptr},
-			  queue_{STA_TACOS_STATEMACHINE_QUEUE_LENGTH}
-		{
-			STA_ASSERT(STA_TACOS_INITIAL_STATE < STA_TACOS_NUM_STATES);
-		}
-
-		void Statemachine::init()
-		{
-
-		}
-
-		void Statemachine::func()
-		{
-			// Wait for a message to be added to the queue.
-			StateTransition transition;
-			queue_.get(&transition, osWaitForever);
-
-			// Check if the transition isn't blocked and is legal.
-			if (!lockoutTimer_.isRunning() && transition.from == currentState_)
-			{
-				STA_ASSERT(transition.to < STA_TACOS_NUM_STATES);
-
-				// Perform the transition and notify the threads. The event flags are set
-				// here in order to allow threads to react immediately.
-				currentState_ = transition.to;
-				Statemachine::stateChangeEvent.set(transition.event);
-				Statemachine::stateChangeEvent.clear(EventFlags::ALL);
-
-				if (failsafeTimer_.isRunning())
-				{
-					failsafeTimer_.stop();
-				}
-
-				// Start the lockout timer if requested.
-				if (transition.lockout != 0)
-				{
-					setLockoutTimer(transition.lockout);
-				}
-			}
-		}
-
-		uint16_t Statemachine::getCurrentState() const
-		{
-			return currentState_;
-		}
-
-		void Statemachine::requestStateTransition(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */)
-		{
-			StateTransition transition;
-			transition.from = from;
-			transition.to = to;
-			transition.event = EventFlags::NORMAL;
-			transition.lockout = lockout;
-
-			// Try to add a state transition request to the queue. Don't wait if another
-			// thread is already requesting a state change.
-			queue_.put(transition, 0);
-		}
-
-		void Statemachine::requestTimedStateTransition(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */)
-		{
-			STA_ASSERT(to < STA_TACOS_NUM_STATES);
-			STA_ASSERT(!failsafeTimer_.isRunning());
-
-			failsafeTimer_.setCallback([from, to, lockout](void* arg) {
-				Statemachine::instance()->requestStateTransition(from, to, lockout);
-			}, NULL);
-
-			failsafeTimer_.start(millis);
-		}
-
-		void Statemachine::setLockoutTimer(uint32_t millis)
-		{
-			STA_ASSERT(!lockoutTimer_.isRunning());
-
-			lockoutTimer_.setCallback([](void *) {  }, nullptr);
-			lockoutTimer_.start(millis);
-		}
-
-		Statemachine* Statemachine::_instance = nullptr;
-
-		RtosEvent Statemachine::stateChangeEvent;
-	}  // namespace tacos
-}  // namespace sta
-
-
-
diff --git a/Tacos/src/thread.cpp b/Tacos/src/thread.cpp
deleted file mode 100644
index 49c6b14..0000000
--- a/Tacos/src/thread.cpp
+++ /dev/null
@@ -1,127 +0,0 @@
-/*
- * thread.cpp
- *
- *  Created on: Sep 14, 2023
- *      Author: Dario
- */
-
-
-#include <sta/tacos/thread.hpp>
-#include <sta/debug/assert.hpp>
-#include <sta/debug/debug.hpp>
-#include <sta/rtos/system/events.hpp>
-
-#include <functional>
-#include <cstring>
-
-
-namespace sta
-{
-	namespace tacos
-	{
-		TacosThread::TacosThread(const char* name, osPriority_t prio)
-				: RtosThread(RtosHandle<osThreadId_t>(Handle::Deferred(&instance_))),
-				  instance_{ NULL },
-				  attribs_{ .name = name, .priority = prio },
-				  running_{false}
-		{}
-
-		TacosThread::TacosThread()
-				: RtosThread(RtosHandle<osThreadId_t>(Handle::Deferred(&instance_))),
-				  instance_{ NULL },
-				  attribs_{ },
-				  running_{false}
-		{}
-
-		void TacosThread::entry_point(void* arg)
-		{
-			STA_ASSERT(arg != nullptr);
-
-			TacosThread* instance = reinterpret_cast<TacosThread*>(arg) ;
-			instance->loop();
-		}
-
-		void TacosThread::start()
-		{
-			STA_ASSERT(!isRunning());
-
-			// If this is the first time starting the thread, it has to be started via rtos first.
-			if (instance_ == NULL)
-			{
-				instance_ = osThreadNew(entry_point, this, &attribs_);
-
-				STA_ASSERT(instance_ != NULL);
-			}
-
-			// Send a thread start signal.
-			sysNotify(STA_RTOS_THREAD_FLAG_START);
-		}
-
-		bool TacosThread::isRunning()
-		{
-			return running_;
-		}
-
-		osThreadId_t TacosThread::getInstance()
-		{
-			STA_ASSERT(isRunning());
-
-			return instance_;
-		}
-
-		const char* TacosThread::getName() const
-		{
-			return attribs_.name;
-		}
-
-		void TacosThread::init() {}
-
-		void TacosThread::loop()
-		{
-			// Infinite loop allowing quick restarts of this thread after termination.
-			while (true)
-			{
-				// The thread has to wait until the system startup has been completed.
-				rtos::waitForStartupEvent();
-
-				// Wait for a thread start flag.
-				wait(STA_RTOS_THREAD_FLAG_START);
-
-				// Call user-defined initialization code.
-				running_ = true;
-				init();
-
-				// Run the thread until the termination flag is set. Reset this
-				while (!isTerminationRequested())
-				{
-					func();
-				}
-
-				// Clear the termination request flag for this thread.
-				deleteTerminationRequest();
-
-				// Call user-defined cleanup code.
-				cleanup();
-				running_ = false;
-			}
-
-			osThreadExit();
-		}
-
-		void TacosThread::cleanup() {}
-
-		bool TacosThread::operator==(const TacosThread& other) const
-		{
-			return std::strcmp(this->getName(), other.getName()) == 0;
-		}
-
-		bool TacosThread::operator<(const TacosThread& other) const
-		{
-			return std::strcmp(this->getName(), other.getName()) < 0;
-		}
-
-		TacosThread::~TacosThread(){}
-
-	}  // namespace tacos
-}  // namespace sta
-