Index: ctrl/firmware/Main/CubeMX/Core/Inc/main.h
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Inc/main.h	(revision 111)
+++ ctrl/firmware/Main/CubeMX/Core/Inc/main.h	(revision 112)
@@ -78,4 +78,6 @@
 #define BAT_I_SENSE_MINUS_Pin GPIO_PIN_7
 #define BAT_I_SENSE_MINUS_GPIO_Port GPIOA
+#define U_BAT_SCALE_SWITCH_Pin GPIO_PIN_2
+#define U_BAT_SCALE_SWITCH_GPIO_Port GPIOB
 #define POWER_4V_EN_Pin GPIO_PIN_15
 #define POWER_4V_EN_GPIO_Port GPIOF
Index: ctrl/firmware/Main/CubeMX/Core/Src/adc.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/adc.c	(revision 111)
+++ ctrl/firmware/Main/CubeMX/Core/Src/adc.c	(revision 112)
@@ -81,8 +81,8 @@
   sConfig.Channel = ADC_CHANNEL_3;
   sConfig.Rank = ADC_REGULAR_RANK_1;
-  sConfig.SamplingTime = ADC_SAMPLETIME_32CYCLES_5;
+  sConfig.SamplingTime = ADC_SAMPLETIME_32CYCLES_5;//ADC_SAMPLETIME_32CYCLES_5;
   sConfig.SingleDiff = ADC_DIFFERENTIAL_ENDED;
   sConfig.OffsetNumber = ADC_OFFSET_NONE;
-  sConfig.Offset = 32767;
+  sConfig.Offset = 0;
   sConfig.OffsetSignedSaturation = DISABLE;
   if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
@@ -112,5 +112,4 @@
 
   HAL_Delay(100U);
-  if (HAL_OK != HAL_ADCEx_Calibration_Start(&hadc2, ADC_CALIB_OFFSET, ADC_DIFFERENTIAL_ENDED)) Error_Handler();
   if (HAL_OK != HAL_ADCEx_Calibration_Start(&hadc2, ADC_CALIB_OFFSET_LINEARITY, ADC_DIFFERENTIAL_ENDED)) Error_Handler();
   HAL_Delay(100U);
@@ -122,10 +121,11 @@
    // Disabling interrupts, because we don't need it during offset calculation
   __HAL_DMA_DISABLE_IT(&hdma_adc2, DMA_IT_HT | DMA_IT_TC);
+  uint32_t TC_flag = __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_adc2);
   for (uint32_t i = 0U; i < max_samples; i++)
   {	  // Waiting for conversion to finish
-	  while(!__HAL_DMA_GET_FLAG(&hdma_adc2, DMA_FLAG_TCIF1_5));
-	  // Reading ADC2 value
+	  while(!__HAL_DMA_GET_FLAG(&hdma_adc2, TC_flag));	  // For correct flag see __HAL_DMA_GET_TC_FLAG_INDEX macro description
+	  // Reading ADC2 values
 	  for (unsigned j = 0; j < ADC2_CHANNELS_NUM; j++) offset[j] += ADC2Data.Raw[j];
-	  __HAL_DMA_CLEAR_FLAG(&hdma_adc2, DMA_FLAG_TCIF1_5);
+	  __HAL_DMA_CLEAR_FLAG(&hdma_adc2, TC_flag);
   }
 
@@ -138,5 +138,5 @@
 	  LL_ADC_SetOffset(hadc2.Instance, LL_ADC_OFFSET_1, LL_ADC_CHANNEL_3, (offset[0]/max_samples) * ((hadc2.Init.OversamplingMode == ENABLE)? hadc2.Init.Oversampling.Ratio: 1));
 	  LL_ADC_SetOffset(hadc2.Instance, LL_ADC_OFFSET_2, LL_ADC_CHANNEL_4, (offset[1]/max_samples) * ((hadc2.Init.OversamplingMode == ENABLE)? hadc2.Init.Oversampling.Ratio: 1));
-	  LL_ADC_SetOffset(hadc2.Instance, LL_ADC_OFFSET_3, LL_ADC_CHANNEL_5, (offset[2]/max_samples) * ((hadc2.Init.OversamplingMode == ENABLE)? hadc2.Init.Oversampling.Ratio: 1));
+	  LL_ADC_SetOffset(hadc2.Instance, LL_ADC_OFFSET_3, LL_ADC_CHANNEL_5, (32767U) * ((hadc2.Init.OversamplingMode == ENABLE)? hadc2.Init.Oversampling.Ratio: 1));
   }
 
