Index: ctrl/firmware/Main/CubeMX/Core/Inc/stm32h7xx_it.h
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Inc/stm32h7xx_it.h	(revision 71)
+++ ctrl/firmware/Main/CubeMX/Core/Inc/stm32h7xx_it.h	(revision 72)
@@ -52,8 +52,5 @@
 void BusFault_Handler(void);
 void UsageFault_Handler(void);
-void SVC_Handler(void);
 void DebugMon_Handler(void);
-void PendSV_Handler(void);
-void SysTick_Handler(void);
 void DMA1_Stream0_IRQHandler(void);
 void DMA1_Stream1_IRQHandler(void);
Index: ctrl/firmware/Main/CubeMX/Core/Src/dma.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/dma.c	(revision 71)
+++ ctrl/firmware/Main/CubeMX/Core/Src/dma.c	(revision 72)
@@ -45,11 +45,11 @@
   /* DMA interrupt init */
   /* DMA1_Stream0_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 0, 0);
+  HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 5, 0);
   HAL_NVIC_EnableIRQ(DMA1_Stream0_IRQn);
   /* DMA1_Stream1_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 0, 0);
+  HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 5, 0);
   HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn);
   /* DMA1_Stream2_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 0, 0);
+  HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 5, 0);
   HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn);
 
Index: ctrl/firmware/Main/CubeMX/Core/Src/main.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/main.c	(revision 71)
+++ ctrl/firmware/Main/CubeMX/Core/Src/main.c	(revision 72)
@@ -19,4 +19,5 @@
 /* Includes ------------------------------------------------------------------*/
 #include "main.h"
+#include "cmsis_os.h"
 #include "dma.h"
 #include "memorymap.h"
@@ -59,4 +60,5 @@
 void SystemClock_Config(void);
 static void MPU_Config(void);
+void MX_FREERTOS_Init(void);
 /* USER CODE BEGIN PFP */
 
@@ -125,4 +127,15 @@
 #undef MX_SDMMC1_SD_Init
   /* USER CODE END 2 */
+
+  /* Init scheduler */
+  osKernelInitialize();
+
+  /* Call init function for freertos objects (in cmsis_os2.c) */
+  MX_FREERTOS_Init();
+
+  /* Start scheduler */
+  osKernelStart();
+
+  /* We should never get here as control is now taken by the scheduler */
 
   /* Infinite loop */
Index: ctrl/firmware/Main/CubeMX/Core/Src/sdmmc.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/sdmmc.c	(revision 71)
+++ ctrl/firmware/Main/CubeMX/Core/Src/sdmmc.c	(revision 72)
@@ -106,5 +106,5 @@
 
     /* SDMMC1 interrupt Init */
-    HAL_NVIC_SetPriority(SDMMC1_IRQn, 0, 0);
+    HAL_NVIC_SetPriority(SDMMC1_IRQn, 5, 0);
     HAL_NVIC_EnableIRQ(SDMMC1_IRQn);
   /* USER CODE BEGIN SDMMC1_MspInit 1 */
Index: ctrl/firmware/Main/CubeMX/Core/Src/spi.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/spi.c	(revision 71)
+++ ctrl/firmware/Main/CubeMX/Core/Src/spi.c	(revision 72)
@@ -127,5 +127,5 @@
 
     /* SPI4 interrupt Init */
-    HAL_NVIC_SetPriority(SPI4_IRQn, 0, 0);
+    HAL_NVIC_SetPriority(SPI4_IRQn, 5, 0);
     HAL_NVIC_EnableIRQ(SPI4_IRQn);
   /* USER CODE BEGIN SPI4_MspInit 1 */
Index: ctrl/firmware/Main/CubeMX/Core/Src/stm32h7xx_hal_msp.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/stm32h7xx_hal_msp.c	(revision 71)
+++ ctrl/firmware/Main/CubeMX/Core/Src/stm32h7xx_hal_msp.c	(revision 72)
@@ -71,4 +71,6 @@
 
   /* System interrupt init*/
+  /* PendSV_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(PendSV_IRQn, 15, 0);
 
   /* USER CODE BEGIN MspInit 1 */
Index: ctrl/firmware/Main/CubeMX/Core/Src/stm32h7xx_it.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/stm32h7xx_it.c	(revision 71)
+++ ctrl/firmware/Main/CubeMX/Core/Src/stm32h7xx_it.c	(revision 72)
@@ -150,17 +150,4 @@
 
 /**
-  * @brief This function handles System service call via SWI instruction.
-  */
-void SVC_Handler(void)
-{
-  /* USER CODE BEGIN SVCall_IRQn 0 */
-
-  /* USER CODE END SVCall_IRQn 0 */
-  /* USER CODE BEGIN SVCall_IRQn 1 */
-
-  /* USER CODE END SVCall_IRQn 1 */
-}
-
-/**
   * @brief This function handles Debug monitor.
   */
@@ -173,31 +160,4 @@
 
   /* USER CODE END DebugMonitor_IRQn 1 */
-}
-
-/**
-  * @brief This function handles Pendable request for system service.
-  */
-void PendSV_Handler(void)
-{
-  /* USER CODE BEGIN PendSV_IRQn 0 */
-
-  /* USER CODE END PendSV_IRQn 0 */
-  /* USER CODE BEGIN PendSV_IRQn 1 */
-
-  /* USER CODE END PendSV_IRQn 1 */
-}
-
-/**
-  * @brief This function handles System tick timer.
-  */
-void SysTick_Handler(void)
-{
-  /* USER CODE BEGIN SysTick_IRQn 0 */
-
-  /* USER CODE END SysTick_IRQn 0 */
-
-  /* USER CODE BEGIN SysTick_IRQn 1 */
-
-  /* USER CODE END SysTick_IRQn 1 */
 }
 
Index: ctrl/firmware/Main/CubeMX/Core/Src/tim.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/tim.c	(revision 71)
+++ ctrl/firmware/Main/CubeMX/Core/Src/tim.c	(revision 72)
@@ -162,5 +162,5 @@
 
     /* TIM3 interrupt Init */
-    HAL_NVIC_SetPriority(TIM3_IRQn, 0, 0);
+    HAL_NVIC_SetPriority(TIM3_IRQn, 5, 0);
     HAL_NVIC_EnableIRQ(TIM3_IRQn);
   /* USER CODE BEGIN TIM3_MspInit 1 */
@@ -188,5 +188,5 @@
 
     /* TIM8 interrupt Init */
-    HAL_NVIC_SetPriority(TIM8_CC_IRQn, 0, 0);
+    HAL_NVIC_SetPriority(TIM8_CC_IRQn, 5, 0);
     HAL_NVIC_EnableIRQ(TIM8_CC_IRQn);
   /* USER CODE BEGIN TIM8_MspInit 1 */
Index: ctrl/firmware/Main/CubeMX/Core/Src/usart.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/usart.c	(revision 71)
+++ ctrl/firmware/Main/CubeMX/Core/Src/usart.c	(revision 72)
@@ -159,5 +159,5 @@
 
     /* USART3 interrupt Init */
-    HAL_NVIC_SetPriority(USART3_IRQn, 0, 0);
+    HAL_NVIC_SetPriority(USART3_IRQn, 5, 0);
     HAL_NVIC_EnableIRQ(USART3_IRQn);
   /* USER CODE BEGIN USART3_MspInit 1 */
Index: ctrl/firmware/Main/CubeMX/charger.ioc
===================================================================
--- ctrl/firmware/Main/CubeMX/charger.ioc	(revision 71)
+++ ctrl/firmware/Main/CubeMX/charger.ioc	(revision 72)
@@ -66,4 +66,13 @@
 Dma.USART3_TX.2.SyncRequestNumber=1
 Dma.USART3_TX.2.SyncSignalID=NONE
+FREERTOS.INCLUDE_uxTaskGetStackHighWaterMark2=1
+FREERTOS.INCLUDE_xTaskGetHandle=1
+FREERTOS.IPParameters=Tasks01,configENABLE_FPU,configENABLE_BACKWARD_COMPATIBILITY,configUSE_TICKLESS_IDLE,configRECORD_STACK_HIGH_ADDRESS,configCHECK_FOR_STACK_OVERFLOW,INCLUDE_xTaskGetHandle,INCLUDE_uxTaskGetStackHighWaterMark2
+FREERTOS.Tasks01=mainTask,24,128,mainTaskStart,Default,NULL,Static,mainTaskBuffer,mainTaskControlBlock
+FREERTOS.configCHECK_FOR_STACK_OVERFLOW=2
+FREERTOS.configENABLE_BACKWARD_COMPATIBILITY=0
+FREERTOS.configENABLE_FPU=1
+FREERTOS.configRECORD_STACK_HIGH_ADDRESS=1
+FREERTOS.configUSE_TICKLESS_IDLE=1
 File.Version=6
 GPIO.groupedBy=Group By Peripherals