@@ -154,4 +154,7 @@
   /* USER CODE BEGIN ADC3_Init 0 */
 
+  // This ADC is used to sample raw voltage on the battery connector.
+  // It is not very precise dut to the voltagedrop on the wires.
+
   /* USER CODE END ADC3_Init 0 */
 
@@ -165,5 +168,5 @@
   */
   hadc3.Instance = ADC3;
-  hadc3.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV256;
+  hadc3.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV1;
   hadc3.Init.Resolution = ADC_RESOLUTION_12B;
   hadc3.Init.DataAlign = ADC3_DATAALIGN_RIGHT;
@@ -234,9 +237,7 @@
 
   HAL_Delay(100U);
-  if (HAL_OK != HAL_ADCEx_Calibration_Start(&hadc3, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED)) Error_Handler();
   if (HAL_OK != HAL_ADCEx_Calibration_Start(&hadc3, ADC_CALIB_OFFSET_LINEARITY, ADC_SINGLE_ENDED)) Error_Handler();
   HAL_Delay(100U);
 
-  //if (HAL_OK != HAL_ADC_Start(&hadc3)) Error_Handler();
   if (HAL_OK != HAL_ADC_Start_DMA(&hadc3, (uint32_t*)&ADC3Data, ADC3_CHANNELS_NUM)) Error_Handler();
   __HAL_DMA_DISABLE_IT(&hdma_adc3, DMA_IT_HT);
Index: ctrl/firmware/Main/CubeMX/Core/Src/gpio.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/gpio.c	(revision 111)
+++ ctrl/firmware/Main/CubeMX/Core/Src/gpio.c	(revision 112)
@@ -61,4 +61,7 @@
 
   /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOB, U_BAT_SCALE_SWITCH_Pin|ETH_SPI_RST_Pin, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(POWER_4V_EN_GPIO_Port, POWER_4V_EN_Pin, GPIO_PIN_RESET);
 
@@ -78,7 +81,4 @@
   /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(ETH_SPI_NSS_GPIO_Port, ETH_SPI_NSS_Pin, GPIO_PIN_SET);
-
-  /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(ETH_SPI_RST_GPIO_Port, ETH_SPI_RST_Pin, GPIO_PIN_RESET);
 
   /*Configure GPIO pins : PE4 PE5 PE10 PE0
@@ -123,10 +123,9 @@
   HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
-  /*Configure GPIO pins : PB2 PB11 PB12 PB13
-                           PB3 PB5 */
-  GPIO_InitStruct.Pin = GPIO_PIN_2|GPIO_PIN_11|GPIO_PIN_12|GPIO_PIN_13
-                          |GPIO_PIN_3|GPIO_PIN_5;
-  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  /*Configure GPIO pins : U_BAT_SCALE_SWITCH_Pin ETH_SPI_NSS_Pin ETH_SPI_RST_Pin */
+  GPIO_InitStruct.Pin = U_BAT_SCALE_SWITCH_Pin|ETH_SPI_NSS_Pin|ETH_SPI_RST_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
 
@@ -146,4 +145,12 @@
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   HAL_GPIO_Init(GPIOG, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : PB11 PB12 PB13 PB3
+                           PB5 */
+  GPIO_InitStruct.Pin = GPIO_PIN_11|GPIO_PIN_12|GPIO_PIN_13|GPIO_PIN_3
+                          |GPIO_PIN_5;
+  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
 
   /*Configure GPIO pins : GSM_PWR_Pin ETH_SPI_PWR_Pin OUTPUT_ON_LED_Pin */
@@ -187,11 +194,4 @@
   HAL_GPIO_Init(ONEWIRE_TEMP_BUS_GPIO_Port, &GPIO_InitStruct);
 
-  /*Configure GPIO pins : ETH_SPI_NSS_Pin ETH_SPI_RST_Pin */
-  GPIO_InitStruct.Pin = ETH_SPI_NSS_Pin|ETH_SPI_RST_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
-
   /*Configure GPIO pin : ETH_SPI_INT_Pin */
   GPIO_InitStruct.Pin = ETH_SPI_INT_Pin;
Index: ctrl/firmware/Main/CubeMX/Core/Src/main.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/main.c	(revision 111)
+++ ctrl/firmware/Main/CubeMX/Core/Src/main.c	(revision 112)
@@ -133,4 +133,6 @@
   MX_DMA_Init();
   MX_BDMA_Init();
+  MX_ADC2_Init();
+  MX_ADC3_Init();
   MX_RTC_Init();
   MX_SPI4_Init();
@@ -143,5 +145,4 @@
   MX_TIM6_Init();
   MX_I2C2_Init();
-  MX_ADC3_Init();
   MX_TIM3_Init();
   MX_I2C1_Init();
@@ -150,5 +151,4 @@
   MX_DAC1_Init();
   MX_TIM1_Init();
-  MX_ADC2_Init();
   /* USER CODE BEGIN 2 */
 
Index: ctrl/firmware/Main/CubeMX/Core/Src/stm32h7xx_it.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/stm32h7xx_it.c	(revision 111)
+++ ctrl/firmware/Main/CubeMX/Core/Src/stm32h7xx_it.c	(revision 112)
@@ -473,6 +473,8 @@
 static volatile int64_t charge_tmp;
 static volatile int64_t load_tmp;
+static volatile int64_t u_bat_tmp_uV;
 static volatile float charge_i;
 static volatile float load_i;
+static volatile float u_bat;
 
 void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc)
@@ -482,11 +484,27 @@
 	if (hadc->Instance == ADC2)
 	{
-		HAL_GPIO_WritePin(RX1_LED_GPIO_Port, RX1_LED_Pin, GPIO_PIN_SET);
+		//HAL_GPIO_WritePin(RX1_LED_GPIO_Port, RX1_LED_Pin, GPIO_PIN_SET);
 		charge_tmp = (((int64_t)ADC2Data.Name.charge_strom * ((int64_t)VREF) * 1000LL) / ((int64_t)0xFFFF)) / 50LL;
 		load_tmp = (((int64_t)ADC2Data.Name.eload_strom * ((int64_t)VREF) * 1000LL) / ((int64_t)0xFFFF)) / 50LL;
+		u_bat_tmp_uV = (int64_t)ADC2Data.Name.u_bat * (int64_t)VREF * 1000LL * 2LL;
+
 		charge_i = (float)charge_tmp / 500LL;
 		load_i = (float)load_tmp / 500LL;
-		printf("\t\t\t%5.2fA, %5.2fA\n", charge_i, load_i);
+		u_bat = (float)u_bat_tmp_uV / (32767.0f * 1000000.0f);
+		//printf("\t\t\t%5.2fA, %5.2fA\n", charge_i, load_i);
 		//printf("%d, %d\n", ADC2Data.Name.charge_strom, ADC2Data.Name.eload_strom);
+
+		//printf("%5.4f\n", u_bat);
+		//HAL_GPIO_WritePin(RX1_LED_GPIO_Port, RX1_LED_Pin, GPIO_PIN_RESET);
+	}
+	else if (hadc->Instance == ADC3)
+	{
+		HAL_GPIO_WritePin(RX1_LED_GPIO_Port, RX1_LED_Pin, GPIO_PIN_SET);
+
+		uint32_t tmp = (((uint32_t)ADC3Data.Name.UBat) * VREF * 2U) / 4095U;
+		float UBat = (float)tmp / 1000.0;
+		printf("%6.4f\n", UBat);
+		//printf("%u\n", (((uint32_t)ADC3Data.Name.UBat) * VREF * 2U) / 4095);
+
 		HAL_GPIO_WritePin(RX1_LED_GPIO_Port, RX1_LED_Pin, GPIO_PIN_RESET);
 	}
Index: ctrl/firmware/Main/CubeMX/Core/Src/tim.c
===================================================================
--- ctrl/firmware/Main/CubeMX/Core/Src/tim.c	(revision 111)
+++ ctrl/firmware/Main/CubeMX/Core/Src/tim.c	(revision 112)
@@ -78,5 +78,5 @@
   }
   sConfigOC.OCMode = TIM_OCMODE_PWM1;
-  sConfigOC.Pulse = 50;
+  sConfigOC.Pulse = 0;//50;
   sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
   sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