@@ -75,16 +84,17 @@
 Mcu.IP0=CORTEX_M7
 Mcu.IP1=DEBUG
-Mcu.IP10=TIM3
-Mcu.IP11=TIM8
-Mcu.IP12=USART3
+Mcu.IP10=SYS
+Mcu.IP11=TIM3
+Mcu.IP12=TIM8
+Mcu.IP13=USART3
 Mcu.IP2=DMA
-Mcu.IP3=MEMORYMAP
-Mcu.IP4=NVIC
-Mcu.IP5=RCC
-Mcu.IP6=RTC
-Mcu.IP7=SDMMC1
-Mcu.IP8=SPI4
-Mcu.IP9=SYS
-Mcu.IPNb=13
+Mcu.IP3=FREERTOS
+Mcu.IP4=MEMORYMAP
+Mcu.IP5=NVIC
+Mcu.IP6=RCC
+Mcu.IP7=RTC
+Mcu.IP8=SDMMC1
+Mcu.IP9=SPI4
+Mcu.IPNb=14
 Mcu.Name=STM32H723ZETx
 Mcu.Package=LQFP144
@@ -121,11 +131,12 @@
 Mcu.Pin36=PG13
 Mcu.Pin37=PG15
-Mcu.Pin38=VP_RTC_VS_RTC_Activate
-Mcu.Pin39=VP_SYS_VS_tim7
+Mcu.Pin38=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin39=VP_RTC_VS_RTC_Activate
 Mcu.Pin4=PH1-OSC_OUT
-Mcu.Pin40=VP_TIM3_VS_ClockSourceINT
-Mcu.Pin41=VP_TIM8_VS_ControllerModeReset
-Mcu.Pin42=VP_TIM8_VS_ClockSourceINT
-Mcu.Pin43=VP_MEMORYMAP_VS_MEMORYMAP
+Mcu.Pin40=VP_SYS_VS_tim7
+Mcu.Pin41=VP_TIM3_VS_ClockSourceINT
+Mcu.Pin42=VP_TIM8_VS_ControllerModeReset
+Mcu.Pin43=VP_TIM8_VS_ClockSourceINT
+Mcu.Pin44=VP_MEMORYMAP_VS_MEMORYMAP
 Mcu.Pin5=PF15
 Mcu.Pin6=PG0
@@ -133,5 +144,5 @@
 Mcu.Pin8=PE7
 Mcu.Pin9=PE11
-Mcu.PinsNb=44
+Mcu.PinsNb=45
 Mcu.ThirdParty0=STMicroelectronics.X-CUBE-AZRTOS-H7.3.3.0
 Mcu.ThirdPartyNb=1
@@ -140,26 +151,29 @@
 MxCube.Version=6.13.0
 MxDb.Version=DB.6.0.130
-NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
-NVIC.DMA1_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
-NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
-NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
-NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
+NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
+NVIC.DMA1_Stream0_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.DMA1_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.ForceEnableDMAVector=true
-NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
-NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
-NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
-NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
+NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
+NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
+NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
+NVIC.PendSV_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:false\:false
 NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
-NVIC.SDMMC1_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
-NVIC.SPI4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
-NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
-NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false
-NVIC.TIM3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
-NVIC.TIM7_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:true
-NVIC.TIM8_CC_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
+NVIC.SDMMC1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.SPI4_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:false\:false\:false
+NVIC.SavedPendsvIrqHandlerGenerated=true
+NVIC.SavedSvcallIrqHandlerGenerated=true
+NVIC.SavedSystickIrqHandlerGenerated=true
+NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true\:false
+NVIC.TIM3_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.TIM7_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
+NVIC.TIM8_CC_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.TimeBase=TIM7_IRQn
 NVIC.TimeBaseIP=TIM7
-NVIC.USART3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
-NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
+NVIC.USART3_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 PA13(JTMS/SWDIO).Mode=Serial_Wire
 PA13(JTMS/SWDIO).Signal=DEBUG_JTMS-SWDIO
@@ -459,4 +473,6 @@
 USART3.SwapParam=ADVFEATURE_SWAP_ENABLE
 USART3.VirtualMode-Asynchronous=VM_ASYNC
+VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
+VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
 VP_MEMORYMAP_VS_MEMORYMAP.Mode=CurAppReg
 VP_MEMORYMAP_VS_MEMORYMAP.Signal=MEMORYMAP_VS_MEMORYMAP