Index: ctrl/firmware/Main/CubeMX/FATFS/Target/sd_diskio.c
===================================================================
--- ctrl/firmware/Main/CubeMX/FATFS/Target/sd_diskio.c	(revision 111)
+++ ctrl/firmware/Main/CubeMX/FATFS/Target/sd_diskio.c	(revision 112)
@@ -208,9 +208,9 @@
       if (SDQueueID == NULL)
       {
- #if (osCMSIS <= 0x20000U)
-      osMessageQDef(SD_Queue, QUEUE_SIZE, uint16_t);
-      SDQueueID = osMessageCreate (osMessageQ(SD_Queue), NULL);
-#else
-      SDQueueID = osMessageQueueNew(QUEUE_SIZE, 2, NULL);
+#if (osCMSIS <= 0x20000U)
+		  osMessageQDef(SD_Queue, QUEUE_SIZE, uint16_t);
+		  SDQueueID = osMessageCreate (osMessageQ(SD_Queue), NULL);
+#else
+		  SDQueueID = osMessageQueueNew(QUEUE_SIZE, 2, NULL);
 #endif
       }
Index: ctrl/firmware/Main/CubeMX/charger.ioc
===================================================================
--- ctrl/firmware/Main/CubeMX/charger.ioc	(revision 111)
+++ ctrl/firmware/Main/CubeMX/charger.ioc	(revision 112)
@@ -26,7 +26,7 @@
 ADC2.Ratio=256
 ADC2.RightBitShift=ADC_RIGHTBITSHIFT_8
-ADC2.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_64CYCLES_5
-ADC2.SamplingTime-2\#ChannelRegularConversion=ADC_SAMPLETIME_64CYCLES_5
-ADC2.SamplingTime-3\#ChannelRegularConversion=ADC_SAMPLETIME_64CYCLES_5
+ADC2.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_32CYCLES_5
+ADC2.SamplingTime-2\#ChannelRegularConversion=ADC_SAMPLETIME_32CYCLES_5
+ADC2.SamplingTime-3\#ChannelRegularConversion=ADC_SAMPLETIME_32CYCLES_5
 ADC2.SingleDiff-0\#ChannelRegularConversion=ADC_DIFFERENTIAL_ENDED
 ADC2.SingleDiff-2\#ChannelRegularConversion=ADC_DIFFERENTIAL_ENDED
@@ -509,6 +509,10 @@
 PA8.Locked=true
 PA8.Signal=GPIO_Input
+PB0.GPIOParameters=GPIO_Label
+PB0.GPIO_Label=BAT_U_SENSE_MINUS
 PB0.Locked=true
 PB0.Signal=ADCx_INN5
+PB1.GPIOParameters=GPIO_Label
+PB1.GPIO_Label=BAT_U_SENSE_PLUS
 PB1.Signal=ADCx_INP5
 PB10.GPIOParameters=GPIO_Speed,GPIO_Label
@@ -529,7 +533,8 @@
 PB15.Mode=Full_Duplex_Master
 PB15.Signal=SPI2_MOSI
-PB2.GPIOParameters=GPIO_Label
+PB2.GPIOParameters=PinState,GPIO_Label
 PB2.GPIO_Label=U_BAT_SCALE_SWITCH
 PB2.Locked=true
+PB2.PinState=GPIO_PIN_RESET
 PB2.Signal=GPIO_Output
 PB4(NJTRST).GPIOParameters=PinState,GPIO_Label
@@ -558,5 +563,5 @@
 PB9.Signal=GPIO_Output
 PC0.GPIOParameters=GPIO_Label
-PC0.GPIO_Label=U_BAT
+PC0.GPIO_Label=BAT_U_RAW
 PC0.Locked=true
 PC0.Signal=ADCx_INP10
@@ -571,5 +576,9 @@
 PC15-OSC32_OUT.Mode=LSE-External-Oscillator
 PC15-OSC32_OUT.Signal=RCC_OSC32_OUT
+PC4.GPIOParameters=GPIO_Label
+PC4.GPIO_Label=LOAD_I_SENSE_PLUS
 PC4.Signal=ADCx_INP4
+PC5.GPIOParameters=GPIO_Label
+PC5.GPIO_Label=LOAD_I_SENSE_MINUS
 PC5.Signal=ADCx_INN4
 PC6.GPIOParameters=GPIO_PuPd,GPIO_Label