/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "crc.h"
#include "dac.h"
#include "dma.h"
#include "iwdg.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

#include <stdio.h>
#include <math.h>

#include "log.h"
#include "sysdata.h"
#include "modbus.h"
#include "SEGGER_RTT.h"
#include "feeprom.h"
#include "tast.h"
#include "string.h"
#include "precharge.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

typedef enum LOGIC {LOGIC_POSITIV, LOGIC_NEGATIV} logic_t;

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

#define TAG "MAIN"

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

#ifdef USE_RAM_FUNC
uint8_t vectorTableInRAM[192] __attribute__ ((aligned (256)));
#endif

          modbus_t    modbusData;
          sys_data_t  sys_data;
volatile  uint16_t    ADC_values[ADC_CHANNELS];
volatile  int32_t	  rawMOSFETsVoltageDrop;
volatile  int32_t	  rawContactVoltageDropPlus;
volatile  int32_t	  rawContactVoltageDropMinus;
          int         command_parser_is_enabled;
volatile  int		  overcurrent_shutdown_is_active = 0;
volatile  int		  overload_shutdown_is_active = 0;
          int		  low_bat_shutdown_is_active = 0;
          int		  temperature_shutdown_is_active = 0;
          int		  mosfets_voltagedrop_shutdown_is_active = 0;
          void		  (*MOSFETS_Management)(void);					// Function pointer that is called in ADC interrupt, depending on the states of LVP&OVP
          void		  (*LVP_OVP[LVP_OVP_EVENT_NUM])(void);			// Function pointers array that contains "what to do" functions for every combination of LVP&OVP
          void		  (*AUTO_Mode)(uint32_t, int);					// Function pointer that contains function that is executed when gSwitch is in AUTO mode (depends on DIP switches)
          logic_t	  LVP_OVP_logic = LOGIC_NEGATIV;				// Default logic is negative
          int		  manual_overdrive_is_enabled;
          uint32_t	  swdioConnection = UINT32_MAX;					// Special variable that contains non-zero number, if SWD-debugger is connected to target
          void		  (*InternalGreenLED_Management)(void);					// Function pointer that controls Green LED
          void		  (*InternalBlueLED_Management)(void);					// Function pointer that controls Blue LED
          void		  (*InternalRedLED_Management)(void);			// Function pointer that controls internal Red LED
          void		  (*ExternalGreenLED_Management)(void);			// Function pointer that controls external Green LED, which is located inside of the button
          void		  (*ExternalRedLED_Management)(void);			// Function pointer that controls external Red LED, which is located inside of the button
          int		  RS485ActiveMode = 1;							// RS485 transsiver is active
          int		  auto_recover_from_temp_shutdown_is_enabled;	// Automatic reconnect after overtemperature disconnect
          //uint16_t  i_samples[I_RMS_SAMPLES_COUNT] __attribute__((aligned(4)));
          //uint16_t  d_samples[I_RMS_SAMPLES_COUNT];
          //uint16_t  u_samples[I_RMS_SAMPLES_COUNT];
//volatile  int32_t	  i_samples_counter = 0;
		  uint16_t	  savedLockKey;
volatile  uint32_t	  overcurrent_shutdown_time = 0xFFFFE0C0;
volatile  uint32_t	  overload_shutdown_time = 0xFFFFE0C0;
		  void		  (*Callibration)(void);
		  //void		  (*Callibration_2)(void);
		  //void		  (*ActuelKeyManagement)(void);
		  uint16_t	  keyAccepted = 0;
		  uint16_t	  savedLockKey;
		  int		  statDataChanged = 0;
volatile  uint32_t	  maxIntegral = UINT32_MAX;
		  void		  (*InrushCurrentManagement)(void);

//#include "raccess.c"
extern accessMode_t accessModeTable[ MAX_ADRESS+1 ];

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
void SYSDATA_Init(void);
void StartUpSequence(void);
#ifdef USE_RAM_FUNC
void CopyingVectorTableToRAM(void);
#endif
void DoNothing();
void OpenBothMOSFETSVeryFast(void);
void CloseBothMOSFETSVeryFast(void);
void OVP_not_present__LVP_not_present(void);
void OVP_present__LVP_not_present(void);
void OVP_not_present__LVP_present(void);
void OVP_present__LVP_present(void);
void ADC_OVP_not_present__LVP_not_present(void);
void ADC_OVP_present__LVP_not_present(void);
void ADC_OVP_not_present__LVP_present(void);
void ADC_OVP_present__LVP_present(void);
void ShowSlaveAddressOnLED(uint16_t address, GPIO_TypeDef *port, uint16_t pin);
inline __attribute__((always_inline)) void MODBUS_Management(void);
void DEBUG_print(uint32_t ticks);
void HeavyCalculations(uint32_t ticks);
void Keys_Management(void);
void AUTO_LVP_OVP_Management(uint32_t ticks, int reset);
void AUTO_LVP_Management(uint32_t ticks, int reset);
void AUTO_OVP_Management(uint32_t ticks, int reset);
void DIP_Switches(void);
void OVP_ignored__LVP_not_present(void);
void OVP_ignored__LVP_present(void);
void ADC_OVP_ignored__LVP_not_present(void);
void ADC_OVP_ignored__LVP_present(void);
void OVP_not_present__LVP_ignored(void);
void OVP_present__LVP_ignored(void);
void ADC_OVP_not_present__LVP_ignored(void);
void ADC_OVP_present__LVP_ignored(void);
void ADC_Close_Both_MOSFETs(void);
void ADC_Open_Both_MOSFETs(void);
void SystemClock_Decrease(void);
void EnterPowerSavingMode(void);
void ExitPowerSavingMode(void);
void StartOffMode(int reset);
void LEDs_Management(void);
void BlueLEDShortBlinking(void);
void TurnBlueLEDOn(void);
void TurnExternalGreenLEDOn(void);
void TurnExternalGreenLEDOff(void);
//void ExternalRedLEDShortBlinking(void);
//void ExternalRedLEDVeryShortBlinking(void);
void ExternalGreenLEDShortBlinking(void);
void OVP_Management_NoAutoreconnect(uint32_t new_time, int reset);
void OVP_present__LVP_ignored_NoAutoreconnect(void);
void LVP_Management_NoAutoreconnect(uint32_t new_time, int reset);
void OVP_ignored__LVP_present_NoAutoreconnect(void);
void LVP_OVP_Management_NoAutoreconnect(uint32_t new_time, int reset);
void ShortCutCheck(void);
void TrueRMSCurrentCalculation(void);
void TrueRMSCurrentOfflineCalculation(void);
void CurrentOfflineCalculation(void);
void CallibrateVoltageDropABMiddlePointOffset(void);
void CallibrateControlCurrentVoltageDropOnContactBB(void);
void CallibrateCurrentSensorZeroOffsetOnContactBB(void);
void ABVoltageDropCalculation(void);
inline __attribute__((always_inline)) void OpenBothMOSFETS(void);
inline __attribute__((always_inline)) void CloseBothMOSFETS(void);
void InrushCurrentDetected(void);
void ExternalRedLED1ShortOnThenLongPauseBlinking(void);
void ExternalRedLED2ShortOnThenLongPauseBlinking(void);
void ExternalRedLED3ShortOnThenLongPauseBlinking(void);
void ExternalRedLED4ShortOnThenLongPauseBlinking(void);
void ExternalRedLED5ShortOnThenLongPauseBlinking(void);
void ExternalRedLED6ShortOnThenLongPauseBlinking(void);
void ExternalRedLED2ShortOnThen2LongOnThenLongPauseBlinking(void);
void RS485DisableButtonManagement(uint32_t new_time);
static void BatteryLowVoltageProtection(/*uint32_t current_time*/);

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */
#ifdef DEBUG
  __HAL_RCC_DBGMCU_CLK_ENABLE();
  DBG->APBFZ1 |= (DBG_APB_FZ1_DBG_TIM2_STOP | DBG_APB_FZ1_DBG_TIM6_STOP | DBG_APB_FZ1_DBG_TIM7_STOP);
  DBG->APBFZ2 |= (DBG_APB_FZ2_DBG_TIM14_STOP | DBG_APB_FZ2_DBG_TIM15_STOP | DBG_APB_FZ2_DBG_TIM16_STOP | DBG_APB_FZ2_DBG_TIM17_STOP);
#endif
  command_parser_is_enabled = 1;
  uint32_t new_time;
  uint32_t old_time = 0;
  //uint32_t MIN_MAX_CALCULATION_DELAY = 3000;
  uint32_t stat_last_time_checked = 0;
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */
#define MX_IWDG_Init  DoNothing	  // This line helps to eliminate early start of WatchDog timer and keep code reconfigurable by CubeMx
  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_ADC1_Init();
  MX_USART1_UART_Init();
  MX_CRC_Init();
  MX_DAC1_Init();
  MX_TIM17_Init();
  MX_IWDG_Init();
  MX_TIM16_Init();
  MX_TIM14_Init();
  MX_TIM7_Init();
  MX_TIM6_Init();
  MX_TIM2_Init();
  MX_TIM15_Init();
  /* USER CODE BEGIN 2 */
#undef MX_IWDG_Init	  // Removing previous definition, that helped not to start a watchdog

//#ifdef DEBUG
  //RCC->APBENR1 |= RCC_APBENR1_DBGEN;
  //DBG->APBFZ1 |= DBG_APB_FZ1_DBG_TIM7_STOP | DBG_APB_FZ1_DBG_TIM2_STOP | DBG_APB_FZ1_DBG_TIM6_STOP;
  //DBG->APBFZ2 |= DBG_APB_FZ2_DBG_TIM14_STOP | DBG_APB_FZ2_DBG_TIM16_STOP | DBG_APB_FZ2_DBG_TIM17_STOP;
//#endif

  SYSDATA_Init();

  if (HAL_TIM_Base_Start(&htim2) != HAL_OK) LOG_E(TAG, "Cannot start TIMER2!");

  SEGGER_RTT_printf(0, RTT_CTRL_CLEAR);
  LOG_I(TAG, "Program started.");
  switch(DBG->IDCODE & 0xFFF)
  {
	  case 0x467: LOG_I(TAG, "Device ID: STM32G0B1 or STM32G0C1"); break;
	  case 0x460: LOG_I(TAG, "Device ID: STM32G071 or STM32G081"); break;
	  case 0x456: LOG_I(TAG, "Device ID: STM32G051 or STM32G061"); break;
	  case 0x466: LOG_I(TAG, "Device ID: STM32G031 or STM32G041"); break;
	  default: LOG_I(TAG, "Device ID: unknown");
  }
  SEGGER_RTT_printf(0, "%s: Revision number: 0x%4X\n", TAG, DBG->IDCODE>>16);
  SEGGER_RTT_printf(0, "Free space for cofiguration in fake EEPROM: %u bytes\n", FEEPROM_ConfigFreeBytes());
  SEGGER_RTT_printf(0, "Free space for statistics in fake EEPROM: %u bytes\n", FEEPROM_StatFreeBytes());
  SEGGER_RTT_printf(0, "MAX_POSSIBLE_DIFF_TO_MEASURE: %u\n", MAX_POSSIBLE_DIFF_TO_MEASURE);
  SEGGER_RTT_printf(0, "ADC_BAT_CRITICAL_VOLTAGE: %u\n", ADC_BAT_CRITICAL_VOLTAGE);
  SEGGER_RTT_printf(0, "CPU Freq: %u Hz\n", HAL_RCC_GetSysClockFreq());

  if (HAL_RCC_GetSysClockFreq() < 64000000)
  {
	  LOG_E(TAG, "CPU speed is not 64MHz!");
	  LOG_E(TAG, "Trying to restart.");
	  HAL_NVIC_SystemReset();
  }

  StartUpSequence();

#ifdef USE_RAM_FUNC
  CopyingVectorTableToRAM();
#endif

  if(FEEPROM_isFirstStart())
  {
	  LOG_W(TAG, "First start! Writing default configuration!");
	  FEEPROM_fullRestore(/*&sys_data*/);
	  FEEPROM_ResetLogData();
  }

  // Fetching configuration from FLASH
  if (FEEPROM_readConfig(&sys_data)) LOG_E(TAG, "Cannot read configuration from FLASH memory!");
  if (FEEPROM_ReadLogData(&sys_data)) LOG_E(TAG, "Cannot read statistcal data from FLASH memory!");

  sys_data.s.startup_cnt++;	  // Start-up counter
  statDataChanged = 1;
  maxIntegral = sys_data.s.inrush_max_current_in_adc * sys_data.s.inrush_curr_integral_steps;
  sys_data.s.copper_v_drop_adc_limit = (sys_data.s.copper_v_drop_adc * 110) / 100;

  ShowSlaveAddressOnLED(sys_data.s.slave_address, LED_ERROR_GPIO_Port, LED_ERROR_Pin);

  // Modbus Initialisierung
  if (sys_data.s.parity_mode == 'e') mbInit(&modbusData, sys_data.s.baudrate, MODBUS_UART_PARITY_EVEN, &huart1, accessModeTable, &keyAccepted);
  else if (sys_data.s.parity_mode == 'o') mbInit(&modbusData, sys_data.s.baudrate, MODBUS_UART_PARITY_ODD, &huart1, accessModeTable, &keyAccepted);
  else mbInit(&modbusData, sys_data.s.baudrate, MODBUS_UART_PARITY_NONE, &huart1, accessModeTable, &keyAccepted);

  // ADC self-calibration
  if (HAL_ADC_Stop(&hadc1) == HAL_OK) // Stopping ADC
  {
      if (HAL_ADCEx_Calibration_Start (&hadc1) == HAL_OK)
      {
          uint32_t calibration_value = HAL_ADCEx_Calibration_GetValue(&hadc1);
          SEGGER_RTT_printf(0, "%s%s: ADC Calibration value: %u\n", RTT_CTRL_TEXT_BRIGHT_GREEN,TAG, calibration_value | 0x3F);
      }
      else LOG_E(TAG, "ADC calibration error!");
  }
  else LOG_E(TAG, "Cannot stop ADC!");

  // DAC calibration
  uint32_t calib_1 = HAL_DACEx_GetTrimOffset(&hdac1, MOSFET_CHANNEL_A);
  uint32_t calib_2 = HAL_DACEx_GetTrimOffset(&hdac1, MOSFET_CHANNEL_B);
  SEGGER_RTT_printf(0, "%s: DAC Calibration value for channel 1: %u\n", TAG, calib_1);
  SEGGER_RTT_printf(0, "%s: DAC Calibration value for channel 2: %u\n", TAG, calib_2);

  // Before ADC start we must initialize our MOSFETs management function pointer
  StartOffMode(1);

  //MOSFETS_Management = &ADC_Open_Both_MOSFETs;								// & is optional, but it clearly refers to pointer initialization
  //sys_data.s.user_button_mode = SWITCH_OFF;									// Initial state of the switch
  if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)ADC_values, ADC_CHANNELS) != HAL_OK) LOG_E(TAG, "Cannot start ADC in DMA mode!");

  DMA1_Channel1->CCR &= ~DMA_CCR_HTIE;											// Disabling Half-Transfer interrupt, because we don't need it

  // Starting DAC
  HAL_DAC_Start(&hdac1, MOSFET_CHANNEL_A);
  HAL_DAC_Start(&hdac1, MOSFET_CHANNEL_B);
  // Setting new values
  HAL_DAC_SetValue(&hdac1, MOSFET_CHANNEL_A, DAC_ALIGN_12B_R, DAC_0V);
  HAL_DAC_SetValue(&hdac1, MOSFET_CHANNEL_B, DAC_ALIGN_12B_R, DAC_0V);

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */

  DIP_Switches();

  // Initializing independant watchdog timer (cannot be disabled)
  //MX_IWDG_Init();

  // Due to the fact, that gSwitch always start in OFF state, after start-up
  // we can enter low power mode, because nothing dangerous happens in OFF mode
  //EnterPowerSavingMode();

  InternalGreenLED_Management = &DoNothing;
  InternalBlueLED_Management = &BlueLEDShortBlinking;
  InternalRedLED_Management = &DoNothing;
  ExternalGreenLED_Management = &DoNothing;
  ExternalRedLED_Management = &DoNothing;
  Callibration = &DoNothing;
  InrushCurrentManagement = &InrushCurrentDetected;

  while (1)																		// Main loop
  {
	  TP4_GPIO_Port->BSRR = TP4_Pin;

	  ABVoltageDropCalculation();

	  BatteryLowVoltageProtection();

	  TP4_GPIO_Port->BRR = TP4_Pin;

	  //TIM2->CNT = 0;
	  //CurrentOfflineCalculation();
	  //TrueRMSCurrentOfflineCalculation();	// Max execution time is
	  //SEGGER_RTT_printf(0, "Function <TrueRMSCurrentCalculation> lasts %u cycles\n", TIM2->CNT);

	  MODBUS_Management();														// This function does not rely on time event

	  Keys_Management();

	  new_time = HAL_GetTick();													// Saving current time
	  if (new_time == old_time) continue;										// If tick value hasn't changed since last time, then it is useless to check the rest of conditions below
	  old_time = new_time;														// Saving current time value

	  //HAL_IWDG_Refresh(&hiwdg);	// 0.5s RESET

	  Callibration();

 	  LEDs_Management();

      // Printing DEBUG messages
	  // If SWD connector is not connected, we do not waste time for printing
	  swdioConnection <<= 1;										// Preparing space for 1 bit
	  swdioConnection |= HAL_GPIO_ReadPin(SWCLK_Port, SWCLK_Pin);	// Sampling SWCLK pin
	  if (swdioConnection) DEBUG_print(new_time);					// If debugger is connected, then we show debug messages

	  // Some not extremely urgent values calculations
	  HeavyCalculations(new_time);	  // Max execution time is 74µs

	  RS485DisableButtonManagement(new_time);

	  //Speicher Log Daten maximal jede Stunde, aber auch nur dann, wenn siche Werte, seit dem letztem mal geändert haben
	  if (new_time - stat_last_time_checked > HOUR_TIME_INTERVALL)
	  {
		  stat_last_time_checked = new_time;
		  LOG_I(TAG, "It is time to save statistical data in Flash memory.");

		  if (statDataChanged)
		  {
			  FEEPROM_StoreLogData(&sys_data);
			  statDataChanged = false;
		  }
	  }


	  static int restartAutoMode = 0;
	  // Checking switch state (Can be changed via Modbus or external button)
	  switch(sys_data.s.user_button_mode)
	  {
		  case SWITCH_OFF:			// If button is set to OFF, then we disconnect both MOSFETS
			  break;

		  case SWITCH_ON:			// If button is set to ON, then we connect both MOSFETS (Danger!!!)
			  // Checking whether temperature, current and battery voltage are within limits
			  if (temperature_shutdown_is_active == 1)
			  {
				  // If so, opening both MOSFETs (i.e. disconnecting load/charger from battery)
#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
				  DisableShortCutDetection();
#endif
				  HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
				  MOSFETS_Management = &ADC_Open_Both_MOSFETs;
				  sys_data.s.relay_status = RELAY_IS_OPENED;
				  HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);
				  ExternalRedLED_Management = &ExternalRedLED1ShortOnThenLongPauseBlinking;
			  }
			  else if (overcurrent_shutdown_is_active == 1)			ExternalRedLED_Management = &ExternalRedLED2ShortOnThenLongPauseBlinking;
			  else if (mosfets_voltagedrop_shutdown_is_active == 1)	ExternalRedLED_Management = &ExternalRedLED3ShortOnThenLongPauseBlinking;
			  else if (overload_shutdown_is_active == 1)			ExternalRedLED_Management = &ExternalRedLED4ShortOnThenLongPauseBlinking;
			  break;

		  case SWITCH_AUTO:
			  // Checking whether temperature, current and battery voltage are within limits
			  if (temperature_shutdown_is_active == 1)
			  {
				  if (!restartAutoMode)
				  {
					  // If so, opening both MOSFETs (i.e. disconnecting load/charger from battery)
					  /*
#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
					  DisableShortCutDetection();
#endif
					  HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
					  MOSFETS_Management = &ADC_Open_Both_MOSFETs;
					  sys_data.s.relay_status = RELAY_IS_OPENED;
					  HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);
					  ExternalRedLED_Management = &ExternalRedLED1ShortOnThenLongPauseBlinking;
					  */
					  restartAutoMode = 1;
				  }
			  }
			  else if (low_bat_shutdown_is_active == 1)
			  {
				  if (!restartAutoMode)
				  {
					  // If so, opening both MOSFETs (i.e. disconnecting load/charger from battery)
#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
					  DisableShortCutDetection();
#endif
					  HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
					  MOSFETS_Management = &DoNothing;
					  OpenBothMOSFETSVeryFast();
					  sys_data.s.relay_status = RELAY_IS_OPENED;
					  HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);
					  ExternalRedLED_Management = &ExternalRedLED5ShortOnThenLongPauseBlinking;
					  restartAutoMode = 1;
				  }
			  }
			  else if (overcurrent_shutdown_is_active == 1)
			  {
				  if (!restartAutoMode) { ExternalRedLED_Management = &ExternalRedLED2ShortOnThenLongPauseBlinking;  restartAutoMode = 1; }
			  }
			  else if (mosfets_voltagedrop_shutdown_is_active == 1)
			  {
				  if (!restartAutoMode) { ExternalRedLED_Management = &ExternalRedLED3ShortOnThenLongPauseBlinking;  restartAutoMode = 1; }
			  }
			  else if (overload_shutdown_is_active == 1)
			  {
				  if (!restartAutoMode) { ExternalRedLED_Management = &ExternalRedLED4ShortOnThenLongPauseBlinking;  restartAutoMode = 1; }
			  }
			  else
			  {
				  // Normal operations

				  /** This is a main function that is called in AUTO mode.
					  Which one function is executed depends on DIP switch state.
					  In general, here can be executed 3 functions:
						1. AUTO_LVP_Management (OVP is ignored)
						2. AUTO_OVP_Management (LVP is ignored)
						3. AUTO_LVP_OVP_Management
					  As long as device is in AUTO mode, this function is called
					  periodically.
				  */
				  AUTO_Mode(new_time, restartAutoMode);
				  restartAutoMode = 0;
			  }
			  break;
      }

    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */

  } /* while (1) */

  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI|RCC_OSCILLATORTYPE_LSI
                              |RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSIDiv = RCC_HSI_DIV8;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.LSIState = RCC_LSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV1;
  RCC_OscInitStruct.PLL.PLLN = 16;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
  RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }

  /** Enables the Clock Security System
  */
  HAL_RCC_EnableCSS();
}

/* USER CODE BEGIN 4 */

//-----------------------------------------------------------------------------

void RS485DisableButtonManagement(uint32_t new_time)
{
	static uint32_t btn_last_time_checked = 0;
	uint32_t BTN_SCAN_PERIOD = 25;
	static uint8_t btn_state = 0;
	static int transition = 1;

	// Scanning special button on the board, which disables RS485 MODBUS interface
	if (new_time - btn_last_time_checked > BTN_SCAN_PERIOD)
	{
		btn_last_time_checked = new_time;
		BTN_SCAN_PERIOD = 25;

		btn_state <<= 1;

		// If this special button is pressed
		if (HAL_GPIO_ReadPin(BTN1_GPIO_Port, BTN1_Pin) == BTN_IS_PRESSED)
		{
			//BTN_SCAN_PERIOD = 1500;
			btn_state |= 1;

			if (btn_state & 0xFF)
			{
				//btn_state = 0;
    			// If MODBUS RS485 interface is enabled
				if (transition)
				{
					transition = 0;
        			if (RS485ActiveMode)
        			{
        				// We disable it
        				InternalBlueLED_Management = &TurnBlueLEDOn;
        				RS485ActiveMode = 0;
        			}
        			else
        			{
        				// We enable it
        				InternalBlueLED_Management = &BlueLEDShortBlinking;
        				RS485ActiveMode = 1;
        			}
				}
			}
		}
		else
		{
			btn_state |= 0;
			if (btn_state == 0) transition = 1;
		}
	}
}

//-----------------------------------------------------------------------------

void ABVoltageDropCalculation(void)
{
	static int32_t ursense_voltage_accum = 0;
	static volatile uint32_t last_time_UabCalculated = 0;
	static int positive_pulse_found = 0;

	static volatile uint32_t new_time;
	new_time = HAL_GetTick();

	if (new_time - last_time_UabCalculated > 1)
	{
		last_time_UabCalculated = new_time;
		// Calculating real voltage drop between contacts B and A in mV
		sys_data.s.ab_raw_adc_value_with_offset = rawMOSFETsVoltageDrop + sys_data.s.ab_middle_point_offset;
		int32_t temp = ((sys_data.s.ab_raw_adc_value_with_offset) * 2 * MAX_POSSIBLE_DIFF_TO_MEASURE) / ADC_MAX_VALUE - MAX_POSSIBLE_DIFF_TO_MEASURE;
		// Calculating averaged value for real voltage drop between contacts B and A in mV
		ursense_voltage_accum -= sys_data.s.ursense_voltage;
		ursense_voltage_accum += temp;
		sys_data.s.ursense_voltage = ursense_voltage_accum /16384;

		if ((sys_data.s.relay_status != RELAY_IS_OPENED)/* || (sys_data.s.relay_status == ONLY_BA_OPENED) || (sys_data.s.relay_status == ONLY_AB_OPENED)*/)
		{
			if (!positive_pulse_found)
			{
				positive_pulse_found = 1;
				ursense_voltage_accum = 0;
				sys_data.s.ursense_voltage = 0;
				return;
			}

			if (sys_data.s.relay_status == ONLY_BA_OPENED)
			{
				// Here we can start tracking the voltage drop on MOSFETs
				if (sys_data.s.ursense_voltage < -(MAX_ALLOWED_MOSFETS_V_DROP + MAX_ALLOWED_MOSFETS_V_DROP_DELTA))
				{
					if (mosfets_voltagedrop_shutdown_is_active == 0)
					{
						HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
						OpenBothMOSFETSVeryFast();
						MOSFETS_Management = &DoNothing;
						HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);
						mosfets_voltagedrop_shutdown_is_active = 1;
						//sys_data.s.device_status |= (1 << ABBA_VOLTAGE_ERROR);
						sys_data.s.mosfets_voltagedrop_error_cnt++;
						statDataChanged = 1;
					}
				}
			}
			else if (sys_data.s.relay_status == ONLY_AB_OPENED)
			{
				// Here we can start tracking the voltage drop on MOSFETs
				if (sys_data.s.ursense_voltage > (MAX_ALLOWED_MOSFETS_V_DROP + MAX_ALLOWED_MOSFETS_V_DROP_DELTA))
				{
					if (mosfets_voltagedrop_shutdown_is_active == 0)
					{
						HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
						OpenBothMOSFETSVeryFast();
						MOSFETS_Management = &DoNothing;
						HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);
						mosfets_voltagedrop_shutdown_is_active = 1;
						//sys_data.s.device_status |= (1 << ABBA_VOLTAGE_ERROR);
						sys_data.s.mosfets_voltagedrop_error_cnt++;
						statDataChanged = 1;
					}
				}
			}
		}
		else positive_pulse_found = 0;

		//SEGGER_RTT_printf(0, "Uab = %4d    Rstatus = %d\n", sys_data.s.ursense_voltage, sys_data.s.relay_status);
	}
}

//-----------------------------------------------------------------------------

void mb_save_lock_key(void)
{
  if (sys_data.s.lockKey == savedLockKey)
  {
    FEEPROM_storeConfig(&sys_data, false);
    savedLockKey = sys_data.s.newLockKey;
    sys_data.s.lockKey = savedLockKey;
  }

  if (savedLockKey != 0)
  {
    sys_data.s.writeLocked = 1;
  }
  else
  {
    sys_data.s.writeLocked = 0;
  }
}

//-----------------------------------------------------------------------------

void SYSDATA_Init(void)
{
  memset(&sys_data, 0, sizeof(sys_data));

  sys_data.s.device_type_id = DEVICE_TYPE_ID;
  sys_data.s.fw_major		= FW_VERSION_MAJOR;
  sys_data.s.fw_minor		= FW_VERSION_MINOR;
  sys_data.s.fw_revision	= FW_VERSION_REVISION;

  sys_data.s.command = 0;
  sys_data.s.device_status = 0;
  sys_data.s.ubsenseb_voltage = 1;
}

//-----------------------------------------------------------------------------

void CurrentOfflineCalculation(void)
{
	static uint32_t last_time = 0;

	uint32_t new_time = HAL_GetTick();

	if (new_time - last_time >= 1)
	{
		last_time = new_time;

		// Sliding average calculation of discharge current
		//current_temperature = (((MAX_TEMP - MIN_TEMP)*((int)ADC_values[TEMP_CHANNEL] - TEMP_SENSOR_ADC_AT_MINUS30))/(TEMP_SENSOR_ADC_AT_PLUS100 - TEMP_SENSOR_ADC_AT_MINUS30)) + MIN_TEMP;
		//temperature_accum -= sys_data.s.temperature;
		//temperature_accum += current_temperature;
		//sys_data.s.temperature = temperature_accum / 32;//>> 5;

	}

}

//-----------------------------------------------------------------------------

void TrueRMSCurrentOfflineCalculation(void)
{
	static uint32_t last_time = 0;
	static uint32_t samples_cnt = 0;
	static uint64_t sum = 0;
	const uint32_t N = 8;

	uint32_t new_time = HAL_GetTick();

	if (new_time - last_time >= 1)
	{
		last_time = new_time;

		// rawContactVoltageDrop is in the range  [-4095, 4095]
		// Saving it in temporary variable, to prevent corruption of its value in ISR
		int32_t tmp = rawContactVoltageDropPlus;
		sum += (tmp * tmp);
		samples_cnt++;
		//SEGGER_RTT_printf(0, "[%3u] %5d %8u\n", samples_cnt, tmp, sum);
	}

	if (samples_cnt > (CONTROL_CURRENT_A*N))	  // Number of samples is picked to correspond to CONTROL_CURRENT_A value. Must be CONTROL_CURRENT_A * N, where N is [1,2,3...]
	{
		samples_cnt = 0;
		//SEGGER_RTT_printf(0, "%u\n", sum);

		if (sys_data.s.ursense_voltage >= 0)
		{
			sys_data.s.current = sqrtf((sum*CONTROL_CURRENT_A)/(sys_data.s.copper_v_drop_adc * sys_data.s.copper_v_drop_adc * N));
		}
		else
		{
			sys_data.s.current = -sqrtf((sum*CONTROL_CURRENT_A)/(sys_data.s.copper_v_drop_adc * sys_data.s.copper_v_drop_adc * N));
		}

		//SEGGER_RTT_printf(0, "%d\n", sys_data.s.current);
		sum = 0;
	}

}

//-----------------------------------------------------------------------------

/*void TrueRMSCurrentCalculation(void)
{
	static uint64_t total_sum = 0;
	static uint32_t total_sum_cnt = 0;

	// Executing this if ISR filled all I_RMS_SAMPLES_COUNT values into i_samples array
	if (i_samples_counter == I_RMS_SAMPLES_COUNT)
	{
#ifdef DEBUG
		static uint64_t sum = 0;
#else
		uint64_t sum = 0;
		//int is_negative;
#endif
		for (int i = 0; i < I_RMS_SAMPLES_COUNT; i++)
		{
#ifdef DEBUG
			static int32_t shifted_value = 0;
			shifted_value = i_samples[i] - (ADC_MAX_VALUE>>1);
			static uint32_t zero_based_value;
#else
			static volatile int32_t shifted_value;
			shifted_value = i_samples[i] - (ADC_MAX_VALUE>>1);	// Delta from middle point of 2047
			//uint32_t zero_based_value;
#endif
			//if (shifted_value >= 0)
			//{
			//	zero_based_value = shifted_value;
			//	//is_negative = 0;
			//}
			//else
			//{
			//	zero_based_value = - shifted_value;
			//	//is_negative = 1;
			//}

			//static float divider;
			//divider = ADC_MAX_VALUE * sys_data.s.copper_v_drop * (1 + (COPPER_TEMP_COEFFICIENT * (sys_data.s.temperature - 200))/10);

			//current_value = (zero_based_value * CONTROL_CURRENT_A * ADC_VREF) / divider;

			//float divider = ADC_MAX_VALUE * sys_data.s.copper_v_drop * (1 + (COPPER_TEMP_COEFFICIENT * (sys_data.s.temperature - sys_data.s.copper_v_drop_temp))/10);
			//float divider = sys_data.s.copper_v_drop_adc * (1 + (COPPER_TEMP_COEFFICIENT * (sys_data.s.temperature - sys_data.s.copper_v_drop_temp))/10);
			static volatile int32_t current_value;
			current_value = (shifted_value * CONTROL_CURRENT_A) / sys_data.s.copper_v_drop_adc;
			if (current_value < 0)
			{
				if (-current_value > sys_data.s.max_discharge_current) sys_data.s.max_discharge_current = -current_value;
				else if (-current_value < sys_data.s.min_discharge_current) sys_data.s.min_discharge_current = -current_value;
			}
			else
			{
				if (current_value > sys_data.s.max_charge_current) sys_data.s.max_charge_current = current_value;
				else if (current_value < sys_data.s.min_charge_current) sys_data.s.min_charge_current = current_value;
			}

			sum += (shifted_value * shifted_value);
			//char div[50];
			//sprintf(div, "%f", divider);
			//SEGGER_RTT_printf(0, "%u, %u\n", u_samples[i], d_samples[i]);
		}

		i_samples_counter = 0;

		if (total_sum_cnt < I_RMS_SAMPLES_SUM_COUNT)
		{
			total_sum += sum;
			total_sum_cnt++;
		}
		else
		{
			total_sum_cnt = 0;
			//SEGGER_RTT_printf(0, "Sum: %d\tTotal: %d\n", sum, total_sum);
			total_sum = (total_sum * CONTROL_CURRENT_A * CONTROL_CURRENT_A) / (sys_data.s.copper_v_drop_adc * sys_data.s.copper_v_drop_adc);
			if (sys_data.s.ursense_voltage >= 0) sys_data.s.current = sqrtf(total_sum/(I_RMS_SAMPLES_COUNT * I_RMS_SAMPLES_SUM_COUNT));
			else sys_data.s.current = -sqrtf(total_sum/(I_RMS_SAMPLES_COUNT * I_RMS_SAMPLES_SUM_COUNT));
			//else sys_data.s.current = sqrtf(total_sum/(I_RMS_SAMPLES_COUNT * I_RMS_SAMPLES_SUM_COUNT));
			total_sum = 0;
		}
	}
}*/

//-----------------------------------------------------------------------------

/*#ifdef USE_RAM_FUNC
__RAM_FUNC void ShortCutCheck(void)
#else
void ShortCutCheck(void)
#endif
{
	static uint32_t last_time_checked = 0;
	static int current_integral_calc_started = 0;
	static uint32_t current_integral = 0;

	uint32_t current_time = HAL_GetTick();
	int32_t current_adc_value = abs(rawContactVoltageDrop - (ADC_MAX_VALUE>>1));

	if (current_time - last_time_checked > 1)
	{
		last_time_checked = current_time;

		if (current_adc_value > ADC_VALUE_DELTA_AT_CONTROL_CURRENT_A)
		{
			if (current_integral_calc_started)
			{
				current_integral += current_adc_value;
				if (current_integral > (ADC_VALUE_DELTA_AT_INRUSH_CURRENT * 5)) overcurrent_shutdown_is_active = 1;
				SEGGER_RTT_printf(0, "\t%d\t%d\n", current_adc_value, current_integral);
			}
			else
			{
				current_integral = 0;
				current_integral += current_adc_value;
				current_integral_calc_started = 1;
				SEGGER_RTT_printf(0, "Overcurrent: %d\n", current_adc_value);
				SEGGER_RTT_printf(0, "Starting integral calculation:\n");
				SEGGER_RTT_printf(0, "\t%d\t%d\n", current_adc_value, current_integral);
			}
		}
		else
		{
			if (current_integral_calc_started)
			{
				SEGGER_RTT_printf(0, "Stopping integral calculation:\n");
				SEGGER_RTT_printf(0, "\t%d\t%d\n", current_adc_value, current_integral);
				current_integral = 0;
				current_integral_calc_started = 0;
			}
		}
	}

}*/

//-----------------------------------------------------------------------------

#ifdef USE_RAM_FUNC
void CopyingVectorTableToRAM(void)
{
	uint32_t SrcAddress = SCB->VTOR;
	uint32_t DstAddress = (uint32_t)vectorTableInRAM;

	if (HAL_DMA_Start(&hdma_memtomem_dma1_channel2, SrcAddress, DstAddress, 192/4) != HAL_OK)
	{
		LOG_E(TAG, "Cannot copy Vector Table from FLASH to RAM! DMA is not ready!");
		while(1);
	}
	else LOG_I(TAG, "Starting Vector Table copying from FLASH to RAM...");

	if (HAL_DMA_PollForTransfer(&hdma_memtomem_dma1_channel2, HAL_DMA_FULL_TRANSFER, 1000) != HAL_OK)
	{
		LOG_E(TAG, "Cannot finish copying Vector Table from FLASH to RAM!");
		while(1);
	}
	else LOG_I(TAG, "Vector Table has been copied from FLASH to RAM.");
	SCB->VTOR = DstAddress;
}
#endif

//------------------------------------------------------------------------------

void LEDBlink(uint32_t *local_prev_on_time,	  // last tick counter value
			  unsigned *local_step,			  // current number of led ignitions
			  unsigned *local_subStep,		  // corresponds to current phase of led blinking process
			  unsigned local_totalStages,	  // how many times led should blink
			  uint16_t *local_turnOnTime,	  // array that contains times for how long led should stay in on state
			  uint16_t *local_turnOffTime,	  // array that contains times for how long led should stay in off state
			  GPIO_TypeDef *GPIOx,			  // port name
			  uint16_t GPIO_Pin)			  // pin number
{
	uint32_t new_time = HAL_GetTick();

	if (*local_step < local_totalStages)
	{
		switch((*local_subStep))
		{
			case 0:	// Phase 0 - turning led on
				HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET);
				*local_prev_on_time = new_time;
				(*local_subStep)++;
				break;

			case 1:	// Phase 1 - turning led off, if it is time
				if (new_time - *local_prev_on_time > local_turnOnTime[*local_step])
				{
					HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET);
					*local_prev_on_time = new_time;
					(*local_subStep)++;
				}
				break;

			case 2:	// Phase 2 - waiting before returning to phase 0
				if (new_time - *local_prev_on_time > local_turnOffTime[*local_step])
				{
					*local_prev_on_time = new_time;
					*local_subStep = 0;
					(*local_step)++;
				}
				break;
		}
	}
	else *local_step = 0;
}

//------------------------------------------------------------------------------

void RedLEDBlink(uint16_t *turnOnPeriods, uint16_t *turnOffPeriods, unsigned totalStages)
{
	static uint32_t RedLEDLastTickTime = 0;
	static unsigned stage = 0;
	static unsigned subStage = 0;

	LEDBlink(&RedLEDLastTickTime, &stage, &subStage, totalStages, turnOnPeriods, turnOffPeriods, LED_ERROR_GPIO_Port, LED_ERROR_Pin);
}

//------------------------------------------------------------------------------

void ExternalRedLEDBlink(uint16_t *turnOnPeriods, uint16_t *turnOffPeriods, unsigned totalStages)
{
	static uint32_t RedLEDLastTickTime = 0;
	static unsigned stage = 0;
	static unsigned subStage = 0;

	LEDBlink(&RedLEDLastTickTime, &stage, &subStage, totalStages, turnOnPeriods, turnOffPeriods, LED_SW_ERROR_GPIO_Port, LED_SW_ERROR_Pin);
}

//------------------------------------------------------------------------------

void ExternalRedLED2ShortOnThen2LongOnThenLongPauseBlinking(void)
{
	const unsigned stages = 4;
	uint16_t turnOnTimes[stages];
	uint16_t turnOffTimes[stages];

	turnOnTimes[0] = 200;
	turnOffTimes[0] = 200;

	turnOnTimes[1] = 200;
	turnOffTimes[1] = 500;

	turnOnTimes[2] = 700;
	turnOffTimes[2] = 500;

	turnOnTimes[stages - 1] = 700;
	turnOffTimes[stages - 1] = 2500;

	ExternalRedLEDBlink(turnOnTimes, turnOffTimes, stages);
	RedLEDBlink(turnOnTimes, turnOffTimes, stages);
}

//------------------------------------------------------------------------------

void ExternalRedLED6ShortOnThenLongPauseBlinking(void)
{
	const unsigned stages = 6;
	uint16_t turnOnTimes[stages];
	uint16_t turnOffTimes[stages];

	for (unsigned i = 0; i < stages - 1; i++)
	{
		turnOnTimes[i] = 200;
		turnOffTimes[i] = 200;
	}

	turnOnTimes[stages - 1] = 200;
	turnOffTimes[stages - 1] = 2500;

	ExternalRedLEDBlink(turnOnTimes, turnOffTimes, stages);
	RedLEDBlink(turnOnTimes, turnOffTimes, stages);
}

//------------------------------------------------------------------------------

void ExternalRedLED5ShortOnThenLongPauseBlinking(void)
{
	const unsigned stages = 5;
	uint16_t turnOnTimes[stages];
	uint16_t turnOffTimes[stages];

	for (unsigned i = 0; i < stages - 1; i++)
	{
		turnOnTimes[i] = 200;
		turnOffTimes[i] = 200;
	}

	turnOnTimes[stages - 1] = 200;
	turnOffTimes[stages - 1] = 2500;

	ExternalRedLEDBlink(turnOnTimes, turnOffTimes, stages);
	RedLEDBlink(turnOnTimes, turnOffTimes, stages);
}

//------------------------------------------------------------------------------

void ExternalRedLED4ShortOnThenLongPauseBlinking(void)
{
	const unsigned stages = 4;
	uint16_t turnOnTimes[stages];
	uint16_t turnOffTimes[stages];

	for (unsigned i = 0; i < stages - 1; i++)
	{
		turnOnTimes[i] = 200;
		turnOffTimes[i] = 200;
	}

	turnOnTimes[stages - 1] = 200;
	turnOffTimes[stages - 1] = 2500;

	ExternalRedLEDBlink(turnOnTimes, turnOffTimes, stages);
	RedLEDBlink(turnOnTimes, turnOffTimes, stages);
}

//------------------------------------------------------------------------------

void ExternalRedLED3ShortOnThenLongPauseBlinking(void)
{
	const unsigned stages = 3;
	uint16_t turnOnTimes[stages];
	uint16_t turnOffTimes[stages];

	for (unsigned i = 0; i < stages - 1; i++)
	{
		turnOnTimes[i] = 200;
		turnOffTimes[i] = 200;
	}

	turnOnTimes[stages - 1] = 200;
	turnOffTimes[stages - 1] = 2500;

	ExternalRedLEDBlink(turnOnTimes, turnOffTimes, stages);
	RedLEDBlink(turnOnTimes, turnOffTimes, stages);
}

//------------------------------------------------------------------------------

void ExternalRedLED2ShortOnThenLongPauseBlinking(void)
{
	const unsigned stages = 2;
	uint16_t turnOnTimes[stages];
	uint16_t turnOffTimes[stages];

	turnOnTimes[0] = 200;
	turnOffTimes[0] = 200;

	turnOnTimes[stages - 1] = 200;
	turnOffTimes[stages - 1] = 2500;

	ExternalRedLEDBlink(turnOnTimes, turnOffTimes, stages);
	RedLEDBlink(turnOnTimes, turnOffTimes, stages);
}

//------------------------------------------------------------------------------

void ExternalRedLED1ShortOnThenLongPauseBlinking(void)
{
	const unsigned stages = 1;
	uint16_t turnOnTimes[stages];
	uint16_t turnOffTimes[stages];

	turnOnTimes[stages - 1] = 200;
	turnOffTimes[stages - 1] = 2500;

	ExternalRedLEDBlink(turnOnTimes, turnOffTimes, stages);
	RedLEDBlink(turnOnTimes, turnOffTimes, stages);
}

//------------------------------------------------------------------------------

/*void ExternalRedLEDVeryShortBlinking(void)
{
	static uint32_t old_on_time = 0;
	static uint32_t led_is_turned_on = 0;
	uint32_t new_time;

	new_time = HAL_GetTick();
	if (!led_is_turned_on && (new_time - old_on_time > 200))
	{
		HAL_GPIO_WritePin(LED_SW_ERROR_GPIO_Port, LED_SW_ERROR_Pin, GPIO_PIN_SET);	// External Red LED on button
		led_is_turned_on = 1;
		old_on_time = new_time;
	}
	else if (led_is_turned_on && (new_time - old_on_time > 200))
	{
		HAL_GPIO_WritePin(LED_SW_ERROR_GPIO_Port, LED_SW_ERROR_Pin, GPIO_PIN_RESET);
		led_is_turned_on = 0;
		old_on_time = new_time;
	}
}*/

//------------------------------------------------------------------------------

/*void ExternalRedLEDShortBlinking(void)
{
	static uint32_t old_on_time = 0;
	static uint32_t led_is_turned_on = 0;
	uint32_t new_time;

	new_time = HAL_GetTick();
	if (!led_is_turned_on && (new_time - old_on_time > 800))
	{
		HAL_GPIO_WritePin(LED_SW_ERROR_GPIO_Port, LED_SW_ERROR_Pin, GPIO_PIN_SET);	// External Red LED on button
		led_is_turned_on = 1;
		old_on_time = new_time;
	}
	else if (led_is_turned_on && (new_time - old_on_time > 200))
	{
		HAL_GPIO_WritePin(LED_SW_ERROR_GPIO_Port, LED_SW_ERROR_Pin, GPIO_PIN_RESET);
		led_is_turned_on = 0;
		old_on_time = new_time;
	}
}*/

//------------------------------------------------------------------------------

void TurnExternalRedLEDOff(void)
{
	HAL_GPIO_WritePin(LED_SW_ERROR_GPIO_Port, LED_SW_ERROR_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(LED_ERROR_GPIO_Port, LED_ERROR_Pin, GPIO_PIN_RESET);
	ExternalRedLED_Management = &DoNothing;
}

//------------------------------------------------------------------------------

void ExternalGreenLEDShortBlinking(void)
{
	static uint32_t old_on_time = 0;
	static uint32_t led_is_turned_on = 0;
	uint32_t new_time;

	new_time = HAL_GetTick();
	if (!led_is_turned_on && (new_time - old_on_time > 800))
	{
		HAL_GPIO_WritePin(LED_SW_STATE_GPIO_Port, LED_SW_STATE_Pin, GPIO_PIN_SET);	// External Green LED on button
		led_is_turned_on = 1;
		old_on_time = new_time;
	}
	else if (led_is_turned_on && (new_time - old_on_time > 200))
	{
		HAL_GPIO_WritePin(LED_SW_STATE_GPIO_Port, LED_SW_STATE_Pin, GPIO_PIN_RESET);
		led_is_turned_on = 0;
		old_on_time = new_time;
	}
}

//------------------------------------------------------------------------------

void TurnExternalGreenLEDOff(void)
{
	HAL_GPIO_WritePin(LED_SW_STATE_GPIO_Port, LED_SW_STATE_Pin, GPIO_PIN_RESET);
	ExternalGreenLED_Management = &DoNothing;
}

//------------------------------------------------------------------------------

void TurnExternalGreenLEDOn(void)
{
	HAL_GPIO_WritePin(LED_SW_STATE_GPIO_Port, LED_SW_STATE_Pin, GPIO_PIN_SET);
	ExternalGreenLED_Management = &DoNothing;
}

//------------------------------------------------------------------------------

void GreenLEDShortBlinking(void)
{
	static uint32_t old_on_time = 0;
	static uint32_t led_is_turned_on = 0;
	uint32_t new_time;

	// Blue LED blinking (950ms - OFF, 50ms - ON)
	new_time = HAL_GetTick();
	if (!led_is_turned_on && (new_time - old_on_time > 950))
	{
		HAL_GPIO_WritePin(LED_STATE_GPIO_Port, LED_STATE_Pin, GPIO_PIN_SET);
		led_is_turned_on = 1;
		old_on_time = new_time;
	}
	else if (led_is_turned_on && (new_time - old_on_time > 50))
	{
		HAL_GPIO_WritePin(LED_STATE_GPIO_Port, LED_STATE_Pin, GPIO_PIN_RESET);
		led_is_turned_on = 0;
		old_on_time = new_time;
	}
}

//------------------------------------------------------------------------------

void TurnGreenLEDOff(void)
{
	HAL_GPIO_WritePin(LED_STATE_GPIO_Port, LED_STATE_Pin, GPIO_PIN_RESET);
	InternalGreenLED_Management = &DoNothing;
}

//------------------------------------------------------------------------------

void TurnGreenLEDOn(void)
{
	HAL_GPIO_WritePin(LED_STATE_GPIO_Port, LED_STATE_Pin, GPIO_PIN_SET);
	InternalGreenLED_Management = &DoNothing;
}

//------------------------------------------------------------------------------

void TurnBlueLEDOn(void)
{
	HAL_GPIO_WritePin(LED_FUNCTION_GPIO_Port, LED_FUNCTION_Pin, GPIO_PIN_SET);
	InternalBlueLED_Management = &DoNothing;
}

//------------------------------------------------------------------------------

void BlueLEDShortBlinking(void)
{
	static uint32_t old_on_time = 0;
	static uint32_t led_is_turned_on = 0;
	uint32_t new_time;

	// Blue LED blinking (950ms - OFF, 50ms - ON)
	new_time = HAL_GetTick();
	if (!led_is_turned_on && (new_time - old_on_time > 950))
	{
		HAL_GPIO_WritePin(LED_FUNCTION_GPIO_Port, LED_FUNCTION_Pin, GPIO_PIN_SET);
		led_is_turned_on = 1;
		old_on_time = new_time;
	}
	else if (led_is_turned_on && (new_time - old_on_time > 50))
	{
		HAL_GPIO_WritePin(LED_FUNCTION_GPIO_Port, LED_FUNCTION_Pin, GPIO_PIN_RESET);
		led_is_turned_on = 0;
		old_on_time = new_time;
	}
}

//------------------------------------------------------------------------------

void LEDs_Management(void)
{
	InternalGreenLED_Management();
	InternalBlueLED_Management();
	InternalRedLED_Management();
	ExternalGreenLED_Management();
	ExternalRedLED_Management();
}

//------------------------------------------------------------------------------

void ExitPowerSavingMode(void)
{
	//HAL_GPIO_WritePin(DISABLE_VBOOST_GPIO_Port, DISABLE_VBOOST_Pin, GPIO_PIN_SET);	// Turning VBOOST voltage generator on

	//HAL_PWREx_DisableLowPowerRunMode();
	//HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1);
	//SystemClock_Config();

	/*modbusData.uart->Instance->PRESC = UART_PRESCALER_DIV8;
	// Modbus Initialisierung
	if (sys_data.s.parity_mode == 'e') mbInit(&modbusData, sys_data.s.baudrate, MODBUS_UART_PARITY_EVEN, &huart1);
	else if (sys_data.s.parity_mode == 'o') mbInit(&modbusData, sys_data.s.baudrate, MODBUS_UART_PARITY_ODD, &huart1);
	else mbInit(&modbusData, sys_data.s.baudrate, MODBUS_UART_PARITY_NONE, &huart1);*/

}

//------------------------------------------------------------------------------

void EnterPowerSavingMode(void)
{
	//SystemClock_Decrease();														// Reduce the System clock to 2 MHz
	//HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE2);				// Set regulator voltage to scale 2
	//HAL_PWREx_EnableLowPowerRunMode();											// Enter LP RUN Mode
}

//------------------------------------------------------------------------------

void SystemClock_Decrease(void)
{
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};

  /* Select HSI as system clock source */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
  if(HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
  {
    /* Initialization Error */
    Error_Handler();
  }

  /* Modify HSI to HSI DIV8 */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSIDiv = RCC_HSI_DIV8;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_OFF;
  if(HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    /* Initialization Error */
    Error_Handler();
  }
}

//------------------------------------------------------------------------------

void DIP_Switches(void)
{
	// Checking positions of DIP switches
	if ((HAL_GPIO_ReadPin(DIP0_GPIO_Port, DIP0_Pin) == DIP_IS_OFF) &&
		(HAL_GPIO_ReadPin(DIP1_GPIO_Port, DIP1_Pin) == DIP_IS_OFF) &&
	    (HAL_GPIO_ReadPin(DIP2_GPIO_Port, DIP2_Pin) == DIP_IS_OFF))
	{
		// Auto-reconnect DIP-switch
		if (HAL_GPIO_ReadPin(DIP3_GPIO_Port, DIP3_Pin) == DIP_IS_ON)
		{
			// Modus 0 (LVP only. Active level - LOW)
			LOG_I(TAG, "Mode 0 is selected. Auto-reconnect is ON.");

			AUTO_Mode = &AUTO_LVP_Management;
			// Assigning default functions to function pointers array, depending on LVP state
			LVP_OVP[0] = &OVP_ignored__LVP_not_present;		//  0
			LVP_OVP[1] = &OVP_ignored__LVP_present;			//  1
			LVP_OVP[2] = LVP_OVP[3] = &DoNothing;			// Not used
			//sys_data.s.ovp_state = 2;						// Ignored state
			LVP_OVP_logic = LOGIC_NEGATIV;
			sys_data.s.dip_mode = 0 | (1 << 3);
		}
		else
		{
			// Modus 0 (LVP only. Active level - LOW)
			LOG_I(TAG, "Mode 0 is selected. Auto-reconnect is OFF.");

			AUTO_Mode = &LVP_Management_NoAutoreconnect;
			// Assigning default functions to function pointers array, depending on LVP state
			LVP_OVP[0] = &OVP_ignored__LVP_not_present;				//  0
			LVP_OVP[1] = &OVP_ignored__LVP_present_NoAutoreconnect;	//  1
			LVP_OVP[2] = LVP_OVP[3] = &DoNothing;			// Not used
			//sys_data.s.ovp_state = 2;						// Ignored state
			LVP_OVP_logic = LOGIC_NEGATIV;
			sys_data.s.dip_mode = 0;
		}
	}
	else if ((HAL_GPIO_ReadPin(DIP0_GPIO_Port, DIP0_Pin) == DIP_IS_ON) &&
			 (HAL_GPIO_ReadPin(DIP1_GPIO_Port, DIP1_Pin) == DIP_IS_OFF) &&
			 (HAL_GPIO_ReadPin(DIP2_GPIO_Port, DIP2_Pin) == DIP_IS_OFF))
	{
		// Auto-reconnect DIP-switch
		if (HAL_GPIO_ReadPin(DIP3_GPIO_Port, DIP3_Pin) == DIP_IS_ON)
		{
			// Modus 1 (OVP only. Active level - LOW)
			LOG_I(TAG, "Mode 1 is selected. Auto-reconnect is ON.");

			AUTO_Mode = &AUTO_OVP_Management;
			// Assigning default functions to function pointers array, depending on OVP state
			LVP_OVP[0] = &OVP_not_present__LVP_ignored;		//  0
			LVP_OVP[1] = &OVP_present__LVP_ignored;			//  1
			LVP_OVP[2] = LVP_OVP[3] = &DoNothing;			// Not used
			//sys_data.s.lvp_state = 2;						// Ignored state
			LVP_OVP_logic = LOGIC_NEGATIV;
			sys_data.s.dip_mode = 1 | (1 << 3);
		}
		else
		{
			// Modus 1 (OVP only. Active level - LOW)
			LOG_I(TAG, "Mode 1 is selected. Auto-reconnect is OFF.");

			AUTO_Mode = &OVP_Management_NoAutoreconnect;
			// Assigning default functions to function pointers array, depending on OVP state
			LVP_OVP[0] = &OVP_not_present__LVP_ignored;				//  0
			LVP_OVP[1] = &OVP_present__LVP_ignored_NoAutoreconnect;	//  1
			LVP_OVP[2] = LVP_OVP[3] = &DoNothing;			// Not used
			//sys_data.s.lvp_state = 2;						// Ignored state
			LVP_OVP_logic = LOGIC_NEGATIV;
			sys_data.s.dip_mode = 1;
		}
	}
	else if ((HAL_GPIO_ReadPin(DIP0_GPIO_Port, DIP0_Pin) == DIP_IS_OFF) &&
			 (HAL_GPIO_ReadPin(DIP1_GPIO_Port, DIP1_Pin) == DIP_IS_ON) &&
			 (HAL_GPIO_ReadPin(DIP2_GPIO_Port, DIP2_Pin) == DIP_IS_OFF))
	{
		// Auto-reconnect DIP-switch
		if (HAL_GPIO_ReadPin(DIP3_GPIO_Port, DIP3_Pin) == DIP_IS_ON)
		{
			// Modus 2 (LVP only. Active level - HIGH)
			LOG_I(TAG, "Mode 2 is selected. Auto-reconnect is ON.");

			AUTO_Mode = &AUTO_LVP_Management;
			// Assigning default functions to function pointers array, depending on LVP state
			LVP_OVP[0] = &OVP_ignored__LVP_not_present;		//  0
			LVP_OVP[1] = &OVP_ignored__LVP_present;			//  1
			LVP_OVP[2] = LVP_OVP[3] = &DoNothing;			// Not used
			//sys_data.s.ovp_state = 2;						// Ignored state
			LVP_OVP_logic = LOGIC_POSITIV;
			sys_data.s.dip_mode = 2 | (1 << 3);
		}
		else
		{
			// Modus 2 (LVP only. Active level - HIGH)
			LOG_I(TAG, "Mode 2 is selected. Auto-reconnect is OFF.");

			AUTO_Mode = &LVP_Management_NoAutoreconnect;
			// Assigning default functions to function pointers array, depending on LVP state
			LVP_OVP[0] = &OVP_ignored__LVP_not_present;				//  0
			LVP_OVP[1] = &OVP_ignored__LVP_present_NoAutoreconnect;	//  1
			LVP_OVP[2] = LVP_OVP[3] = &DoNothing;					// Not used
			//sys_data.s.ovp_state = 2;								// Ignored state
			LVP_OVP_logic = LOGIC_POSITIV;
			sys_data.s.dip_mode = 2;
		}
	}
	else if ((HAL_GPIO_ReadPin(DIP0_GPIO_Port, DIP0_Pin) == DIP_IS_ON) &&
			 (HAL_GPIO_ReadPin(DIP1_GPIO_Port, DIP1_Pin) == DIP_IS_ON) &&
			 (HAL_GPIO_ReadPin(DIP2_GPIO_Port, DIP2_Pin) == DIP_IS_OFF))
	{
		// Auto-reconnect DIP-switch
		if (HAL_GPIO_ReadPin(DIP3_GPIO_Port, DIP3_Pin) == DIP_IS_ON)
		{
			// Modus 3 (OVP only. Active level - HIGH)
			LOG_I(TAG, "Mode 3 is selected. Auto-reconnect is ON.");

			AUTO_Mode = &AUTO_OVP_Management;
			// Assigning default functions to function pointers array, depending on OVP state
			LVP_OVP[0] = &OVP_not_present__LVP_ignored;		//  0
			LVP_OVP[1] = &OVP_present__LVP_ignored;			//  1
			LVP_OVP[2] = LVP_OVP[3] = &DoNothing;			// Not used
			//sys_data.s.lvp_state = 2;						// Ignored state
			LVP_OVP_logic = LOGIC_POSITIV;
			sys_data.s.dip_mode = 3 | (1 << 3);
		}
		else
		{
			// Modus 3 (OVP only. Active level - HIGH)
			LOG_I(TAG, "Mode 3 is selected. Auto-reconnect is OFF.");

			AUTO_Mode = &OVP_Management_NoAutoreconnect;
			// Assigning default functions to function pointers array, depending on OVP state
			LVP_OVP[0] = &OVP_not_present__LVP_ignored;				//  0
			LVP_OVP[1] = &OVP_present__LVP_ignored_NoAutoreconnect;	//  1
			LVP_OVP[2] = LVP_OVP[3] = &DoNothing;					// Not used
			//sys_data.s.lvp_state = 2;								// Ignored state
			LVP_OVP_logic = LOGIC_POSITIV;
			sys_data.s.dip_mode = 3;
		}
	}
	else if ((HAL_GPIO_ReadPin(DIP0_GPIO_Port, DIP0_Pin) == DIP_IS_OFF) &&
			 (HAL_GPIO_ReadPin(DIP1_GPIO_Port, DIP1_Pin) == DIP_IS_OFF) &&
			 (HAL_GPIO_ReadPin(DIP2_GPIO_Port, DIP2_Pin) == DIP_IS_ON))
	{
		// Auto-reconnect DIP-switch
		if (HAL_GPIO_ReadPin(DIP3_GPIO_Port, DIP3_Pin) == DIP_IS_ON)
		{
			// Modus 4 (LVP & OVP. Active level - LOW. With autoreconnect)
			LOG_I(TAG, "Mode 4 is selected. Auto-reconnect is ON.");

			AUTO_Mode = &AUTO_LVP_OVP_Management;
			// Assigning default functions to function pointers array, depending on LVP and OVP events combinations
			LVP_OVP[0] = &OVP_not_present__LVP_not_present; // 00 - 0
			LVP_OVP[1] = &OVP_not_present__LVP_present;		// 01 - 1
			LVP_OVP[2] = &OVP_present__LVP_not_present;		// 10 - 2
			LVP_OVP[3] = &OVP_present__LVP_present;			// 11 - 3
			LVP_OVP_logic = LOGIC_NEGATIV;
			sys_data.s.dip_mode = 4 | (1 << 3);
		}
		else
		{
			// Modus 4 (LVP & OVP. Active level - LOW. Without autoreconnect)
			LOG_I(TAG, "Mode 4 is selected. Auto-reconnect is OFF.");

			AUTO_Mode = &LVP_OVP_Management_NoAutoreconnect;
			// Assigning default functions to function pointers array, depending on LVP and OVP events combinations
			LVP_OVP[0] = &OVP_not_present__LVP_not_present;	// 00 - 0
			LVP_OVP[1] = &OVP_not_present__LVP_present;		// 01 - 1
			LVP_OVP[2] = &OVP_present__LVP_not_present;		// 10 - 2
			LVP_OVP[3] = &OVP_present__LVP_present;			// 11 - 3
			LVP_OVP_logic = LOGIC_NEGATIV;
			sys_data.s.dip_mode = 4;
		}
	}
	else if ((HAL_GPIO_ReadPin(DIP0_GPIO_Port, DIP0_Pin) == DIP_IS_ON) &&
			 (HAL_GPIO_ReadPin(DIP1_GPIO_Port, DIP1_Pin) == DIP_IS_OFF) &&
			 (HAL_GPIO_ReadPin(DIP2_GPIO_Port, DIP2_Pin) == DIP_IS_ON))
	{
		// Auto-reconnect DIP-switch
		if (HAL_GPIO_ReadPin(DIP3_GPIO_Port, DIP3_Pin) == DIP_IS_ON)
		{
			// Modus 5 (LVP & OVP. Active level - HIGH. With autoreconnect)
			LOG_I(TAG, "Mode 5 is selected. Auto-reconnect is ON.");

			AUTO_Mode = &AUTO_LVP_OVP_Management;
			// Assigning default functions to function pointers array, depending on LVP and OVP events combinations
			LVP_OVP[0] = &OVP_not_present__LVP_not_present; // 00 - 0
			LVP_OVP[1] = &OVP_not_present__LVP_present;		// 01 - 1
			LVP_OVP[2] = &OVP_present__LVP_not_present;		// 10 - 2
			LVP_OVP[3] = &OVP_present__LVP_present;			// 11 - 3
			LVP_OVP_logic = LOGIC_POSITIV;
			sys_data.s.dip_mode = 5 | (1 << 3);
		}
		else
		{
			// Modus 5 (LVP & OVP. Active level - HIGH. Without autoreconnect)
			LOG_I(TAG, "Mode 5 is selected. Auto-reconnect is OFF.");

			AUTO_Mode = &LVP_OVP_Management_NoAutoreconnect;
			// Assigning default functions to function pointers array, depending on LVP and OVP events combinations
			LVP_OVP[0] = &OVP_not_present__LVP_not_present; // 00 - 0
			LVP_OVP[1] = &OVP_not_present__LVP_present;		// 01 - 1
			LVP_OVP[2] = &OVP_present__LVP_not_present;		// 10 - 2
			LVP_OVP[3] = &OVP_present__LVP_present;			// 11 - 3
			LVP_OVP_logic = LOGIC_POSITIV;
			sys_data.s.dip_mode = 5;
		}
	}
	else  // The rest of the combinations
	{
		// Auto-reconnect DIP-switch
		if (HAL_GPIO_ReadPin(DIP3_GPIO_Port, DIP3_Pin) == DIP_IS_ON)
		{
			// Modus 4 (LVP & OVP. Active level - LOW. With autoreconnect)
			LOG_I(TAG, "Illegal Mode is selected. Default Mode 4 is selected. Auto-reconnect is ON.");

			AUTO_Mode = &AUTO_LVP_OVP_Management;
			// Assigning default functions to function pointers array, depending on LVP and OVP events combinations
			LVP_OVP[0] = &OVP_not_present__LVP_not_present; // 00 - 0
			LVP_OVP[1] = &OVP_not_present__LVP_present;		// 01 - 1
			LVP_OVP[2] = &OVP_present__LVP_not_present;		// 10 - 2
			LVP_OVP[3] = &OVP_present__LVP_present;			// 11 - 3
			LVP_OVP_logic = LOGIC_NEGATIV;
			sys_data.s.dip_mode = 4 | (1 << 3);
		}
		else
		{
			// Modus 4 (LVP & OVP. Active level - LOW. Without autoreconnect)
			LOG_I(TAG, "Illegal Mode is selected. Default Mode 4 is selected. Auto-reconnect is OFF.");

			AUTO_Mode = &LVP_OVP_Management_NoAutoreconnect;
			// Assigning default functions to function pointers array, depending on LVP and OVP events combinations
			LVP_OVP[0] = &OVP_not_present__LVP_not_present;						// 00 - 0
			LVP_OVP[1] = &OVP_not_present__LVP_present;							// 01 - 1
			LVP_OVP[2] = &OVP_present__LVP_not_present;							// 10 - 2
			LVP_OVP[3] = &OVP_present__LVP_present;								// 11 - 3
			LVP_OVP_logic = LOGIC_NEGATIV;
			sys_data.s.dip_mode = 4;
		}
	}

	if (HAL_GPIO_ReadPin(DIP4_GPIO_Port, DIP4_Pin) == DIP_IS_ON)
	{
		manual_overdrive_is_enabled = 1;	// Manual overdrive
		sys_data.s.dip_mode |= (1 << 4);
	}
	else manual_overdrive_is_enabled = 0;

	if (HAL_GPIO_ReadPin(DIP5_GPIO_Port, DIP5_Pin) == DIP_IS_ON)
	{
		auto_recover_from_temp_shutdown_is_enabled = 1;
		sys_data.s.dip_mode |= (1 << 5);
	}
	else auto_recover_from_temp_shutdown_is_enabled = 0;


	// At the end of this function we can DeInit all these ports
	// to minimize power consumption

	HAL_GPIO_DeInit(DIP0_GPIO_Port, DIP0_Pin);
	HAL_GPIO_DeInit(DIP1_GPIO_Port, DIP1_Pin);
	HAL_GPIO_DeInit(DIP2_GPIO_Port, DIP2_Pin);
	HAL_GPIO_DeInit(DIP3_GPIO_Port, DIP3_Pin);
	HAL_GPIO_DeInit(DIP4_GPIO_Port, DIP4_Pin);
	HAL_GPIO_DeInit(DIP5_GPIO_Port, DIP5_Pin);
	HAL_GPIO_DeInit(DIP6_GPIO_Port, DIP6_Pin);
	HAL_GPIO_DeInit(DIP7_GPIO_Port, DIP7_Pin);
}

//------------------------------------------------------------------------------

void OVP_Management_NoAutoreconnect(uint32_t new_time, int reset)
{
	static void (*WhatToDo[LVP_OVP_EVENT_NUM>>1])(void);
	static const uint32_t newEventDelay[LVP_OVP_EVENT_NUM>>1] = {10, 3000};		// Delays in ms for LVP events
	static const uint32_t repeatEventDelay[LVP_OVP_EVENT_NUM>>1] = {1, 100};	// Delays in ms for repeating LVP events
	static uint32_t OVP_SCAN_PERIOD = 1;
	static uint32_t ovp_last_time_checked = 0;
	static int lastIdx = -1;													// Impossible index, to make initial assignment

	// This helps to reinitialize AUTO mode when exiting OFF mode with button or modbus
	if (reset) { lastIdx = -1; return; }

	if (new_time - ovp_last_time_checked > OVP_SCAN_PERIOD)
	{
		// Saving time
		ovp_last_time_checked = new_time;

		// Reading the state of LVP pin
		GPIO_PinState current_OVP_state = HAL_GPIO_ReadPin(OVP_IN_GPIO_Port, OVP_IN_Pin);
		if (LVP_OVP_logic == LOGIC_POSITIV) current_OVP_state = !current_OVP_state;

		// Creating index from LVP values: 0 and 1
		int Idx = current_OVP_state;

		// If new value of LVP sygnal differs from old one, then we must update our function pointer
		if (Idx > lastIdx)
		{
			WhatToDo[Idx] = LVP_OVP[Idx];
			// When state of LVP changes, we introduce a delay, "debouncing"
			OVP_SCAN_PERIOD = newEventDelay[Idx];
		}
		else
		{
			WhatToDo[Idx] = &DoNothing;
			// When previous state is the same like current, then we do nothing with slower rate
			OVP_SCAN_PERIOD = repeatEventDelay[Idx];
		}

		// Depending on the state of the LVP pin, calling specific function, periodically
		WhatToDo[Idx]();
		lastIdx = Idx;
	}
}

//------------------------------------------------------------------------------

void AUTO_OVP_Management(uint32_t new_time, int reset)
{
	static void (*WhatToDo[LVP_OVP_EVENT_NUM>>1])(void);
	static const uint32_t newEventDelay[LVP_OVP_EVENT_NUM>>1] = {10, 3000};		// Delays in ms for LVP events
	static const uint32_t repeatEventDelay[LVP_OVP_EVENT_NUM>>1] = {1, 100};	// Delays in ms for repeating LVP events
	static uint32_t OVP_SCAN_PERIOD = 1;
	static uint32_t ovp_last_time_checked = 0;
	static unsigned int lastIdx = 2;											// Impossible index, to make initial assignment

	// This helps to reinitialize AUTO mode when exiting OFF mode with button or modbus
	if (reset)
	{
		lastIdx = 2;
		return;
	}

	if (new_time - ovp_last_time_checked > OVP_SCAN_PERIOD)
	{
		// Saving time
		ovp_last_time_checked = new_time;

		// Reading the state of LVP pin
		GPIO_PinState current_OVP_state = HAL_GPIO_ReadPin(OVP_IN_GPIO_Port, OVP_IN_Pin);
		if (LVP_OVP_logic == LOGIC_POSITIV) current_OVP_state = !current_OVP_state;

		// Creating index from LVP values: 0 and 1
		unsigned int Idx = current_OVP_state;

		// If new value of LVP sygnal differs from old one, then we must update our function pointer
		if (Idx != lastIdx)
		{
			WhatToDo[Idx] = LVP_OVP[Idx];
			// When state of LVP changes, we introduce a delay, "debouncing"
			OVP_SCAN_PERIOD = newEventDelay[Idx];
		}
		else
		{
			WhatToDo[Idx] = &DoNothing;
			// When previous state is the same like current, then we do nothing with slower rate
			OVP_SCAN_PERIOD = repeatEventDelay[Idx];
		}

		// Depending on the state of the LVP pin, calling specific function, periodically
		WhatToDo[Idx]();
		lastIdx = Idx;
	}
}

//------------------------------------------------------------------------------

void LVP_Management_NoAutoreconnect(uint32_t new_time, int reset)
{
	static void (*WhatToDo[LVP_OVP_EVENT_NUM>>1])(void);
	static const uint32_t newEventDelay[LVP_OVP_EVENT_NUM>>1] = {10, 3000};		// Delays in ms for LVP events
	static const uint32_t repeatEventDelay[LVP_OVP_EVENT_NUM>>1] = {1, 100};	// Delays in ms for repeating LVP events
	static uint32_t LVP_SCAN_PERIOD = 1;
	static uint32_t lvp_last_time_checked = 0;
	static int lastIdx = -1;											// Impossible index, to make initial assignment

	// This helps to reinitialize AUTO mode when exiting OFF mode with button or modbus
	if (reset)
	{
		lastIdx = -1;
		return;
	}

	if (new_time - lvp_last_time_checked > LVP_SCAN_PERIOD)
	{
		// Saving time
		lvp_last_time_checked = new_time;

		// Reading the state of LVP pin
		GPIO_PinState current_LVP_state = HAL_GPIO_ReadPin(LVP_IN_GPIO_Port, LVP_IN_Pin);
		if (LVP_OVP_logic == LOGIC_POSITIV) current_LVP_state = !current_LVP_state;

		// Creating index from LVP values: 0 and 1
		int Idx = current_LVP_state;

		// If new value of LVP sygnal differs from old one, then we must update our function pointer
		if (Idx > lastIdx)
		{
			WhatToDo[Idx] = LVP_OVP[Idx];
			// When state of LVP changes, we introduce a delay, "debouncing"
			LVP_SCAN_PERIOD = newEventDelay[Idx];
		}
		else
		{
			WhatToDo[Idx] = &DoNothing;
			// When previous state is the same like current, then we do nothing with slower rate
			LVP_SCAN_PERIOD = repeatEventDelay[Idx];
		}

		// Depending on the state of the LVP pin, calling specific function, periodically
		WhatToDo[Idx]();
		lastIdx = Idx;
	}
}

//------------------------------------------------------------------------------

void AUTO_LVP_Management(uint32_t new_time, int reset)
{
	static void (*WhatToDo[LVP_OVP_EVENT_NUM>>1])(void);
	static const uint32_t newEventDelay[LVP_OVP_EVENT_NUM>>1] = {10, 3000};		// Delays in ms for LVP events
	static const uint32_t repeatEventDelay[LVP_OVP_EVENT_NUM>>1] = {1, 100};	// Delays in ms for repeating LVP events
	static uint32_t LVP_SCAN_PERIOD = 1;
	static uint32_t lvp_last_time_checked = 0;
	static unsigned int lastIdx = 2;											// Impossible index, to make initial assignment

	// This helps to reinitialize AUTO mode when exiting OFF mode with button or modbus
	if (reset)
	{
		lastIdx = 2;
		return;
	}

	if (new_time - lvp_last_time_checked > LVP_SCAN_PERIOD)
	{
		// Saving time
		lvp_last_time_checked = new_time;

		// Reading the state of LVP pin
		GPIO_PinState current_LVP_state = HAL_GPIO_ReadPin(LVP_IN_GPIO_Port, LVP_IN_Pin);
		if (LVP_OVP_logic == LOGIC_POSITIV) current_LVP_state = !current_LVP_state;

		// Creating index from LVP values: 0 and 1
		unsigned int Idx = current_LVP_state;

		// If new value of LVP sygnal differs from old one, then we must update our function pointer
		if (Idx != lastIdx)
		{
			WhatToDo[Idx] = LVP_OVP[Idx];
			// When state of LVP changes, we introduce a delay, "debouncing"
			LVP_SCAN_PERIOD = newEventDelay[Idx];
		}
		else
		{
			WhatToDo[Idx] = &DoNothing;
			// When previous state is the same like current, then we do nothing with slower rate
			LVP_SCAN_PERIOD = repeatEventDelay[Idx];
		}

		// Depending on the state of the LVP pin, calling specific function, periodically
		WhatToDo[Idx]();
		lastIdx = Idx;
	}
}

//------------------------------------------------------------------------------

void LVP_OVP_Management_NoAutoreconnect(uint32_t new_time, int reset)
{
	static void (*WhatToDo[LVP_OVP_EVENT_NUM])(void);
	static const uint32_t newEventDelay[LVP_OVP_EVENT_NUM] = {10, 3000, 3000, 3000};	// Delays in ms for new LVP&OVP combination events
	static const uint32_t repeatEventDelay[LVP_OVP_EVENT_NUM] = {1, 100, 100, 100};		// Delays in ms for repeating LVP&OVP combination events
	static uint32_t LVP_OVP_SCAN_PERIOD = 1;
	static uint32_t lvp_ovp_last_time_checked = 0;
	static int lastIdx = -1;													// Impossible index, to make initial assignment
	static int ovp_lvp_flag = 0;

	// This helps to reinitialize AUTO mode when exiting OFF mode with button or modbus
	if (reset)
	{
		lastIdx = -1;
		ovp_lvp_flag = 0;
		return;
	}

	if (new_time - lvp_ovp_last_time_checked > LVP_OVP_SCAN_PERIOD)
	{
		// Saving time
		lvp_ovp_last_time_checked = new_time;

		// Reading the states of OVP&LVP pins
		GPIO_PinState current_OVP_state = HAL_GPIO_ReadPin(OVP_IN_GPIO_Port, OVP_IN_Pin);
		GPIO_PinState current_LVP_state = HAL_GPIO_ReadPin(LVP_IN_GPIO_Port, LVP_IN_Pin);
		if (LVP_OVP_logic == LOGIC_POSITIV)
		{
			current_OVP_state = !current_OVP_state;
			current_LVP_state = !current_LVP_state;
		}

		// Creating index from OVP and LVP values combinations: 0, 1, 2 and 3
		int Idx = (current_OVP_state << 1) | current_LVP_state;

		// Checking previous combination of OVP&LVP
		if (Idx != lastIdx)
		{
			if (!ovp_lvp_flag)
			{
				if (Idx > 0) ovp_lvp_flag = 1;

				WhatToDo[Idx] = LVP_OVP[Idx];
				// When states of OVP&LVP changes, we introduce a delay, "debouncing"
				LVP_OVP_SCAN_PERIOD = newEventDelay[Idx];
			}
		}
		else
		{
			WhatToDo[Idx] = &DoNothing;
			// When previous state is the same like current, then we do nothing with slower rate
			LVP_OVP_SCAN_PERIOD = repeatEventDelay[Idx];
		}

		// Depending on the combination of the OVP and LVP states, calling specific function, periodically
		if (WhatToDo[Idx]) WhatToDo[Idx]();
		lastIdx = Idx;
	}
}

//------------------------------------------------------------------------------

void AUTO_LVP_OVP_Management(uint32_t new_time, int reset)
{
	static void (*WhatToDo[LVP_OVP_EVENT_NUM])(void);
	static const uint32_t newEventDelay[LVP_OVP_EVENT_NUM] = {10, 3000, 3000, 3000};	// Delays in ms for new LVP&OVP combination events
	static const uint32_t repeatEventDelay[LVP_OVP_EVENT_NUM] = {1, 100, 100, 100};		// Delays in ms for repeating LVP&OVP combination events
	static uint32_t LVP_OVP_SCAN_PERIOD = 1;
	static uint32_t lvp_ovp_last_time_checked = 0;
	static unsigned int lastIdx = 4;													// Impossible index, to make initial assignment

	// This helps to reinitialize AUTO mode when exiting OFF mode with button or modbus
	if (reset)
	{
		lastIdx = 4;
		return;
	}

	if (new_time - lvp_ovp_last_time_checked > LVP_OVP_SCAN_PERIOD)
	{
		// Saving time
		lvp_ovp_last_time_checked = new_time;

		// Reading the states of OVP&LVP pins
		GPIO_PinState current_OVP_state = HAL_GPIO_ReadPin(OVP_IN_GPIO_Port, OVP_IN_Pin);
		GPIO_PinState current_LVP_state = HAL_GPIO_ReadPin(LVP_IN_GPIO_Port, LVP_IN_Pin);
		if (LVP_OVP_logic == LOGIC_POSITIV)
		{
			current_OVP_state = !current_OVP_state;
			current_LVP_state = !current_LVP_state;
		}

		// Creating index from OVP and LVP values combinations: 0, 1, 2 and 3
		unsigned int Idx = (current_OVP_state << 1) | current_LVP_state;

		// If new combination of OVP&LVP sygnals differ from old one, then we must update our function pointer
		if (Idx != lastIdx)
		{
			WhatToDo[Idx] = LVP_OVP[Idx];
			// When states of OVP&LVP changes, we introduce a delay, "debouncing"
			LVP_OVP_SCAN_PERIOD = newEventDelay[Idx];
		}
		else
		{
			WhatToDo[Idx] = &DoNothing;
			// When previous state is the same like current, then we do nothing with slower rate
			LVP_OVP_SCAN_PERIOD = repeatEventDelay[Idx];
		}

		// Depending on the combination of the OVP and LVP states, calling specific function, periodically
		WhatToDo[Idx]();
		lastIdx = Idx;
	}
}

//------------------------------------------------------------------------------
static uint32_t last_time_started = 0;

void StartAutoMode(void)
{
	uint32_t current_time = HAL_GetTick();

	// Checking whether we are already in this mode
	if ((sys_data.s.user_button_mode != SWITCH_AUTO) && (sys_data.s.user_button_mode != SWITCH_ON))
	{
		// We should not allow to start this mode very often
		if (current_time - last_time_started > 1000)
		{
			if ((current_time - overload_shutdown_time > 10000) && (current_time - overcurrent_shutdown_time > 10000))
			{
    			HAL_TIM_Base_Stop_IT(&ON_MODE_AUTO_OFF_TIMER);
    			last_time_started = current_time;

    			// Turning VBOOST voltage generator on
    			//HAL_GPIO_WritePin(DISABLE_VBOOST_GPIO_Port, DISABLE_VBOOST_Pin, GPIO_PIN_SET);

    			// After 250ms of VBOOST stabilization time, we turn on MOSFETs regulation
    			//HAL_TIM_Base_Stop_IT(&VBOOST_ON_TIMER);
    			__HAL_TIM_CLEAR_FLAG(&VBOOST_ON_TIMER, TIM_FLAG_UPDATE);
    			__HAL_TIM_SET_COUNTER(&VBOOST_ON_TIMER, 0);
    			HAL_TIM_Base_Start_IT(&VBOOST_ON_TIMER);

				//CLEAR_BIT(&hadc1->State, HAL_ADC_STATE_AWD2);
				//CLEAR_BIT(&hadc1->State, HAL_ADC_STATE_AWD3);

#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
				//__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_AWD2);
				//__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_AWD3);
				//NVIC_ClearPendingIRQ(ADC1_COMP_IRQn);
				HAL_NVIC_EnableIRQ(ADC1_COMP_IRQn);
#endif

    			sys_data.s.switch_cnt++;
    			statDataChanged = 1;
    			//HAL_GPIO_WritePin(FET_PULLDOWN_A_GPIO_Port, FET_PULLDOWN_A_Pin, GPIO_PIN_RESET);
    			//HAL_GPIO_WritePin(FET_PULLDOWN_B_GPIO_Port, FET_PULLDOWN_B_Pin, GPIO_PIN_RESET);
			}
		}
	}
}

//------------------------------------------------------------------------------

void StartOffMode(int reset)
{
	uint32_t current_time = HAL_GetTick();

	// Checking whether we are allowed to enter this mode
	//if ((sys_data.s.user_button_mode == SWITCH_ON) || (sys_data.s.user_button_mode == SWITCH_AUTO) || reset)
	//{
		if ((current_time - last_time_started > 500) || reset)
		{
			HAL_TIM_Base_Stop_IT(&ON_MODE_AUTO_OFF_TIMER);
			if (sys_data.s.user_button_mode != SWITCH_OFF) last_time_started = current_time;

			sys_data.s.user_button_mode = SWITCH_OFF;

#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
			DisableShortCutDetection();
#endif
			HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
			MOSFETS_Management = &ADC_Open_Both_MOSFETs;
			sys_data.s.relay_status = RELAY_IS_OPENED;
			HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);

			ExternalGreenLED_Management = &TurnExternalGreenLEDOff;
			ExternalRedLED_Management = &TurnExternalRedLEDOff;
			InternalGreenLED_Management = &TurnGreenLEDOff;

			// After 100ms we turn off VBOOST voltage for power saving
			//HAL_TIM_Base_Stop_IT(&VBOOST_OFF_TIMER);
			__HAL_TIM_CLEAR_FLAG(&VBOOST_OFF_TIMER, TIM_FLAG_UPDATE);
			__HAL_TIM_SET_COUNTER(&VBOOST_OFF_TIMER, 0);
			HAL_TIM_Base_Start_IT(&VBOOST_OFF_TIMER);

#ifndef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
			__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_AWD2);
			__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_AWD3);
			NVIC_ClearPendingIRQ(ADC1_COMP_IRQn);
			HAL_NVIC_EnableIRQ(ADC1_COMP_IRQn);
#endif
			
			overcurrent_shutdown_is_active = 0;
			overload_shutdown_is_active = 0;
			temperature_shutdown_is_active = 0;
			mosfets_voltagedrop_shutdown_is_active = 0;

			sys_data.s.last_shortcut_during_charge = 0;
			sys_data.s.last_shortcut_during_discharge = 0;
		}
	//}
}

//------------------------------------------------------------------------------

void StartOnMode(void)
{
	//static uint32_t last_time_started = 0;

	//uint32_t current_time = HAL_GetTick();

	// Checking whether we are already in this mode
	if ((sys_data.s.user_button_mode != SWITCH_ON) && (sys_data.s.user_button_mode != SWITCH_AUTO))
	{
		if (manual_overdrive_is_enabled)
		{
			// We should not allow to start this mode very often
			//if (current_time - last_time_started > 3000)
			//{
				//last_time_started = current_time;
				last_time_started = HAL_GetTick();

				//HAL_GPIO_WritePin(DISABLE_VBOOST_GPIO_Port, DISABLE_VBOOST_Pin, VBOOST_ENABLE);

				// After 250ms of VBOOST stabilization time, we turn on MOSFETs On mode
				__HAL_TIM_CLEAR_FLAG(&htim6, TIM_FLAG_UPDATE);
				__HAL_TIM_SET_COUNTER(&htim6, 0);
				HAL_TIM_Base_Start_IT(&htim6);

				// To prevent some unintentional damage to cells we turn this mode off after 1 minute
				__HAL_TIM_CLEAR_FLAG(&htim16, TIM_FLAG_UPDATE);
				__HAL_TIM_SET_COUNTER(&htim16, 0);
				HAL_TIM_Base_Start_IT(&htim16);

				sys_data.s.switch_cnt++;
				statDataChanged = 1;
				//HAL_GPIO_WritePin(FET_PULLDOWN_A_GPIO_Port, FET_PULLDOWN_A_Pin, GPIO_PIN_RESET);
				//HAL_GPIO_WritePin(FET_PULLDOWN_B_GPIO_Port, FET_PULLDOWN_B_Pin, GPIO_PIN_RESET);

				//sys_data.s.user_button_mode = SWITCH_ON;

				// We disable shortcut detecting interrupt
				//HAL_NVIC_DisableIRQ(ADC1_COMP_IRQn);
			//}
		}
	}
}

//------------------------------------------------------------------------------

void Keys_Management(void)
{
	static uint32_t last_time_checked = 0;

	uint32_t current_time = HAL_GetTick();

	if (current_time - last_time_checked >= 1)
	{
		last_time_checked = current_time;
		checkKeys();

		if (get_key_short(SW_ON_Pin))
		{
			LOG_I(TAG, "UP button is pressed.");
			StartAutoMode();
		}
		else if (get_key_long(SW_ON_Pin))
		{
			LOG_I(TAG, "UP button is long-pressed.");
			StartOnMode();
		}
		else if (get_key_short(SW_OFF_Pin))
		{
			LOG_I(TAG, "DOWN button is pressed.");
			StartOffMode(0);
		}
	}
}

//------------------------------------------------------------------------------

static inline __attribute__((always_inline)) void CalculatingSwitchSideVoltage(void)
{
	static int32_t ubsensea_voltage_accum = 0;

	// Calculatiing and averaging switch side voltage
	int32_t temp = ADC_values[U_SW_CHANNEL];									// Converting ADC value into int32_t type
	temp = (temp*ADC_VREF)>>ADC_RESOLUTION;										// Getting voltage value on ADC input [0:3000]mV
#ifdef VARIANT_24V
	temp = (temp*(R19 + R20 + R27 + R26))/R26;
#else
	temp = (temp*(R25 + R24))/R24;												// Getting voltage value on contact A [0:17100]mV
#endif
	ubsensea_voltage_accum -= sys_data.s.ubsensea_voltage;
	if (ubsensea_voltage_accum < 0) ubsensea_voltage_accum = 0;
	ubsensea_voltage_accum += temp;
	sys_data.s.ubsensea_voltage = ubsensea_voltage_accum / 8;
}

//------------------------------------------------------------------------------

inline __attribute__((always_inline)) void CalculatingMinMaxVoltagesForContactA(void)
{
	// Calculating MIN & MAX voltage values for contact A
	if (sys_data.s.ubsensea_voltage > sys_data.s.max_ubsensea_voltage) sys_data.s.max_ubsensea_voltage = sys_data.s.ubsensea_voltage;
	else if (sys_data.s.ubsensea_voltage < sys_data.s.min_ubsensea_voltage) sys_data.s.min_ubsensea_voltage = sys_data.s.ubsensea_voltage;
}

//------------------------------------------------------------------------------

static inline __attribute__((always_inline)) void CalculatingAndAveragingVoltageOnContactB(void)
{
	const uint32_t AVG_VALUE = 64U;
	static int32_t ubsenseb_voltage_accum = U_BAT_RECOVERY_VOLTAGE_mV * AVG_VALUE;		// << ubsenseb_voltage_accum_avg;

	// Special case for Battery undervoltage condition. During normal operation ubsenseb_voltage will never be zero
	//if (sys_data.s.ubsenseb_voltage == 0) ubsenseb_voltage_accum = 0;

	// Calculating and averaging battery side voltage
	int32_t temp = ADC_values[U_BAT_CHANNEL];									
	temp = (temp*ADC_VREF)>>ADC_RESOLUTION;										// Getting voltage value on ADC input [0:3000]mV
#ifdef VARIANT_24V
	temp = (temp*(R23 + R24 + R25 + R33))/R33;									// Getting voltage value on contact B []mV
#else
	temp = (temp*(R23 + R22))/R22;												// Getting voltage value on contact B [0:17100]mV
#endif
	ubsenseb_voltage_accum -= sys_data.s.ubsenseb_voltage;
	if (ubsenseb_voltage_accum < 0) ubsenseb_voltage_accum = 0;
	ubsenseb_voltage_accum += temp;
	sys_data.s.ubsenseb_voltage = ubsenseb_voltage_accum / AVG_VALUE;			//>> ubsenseb_voltage_accum_avg;
}

//------------------------------------------------------------------------------

static inline __attribute__((always_inline)) void TestingForLowVoltageShutdown(void)
{
	const uint32_t REACTIVATION_DELAY_ms = 10000;
	static uint32_t low_bat_event_last_time; 

	// If voltage on contact B is lower than critical one, then we must initiate "low voltage shutdown"
	if (sys_data.s.ubsenseb_voltage < U_BAT_CRITICAL_VOLTAGE_mV)
	{
		if (low_bat_shutdown_is_active == 0)
		{
#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
			DisableShortCutDetection();
#endif
			HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
			MOSFETS_Management = &DoNothing;
			OpenBothMOSFETSVeryFast();
			sys_data.s.relay_status = RELAY_IS_OPENED;
			HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);
			ExternalRedLED_Management = &ExternalRedLED5ShortOnThenLongPauseBlinking;

			low_bat_shutdown_is_active = 1;
			sys_data.s.device_status |= (1 << LOWBAT_ERROR);
			sys_data.s.lowbat_error_cnt++;
			statDataChanged = 1;

			low_bat_event_last_time = HAL_GetTick();
		}
	}
	else if (sys_data.s.ubsenseb_voltage > U_BAT_RECOVERY_VOLTAGE_mV)
	{
		if (low_bat_event_last_time + REACTIVATION_DELAY_ms < HAL_GetTick())
		{
			low_bat_shutdown_is_active = 0;
			sys_data.s.device_status &= ~(1 << LOWBAT_ERROR);
		}
	}

	/*if (low_bat_shutdown_is_active == 1)
	{
		
	}

	if (sys_data.s.ubsenseb_voltage > U_BAT_RECOVERY_VOLTAGE_mV)
	{
		low_bat_shutdown_is_active = 0;
		sys_data.s.device_status &= ~(1 << LOWBAT_ERROR);
	}*/
}

//------------------------------------------------------------------------------

inline __attribute__((always_inline)) void CalculatingMinMaxVoltagesForContactB(void)
{
	// Calculating MIN & MAX voltage values for contact B
	if (sys_data.s.ubsenseb_voltage > sys_data.s.max_ubsenseb_voltage) sys_data.s.max_ubsenseb_voltage = sys_data.s.ubsenseb_voltage;
	else if (sys_data.s.ubsenseb_voltage < sys_data.s.min_ubsenseb_voltage) sys_data.s.min_ubsenseb_voltage = sys_data.s.ubsenseb_voltage;
}

//------------------------------------------------------------------------------

static inline __attribute__((always_inline)) void SettingNewValuesForShortcutDetection(int isEnabled)
{
	// Setting limits for analogue watchdog to catch overcurrent events
	static int16_t last_shortcut_current_in_mV = 0;

	if (last_shortcut_current_in_mV != sys_data.s.shortcut_current_in_mV)
	{
		HAL_ADC_Stop_DMA(&hadc1);

		// Compensating possible skin-effect
		// Shortcut current lower then 400A does not make much sense
		if (sys_data.s.shortcut_current_in_mV < sys_data.s.copper_v_drop) sys_data.s.shortcut_current_in_mV = sys_data.s.copper_v_drop;
		if (sys_data.s.shortcut_current_in_mV > ADC_VREF) sys_data.s.shortcut_current_in_mV = ADC_VREF;

		//int32_t maxShortCutCurrent = (CONTROL_CURRENT_A * ADC_MAX_VALUE) / sys_data.s.copper_v_drop_adc;
		//if (compensatedShortcut_current > maxShortCutCurrent) compensatedShortcut_current = maxShortCutCurrent;

		last_shortcut_current_in_mV = sys_data.s.shortcut_current_in_mV;
		// Refreshing shortcut detecting value for Analog Watchdog
		ADC_AnalogWDGConfTypeDef AnalogWDGConfig = {0};

		AnalogWDGConfig.WatchdogNumber = ADC_ANALOGWATCHDOG_2;
		AnalogWDGConfig.WatchdogMode = ADC_ANALOGWATCHDOG_SINGLE_REG;
		AnalogWDGConfig.Channel = ADC_CHANNEL_2;  // I+ current sensor
		AnalogWDGConfig.ITMode = (isEnabled)? ENABLE: DISABLE;
		AnalogWDGConfig.HighThreshold = (sys_data.s.shortcut_current_in_mV * ADC_MAX_VALUE)/ADC_VREF;
		AnalogWDGConfig.LowThreshold = 0x000;
		if (HAL_ADC_AnalogWDGConfig(&hadc1, &AnalogWDGConfig) != HAL_OK) Error_Handler();

		AnalogWDGConfig.WatchdogNumber = ADC_ANALOGWATCHDOG_3;
		AnalogWDGConfig.WatchdogMode = ADC_ANALOGWATCHDOG_SINGLE_REG;
		AnalogWDGConfig.Channel = ADC_CHANNEL_6;  // I- current sensor
		AnalogWDGConfig.ITMode = (isEnabled)? ENABLE: DISABLE;;
		AnalogWDGConfig.HighThreshold = (sys_data.s.shortcut_current_in_mV * ADC_MAX_VALUE)/ADC_VREF;
		//sys_data.s.reserved = AnalogWDGConfig.HighThreshold;
		AnalogWDGConfig.LowThreshold = 0x000;
		if (HAL_ADC_AnalogWDGConfig(&hadc1, &AnalogWDGConfig) != HAL_OK) Error_Handler();

		if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)ADC_values, ADC_CHANNELS) != HAL_OK) LOG_E(TAG, "Cannot start ADC in DMA mode!");
		DMA1_Channel1->CCR &= ~DMA_CCR_HTIE;	// Disabling Half-Transfer interrupt, because we don't need it
	}
}

//------------------------------------------------------------------------------

#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF

inline __attribute__((always_inline)) void DisableShortCutDetection(void)
{
	__HAL_ADC_DISABLE_IT(&hadc1, ADC_IT_AWD2);
	__HAL_ADC_DISABLE_IT(&hadc1, ADC_IT_AWD3);
	__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_AWD2);
	__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_AWD3);

	// Switch-off process lasts approximately 1ms
	// We start this timer and in interrupt reactivate shortcut detection
	HAL_TIM_Base_Stop_IT(&htim15);
	__HAL_TIM_SetCounter(&htim15, 0);
	__HAL_TIM_CLEAR_FLAG(&htim15, TIM_IT_UPDATE);
	HAL_TIM_Base_Start_IT(&htim15);

	//HAL_GPIO_WritePin(TP2_GPIO_Port, TP2_Pin, GPIO_PIN_SET);
}

//------------------------------------------------------------------------------

inline __attribute__((always_inline)) void EnableShortCutDetection(void)
{
	__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_AWD2);
	__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_AWD3);
	__HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_AWD2);
	__HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_AWD3);

	//HAL_GPIO_WritePin(TP2_GPIO_Port, TP2_Pin, GPIO_PIN_RESET);
}

#endif

//------------------------------------------------------------------------------

static inline __attribute__((always_inline)) void BatteryLowVoltageProtection(/*uint32_t current_time*/)
{
	//static uint32_t last_time = 0;

	// During first start, somtimes we read 0s from ADC, so we need to wait for a while before calculating Min & Max values, especially Min values
	//if (current_time - last_time >= 0U)
	{
		//last_time = current_time;

		CalculatingAndAveragingVoltageOnContactB();

		TestingForLowVoltageShutdown();
	}
}

//------------------------------------------------------------------------------

void HeavyCalculations(uint32_t current_time)
{
	static uint32_t heavy_calc_last_time = 0;
	static uint32_t HEAVY_CALCULATIONS_PERIOD = 3000;
    static int32_t temperature_accum = 0;
	int current_temperature;
	//static int32_t prev_ursense_voltage = 0;
	//int const ubsenseb_voltage_accum_avg = 5;
	//int const ubsensea_voltage_accum_avg = 4;

	static int64_t ubbsense_voltage_accum = 0;


	// During first start, somtimes we read 0s from ADC, so we need to wait for a while before calculating Min & Max values, especially Min values
	if (current_time - heavy_calc_last_time > HEAVY_CALCULATIONS_PERIOD)
	{
		heavy_calc_last_time = current_time;
		HEAVY_CALCULATIONS_PERIOD = 135;

		CalculatingSwitchSideVoltage();

		CalculatingMinMaxVoltagesForContactA();

		//CalculatingAndAveragingVoltageOnContactB();

		//TestingForLowVoltageShutdown();

		CalculatingMinMaxVoltagesForContactB();

		// Sliding average calculation for board temperature
		current_temperature = (((MAX_TEMP - MIN_TEMP)*((int32_t)ADC_values[TEMP_CHANNEL] - TEMP_SENSOR_ADC_AT_MINUS30))/(TEMP_SENSOR_ADC_AT_PLUS100 - TEMP_SENSOR_ADC_AT_MINUS30)) + MIN_TEMP;
		temperature_accum -= sys_data.s.temperature;
		temperature_accum += current_temperature;
		sys_data.s.temperature = temperature_accum / 32;//>> 5;
		//sys_data.s.temperature = current_temperature;

		// Calculating MIN & MAX temperature values
		if (sys_data.s.temperature > sys_data.s.max_temperature) sys_data.s.max_temperature = sys_data.s.temperature;
		else if (sys_data.s.temperature < sys_data.s.min_temperature) sys_data.s.min_temperature = sys_data.s.temperature;

		// If temperature is above critical then we initiate temperature shutdown
		if (sys_data.s.temperature >= sys_data.s.temperature_shutdown)
		{
			if (temperature_shutdown_is_active == 0)
			{
				temperature_shutdown_is_active = 1;
				sys_data.s.device_status |= (1 << OVERTEMP_ERROR);
				sys_data.s.overtemp_error_cnt++;
				statDataChanged = 1;
			}
		}
		else if (sys_data.s.temperature < (((100 - TEMP_RECOVER_PERCENT)*sys_data.s.temperature_shutdown)/100))
		{
			if (auto_recover_from_temp_shutdown_is_enabled)
			{
				temperature_shutdown_is_active = 0;
				sys_data.s.device_status &= ~(1 << OVERTEMP_ERROR);
			}
		}

		if (overcurrent_shutdown_is_active == 1) sys_data.s.device_status |= (1 << OVERCURRENT_ERROR);
		else sys_data.s.device_status &= ~(1 << OVERCURRENT_ERROR);

		if (overload_shutdown_is_active == 1) sys_data.s.device_status |= (1 << OVERLOAD_ERROR);
		else sys_data.s.device_status &= ~(1 << OVERLOAD_ERROR);

		if (mosfets_voltagedrop_shutdown_is_active == 1) sys_data.s.device_status |= (1 << ABBA_VOLTAGE_ERROR);
		else sys_data.s.device_status &= ~(1 << ABBA_VOLTAGE_ERROR);

		// Calculating MIN & MAX voltage drop
		if (sys_data.s.ursense_voltage < 0)
		{
			// Discharge
			if (-sys_data.s.ursense_voltage > sys_data.s.max_discharge_ursense_voltage) sys_data.s.max_discharge_ursense_voltage = -sys_data.s.ursense_voltage;
			else if (-sys_data.s.ursense_voltage < sys_data.s.min_discharge_ursense_voltage) sys_data.s.min_discharge_ursense_voltage = -sys_data.s.ursense_voltage;
		}
		else
		{
			// Charge
			if (sys_data.s.ursense_voltage > sys_data.s.max_charge_ursense_voltage) sys_data.s.max_charge_ursense_voltage = sys_data.s.ursense_voltage;
			else if (sys_data.s.ursense_voltage < sys_data.s.min_charge_ursense_voltage) sys_data.s.min_charge_ursense_voltage = sys_data.s.ursense_voltage;
		}

		// Saving raw ADC value for voltage drop in special register
		sys_data.s.adc_plus_current_sensor = rawContactVoltageDropPlus;
		sys_data.s.adc_minus_current_sensor = rawContactVoltageDropMinus;

		// Calculating averaged voltage drop on COPPER-plate on contact B
		static int32_t rawContactVoltageDropPlus_accum = 0;
		static int32_t rawContactVoltageDropMinus_accum = 0;
		static int32_t tmp_i_plus = 0;
		static int32_t tmp_i_minus = 0;
		const int32_t AVG = 16;
		rawContactVoltageDropPlus_accum -= tmp_i_plus;
		rawContactVoltageDropPlus_accum += rawContactVoltageDropPlus;
		tmp_i_plus = rawContactVoltageDropPlus_accum / AVG;
		rawContactVoltageDropMinus_accum -= tmp_i_minus;
		rawContactVoltageDropMinus_accum += rawContactVoltageDropMinus;
		tmp_i_minus = rawContactVoltageDropMinus_accum / AVG;
		uint32_t final_i_plus = (tmp_i_plus >= sys_data.s.i_plus_offset)? tmp_i_plus - sys_data.s.i_plus_offset: 0;
		uint32_t final_i_minus = (tmp_i_minus >= sys_data.s.i_minus_offset)? tmp_i_minus - sys_data.s.i_minus_offset: 0;
		int32_t rawContactVoltageDrop = final_i_plus - final_i_minus;
		sys_data.s.ubbsense_voltage = (rawContactVoltageDrop * ADC_VREF) / ADC_MAX_VALUE;
		sys_data.s.current = (rawContactVoltageDrop * CONTROL_CURRENT_A) / sys_data.s.copper_v_drop_adc;
		//SEGGER_RTT_printf(0, "Iadc+ = %u    Iadc- = %u    Iadc = %d    Ubb = %d mV    I = %d A\n", final_i_plus, final_i_minus, rawContactVoltageDrop, sys_data.s.ubbsense_voltage, sys_data.s.current);

		if ((sys_data.s.relay_status == RELAY_IS_OPENED) && (InternalGreenLED_Management != &TurnGreenLEDOff)) InternalGreenLED_Management = &TurnGreenLEDOff;
		else if ((sys_data.s.relay_status == RELAY_IS_CLOSED) && (InternalGreenLED_Management != &TurnGreenLEDOn)) InternalGreenLED_Management = &TurnGreenLEDOn;
		else if (InternalGreenLED_Management != &GreenLEDShortBlinking) InternalGreenLED_Management = &GreenLEDShortBlinking;

		GPIO_PinState current_OVP_state = HAL_GPIO_ReadPin(OVP_IN_GPIO_Port, OVP_IN_Pin);
		GPIO_PinState current_LVP_state = HAL_GPIO_ReadPin(LVP_IN_GPIO_Port, LVP_IN_Pin);
		if (LVP_OVP_logic == LOGIC_POSITIV)
		{
			//if (sys_data.s.lvp_state != 2)
			//{
				if (current_LVP_state == GPIO_PIN_SET) sys_data.s.lvp_state = 0;
				else sys_data.s.lvp_state = 1;
			//}
			//if (sys_data.s.ovp_state != 2)
			//{
				if (current_OVP_state == GPIO_PIN_SET) sys_data.s.ovp_state = 0;
				else sys_data.s.ovp_state = 1;
			//}
		}
		else
		{
			//if (sys_data.s.lvp_state != 2)
			//{
				if (current_LVP_state == GPIO_PIN_RESET) sys_data.s.lvp_state = 0;
				else sys_data.s.lvp_state = 1;
			//}
			//if (sys_data.s.ovp_state != 2)
			//{
				if (current_OVP_state == GPIO_PIN_RESET) sys_data.s.ovp_state = 0;
				else sys_data.s.ovp_state = 1;
			//}
		}
		//HAL_GPIO_TogglePin(OUT_CTRL_GPIO_Port, OUT_CTRL_Pin);
		// If voltage on contact A is greater than voltage on contact B, then we assume a charger presence
		// We allow a pre-heater to be turned on
		// when OVP sygnal is active - when battery is cold
		static int32_t heater_cnt = 0;
		if ((sys_data.s.relay_status == RELAY_IS_CLOSED) || (sys_data.s.relay_status == ONLY_BA_OPENED) || (sys_data.s.relay_status == ONLY_AB_OPENED))
		{
			if ((sys_data.s.ursense_voltage > BAT_CHARGE_SIGN_V))
			{
				HAL_GPIO_WritePin(OUT_CTRL_GPIO_Port, OUT_CTRL_Pin, OUT_CTRL_ACTIVATE);
				heater_cnt = 500;
				//if (sys_data.s.ovp_state == 1) HAL_GPIO_WritePin(OUT_CTRL_GPIO_Port, OUT_CTRL_Pin, GPIO_PIN_SET);
				//else HAL_GPIO_WritePin(OUT_CTRL_GPIO_Port, OUT_CTRL_Pin, GPIO_PIN_RESET);
			}
			else if (sys_data.s.ursense_voltage <= 0)
			{
				if (heater_cnt > 0) heater_cnt--;
				if (heater_cnt == 0) HAL_GPIO_WritePin(OUT_CTRL_GPIO_Port, OUT_CTRL_Pin, OUT_CTRL_DEACTIVATE);
			}
		}
		else HAL_GPIO_WritePin(OUT_CTRL_GPIO_Port, OUT_CTRL_Pin, OUT_CTRL_DEACTIVATE);

		// Updating Inrush-Current value (A)
		static uint16_t last_inrush_max_current_in_mV = 0;
		if ((last_inrush_max_current_in_mV != sys_data.s.inrush_max_current_in_mV) && (sys_data.s.inrush_max_current_in_mV > sys_data.s.copper_v_drop))
		{
			if (sys_data.s.inrush_max_current_in_mV >= ADC_VREF)  sys_data.s.inrush_max_current_in_mV = ADC_VREF;

			last_inrush_max_current_in_mV = sys_data.s.inrush_max_current_in_mV;
			sys_data.s.inrush_max_current_in_adc = (sys_data.s.inrush_max_current_in_mV * ADC_MAX_VALUE) / ADC_VREF;
			maxIntegral = sys_data.s.inrush_max_current_in_adc * sys_data.s.inrush_curr_integral_steps;
		}

		// Updating Inrush-Current period value (µs)
		static uint16_t last_inrush_curr_period = 0;
		if (last_inrush_curr_period != sys_data.s.inrush_curr_period)
		{
			if (sys_data.s.inrush_curr_period < 31) sys_data.s.inrush_curr_period = 31;

			last_inrush_curr_period = sys_data.s.inrush_curr_period;
			sys_data.s.inrush_curr_integral_steps = (CURRENT_INTEGRAL_FREQ * sys_data.s.inrush_curr_period) / (1000*1000);
			maxIntegral = sys_data.s.inrush_max_current_in_adc * sys_data.s.inrush_curr_integral_steps;
		}

		// Testing if there is a current on battery heater
		if (HAL_GPIO_ReadPin(OUT_CS_GPIO_Port, OUT_CS_Pin) == GPIO_PIN_SET)	sys_data.s.heater_status = 1;
		else sys_data.s.heater_status = 0;

		SettingNewValuesForShortcutDetection(1);
	}
}

//------------------------------------------------------------------------------

void DEBUG_print(uint32_t current_time)
{
	static uint32_t debug_print_old_time = 0;

	if (current_time - debug_print_old_time > 77) // 77
	{
		//SEGGER_RTT_printf(0, "%4d\n", rawVoltageDrop);
		debug_print_old_time = current_time;
		const char* separator_color = RTT_CTRL_TEXT_BLACK;
		const char* values_color = RTT_CTRL_TEXT_BRIGHT_GREEN;

		SEGGER_RTT_printf(0, "%s%s", values_color, TAG);
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);
		SEGGER_RTT_printf(0, "Vab: %4d mV", sys_data.s.ursense_voltage);
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);
		SEGGER_RTT_printf(0, "Vbb: %5d mV", sys_data.s.ubbsense_voltage);
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);
		SEGGER_RTT_printf(0, "I: %5d A", sys_data.s.current);
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);
		SEGGER_RTT_printf(0, "Va: %6d mV", sys_data.s.ubsensea_voltage);
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);
		SEGGER_RTT_printf(0, "Vb: %6d mV", sys_data.s.ubsenseb_voltage);
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);
		SEGGER_RTT_printf(0, "OVP: %1s", (sys_data.s.ovp_state == 0)? "N": RTT_CTRL_TEXT_RED"Y"RTT_CTRL_TEXT_BRIGHT_GREEN);
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);
		SEGGER_RTT_printf(0, "LVP: %1s", (sys_data.s.lvp_state == 0)? "N": RTT_CTRL_TEXT_RED"Y"RTT_CTRL_TEXT_BRIGHT_GREEN);
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);
		SEGGER_RTT_printf(0, "DAC_A: %4d", DAC_HANDLE.Instance->DAC_CH_A);
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);
		SEGGER_RTT_printf(0, "DAC_B: %4d", DAC_HANDLE.Instance->DAC_CH_B);
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);
		SEGGER_RTT_printf(0, "R: %s", (sys_data.s.relay_status == 0)? "OP": (sys_data.s.relay_status == 1)? "CL": (sys_data.s.relay_status == 2)? "BA": "AB");
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);
		SEGGER_RTT_printf(0, "T: %2d.%d \260C", sys_data.s.temperature/10, abs(sys_data.s.temperature - (sys_data.s.temperature/10)*10) );
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);
		if (sys_data.s.device_status & (1 << OVERTEMP_ERROR))
		{
			SEGGER_RTT_printf(0, "%s", RTT_CTRL_TEXT_BRIGHT_RED);
			SEGGER_RTT_printf(0, "%s", "OT");
		}
		else SEGGER_RTT_printf(0, "%s", "OT");
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);

		if (sys_data.s.device_status & (1 << OVERCURRENT_ERROR))
		{
			SEGGER_RTT_printf(0, "%s", RTT_CTRL_TEXT_BRIGHT_RED);
			SEGGER_RTT_printf(0, "%s", "OC");
		}
		else SEGGER_RTT_printf(0, "%s", "OC");
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);

		if (sys_data.s.device_status & (1 << OVERLOAD_ERROR))
		{
			SEGGER_RTT_printf(0, "%s", RTT_CTRL_TEXT_BRIGHT_RED);
			SEGGER_RTT_printf(0, "%s", "OL");
		}
		else SEGGER_RTT_printf(0, "%s", "OL");
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);

		if (sys_data.s.device_status & (1 << LOWBAT_ERROR))
		{
			SEGGER_RTT_printf(0, "%s", RTT_CTRL_TEXT_BRIGHT_RED);
			SEGGER_RTT_printf(0, "%s", "LB");
		}
		else SEGGER_RTT_printf(0, "%s", "LB");
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);

		if (sys_data.s.device_status & (1 << ABBA_VOLTAGE_ERROR))
		{
			SEGGER_RTT_printf(0, "%s", RTT_CTRL_TEXT_BRIGHT_RED);
			SEGGER_RTT_printf(0, "%s", "AB");
		}
		else SEGGER_RTT_printf(0, "%s", "AB");
		SEGGER_RTT_printf(0, "%s | %s", separator_color, values_color);

		if (HAL_GPIO_ReadPin(OUT_CTRL_GPIO_Port, OUT_CTRL_Pin) == OUT_CTRL_ACTIVATE)
		{
			SEGGER_RTT_printf(0, "%s", RTT_CTRL_TEXT_BRIGHT_CYAN);
			SEGGER_RTT_printf(0, "%s", "CHG - 1");
		}
		else
		{
			SEGGER_RTT_printf(0, "%s", "CHG - 0");
		}

		SEGGER_RTT_printf(0, "\n");
	}
}

//------------------------------------------------------------------------------

inline __attribute__((always_inline)) void MODBUS_Management(void)
{
	// Modbus Kommunikation
	if (mbGetFrameComplete(&modbusData))
	{
		if (mbSlaveCheckModbusRtuQuery(&modbusData) == RESPOND_TO_QUERY)
		{
			if (RS485ActiveMode)
			{
				mbSlaveProcessRtuQuery(&modbusData);
			}
		}
		else huart1.RxState = HAL_UART_STATE_BUSY_RX;
	}

	// This code prevents unauthorized write-access to registers
	// Prüfe KEY
	if (sys_data.s.lockKey == savedLockKey)
	{
	  sys_data.s.keyAccepted = 1;
	  keyAccepted = 1;
	}
	else
	{
	  sys_data.s.keyAccepted = 0;
	  keyAccepted = 0;
	}

	// Checking whether command parser is active or not
	if (command_parser_is_enabled)
	{
		// Checking command which came via Modbus
		if ((sys_data.s.command != 0) && (modbusData.current_query == MB_QUERY_NOTHING))
		{
			switch (sys_data.s.command)
			{
				case COMMAND_SAVE_CONFIG:
					// Saving new settings without SN in FLASH
					if (FEEPROM_storeConfig(&sys_data, 0)) LOG_E(TAG, "Cannot save data in FLASH memory!");
					// Fetching configuration from FLASH
					if (FEEPROM_readConfig(&sys_data)) LOG_E(TAG, "Cannot read configuration from FLASH memory!");
					//if (FEEPROM_ReadLogData(&sys_data)) LOG_E(TAG, "Cannot read statistcal data from FLASH memory!");
					break;

				case COMMAND_SAVE_LOCK_KEY:
					LOG_I(TAG, "SAVE LOCK-KEY COMMAND.");
					mb_save_lock_key();
					break;

				case COMMAND_RESTART_MODBUS:
					// Modbus Re-initialization
					if (sys_data.s.parity_mode == 'e') mbInit(&modbusData, sys_data.s.baudrate, MODBUS_UART_PARITY_EVEN, &huart1, accessModeTable, &keyAccepted);
					else if (sys_data.s.parity_mode == 'o') mbInit(&modbusData, sys_data.s.baudrate, MODBUS_UART_PARITY_ODD, &huart1, accessModeTable, &keyAccepted);
					else mbInit(&modbusData, sys_data.s.baudrate, MODBUS_UART_PARITY_NONE, &huart1, accessModeTable, &keyAccepted);
					break;

				case COMMAND_RESTORE_DEFAULTS:
					if (FEEPROM_fullRestore(/*&sys_data*/)) LOG_E(TAG, "Cannot restore default settings from FLASH memory!");
					FEEPROM_ResetLogData();
					// Fetching configuration from FLASH
					if (FEEPROM_readConfig(&sys_data)) LOG_E(TAG, "Cannot read configuration from FLASH memory!");
					if (FEEPROM_ReadLogData(&sys_data)) LOG_E(TAG, "Cannot read statistcal data from FLASH memory!");
					break;

				case COMMAND_ON_SWITCH: StartOnMode(); break;

				case COMMAND_OFF_SWITCH: StartOffMode(0); break;

				case COMMAND_AUTO_SWITCH: StartAutoMode(); break;

				case COMMAND_SAVE_CONFIG_WITH_SN:
					// Saving new settings with new SN in FLASH
					if (FEEPROM_storeConfig(&sys_data, 1)) LOG_E(TAG, "Cannot save new SN in FLASH memory!");
					// Fetching configuration from FLASH
					if (FEEPROM_readConfig(&sys_data)) LOG_E(TAG, "Cannot read configuration from FLASH memory!");
					//if (FEEPROM_ReadLogData(&sys_data)) LOG_E(TAG, "Cannot read statistcal data from FLASH memory!");
					break;

				case COMMAND_RESTART:
					OpenBothMOSFETSVeryFast();
					NVIC_SystemReset();
					break;

				case COMMAND_OFFSET_CALIBRATION:
					if (Callibration == DoNothing) Callibration = &CallibrateVoltageDropABMiddlePointOffset;
					break;

				case COMMAND_CURRENT_CALIBRATION:
					if (Callibration == DoNothing) Callibration = &CallibrateControlCurrentVoltageDropOnContactBB;
					break;

				case COMMAND_CURRENT_OFFSET_CALIBRATION:
					if (Callibration == DoNothing) Callibration = &CallibrateCurrentSensorZeroOffsetOnContactBB;
					break;

				case COMMAND_TURN_OVERLOAD_DETECTION_OFF:
					InrushCurrentManagement = &DoNothing;
					break;

				case COMMAND_TURN_OVERLOAD_DETECTION_ON:
					InrushCurrentManagement = &InrushCurrentDetected;
					break;

				default:
					// When we recieve some unknown command we freeze our command parser for 10 sec (to prevent bruteforce).
					__HAL_TIM_CLEAR_FLAG(&htim17, TIM_FLAG_UPDATE);
					__HAL_TIM_SET_COUNTER(&htim17, 0);
					if (HAL_TIM_Base_Start_IT(&htim17) != HAL_OK) LOG_E(TAG, "Cannot start TIM17 in ISR mode!");
					command_parser_is_enabled = 0;
					LOG_W(TAG, "Unknown command!");
			}
			sys_data.s.command = 0;
		}
	}
	else sys_data.s.command = 0;
}

//------------------------------------------------------------------------------

void OVP_not_present__LVP_ignored(void)
{
	HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
#ifdef INVERTER_CAP_PRECHARGE
	SetReturnFunction(&ADC_OVP_not_present__LVP_ignored);
	MOSFETS_Management = &PreChargeStage;
#else
	MOSFETS_Management = &ADC_OVP_not_present__LVP_ignored;
#endif
	sys_data.s.relay_status = RELAY_IS_CLOSED;
	HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);

	ExternalGreenLED_Management = &ExternalGreenLEDShortBlinking;
}

//------------------------------------------------------------------------------

void OVP_present__LVP_ignored(void)
{
#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
	DisableShortCutDetection();
#endif
	HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
	MOSFETS_Management = &ADC_OVP_present__LVP_ignored;
	sys_data.s.relay_status = RELAY_IS_OPENED;
	HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);

	ExternalGreenLED_Management = &ExternalGreenLEDShortBlinking;
	sys_data.s.ovp_cnt++;
	statDataChanged = 1;
}

//------------------------------------------------------------------------------

void OVP_present__LVP_ignored_NoAutoreconnect(void)
{
#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
	DisableShortCutDetection();
#endif
	HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
	MOSFETS_Management = &ADC_Open_Both_MOSFETs;
	sys_data.s.relay_status = RELAY_IS_OPENED;
	HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);

	sys_data.s.user_button_mode = SWITCH_OFF;

	ExternalGreenLED_Management = &TurnExternalGreenLEDOff;

	// After 100ms we turn off VBOOST voltage for power saving
	__HAL_TIM_CLEAR_FLAG(&VBOOST_OFF_TIMER, TIM_FLAG_UPDATE);
	__HAL_TIM_SET_COUNTER(&VBOOST_OFF_TIMER, 0);
	HAL_TIM_Base_Start_IT(&VBOOST_OFF_TIMER);

	sys_data.s.ovp_cnt++;
	statDataChanged = 1;
}

//------------------------------------------------------------------------------

void OVP_ignored__LVP_not_present(void)
{
	HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
#ifdef INVERTER_CAP_PRECHARGE
	SetReturnFunction(&ADC_OVP_ignored__LVP_not_present);
	MOSFETS_Management = &PreChargeStage;
#else
	MOSFETS_Management = &ADC_OVP_ignored__LVP_not_present;
#endif
	sys_data.s.relay_status = RELAY_IS_CLOSED;
	HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);

	ExternalGreenLED_Management = &ExternalGreenLEDShortBlinking;
	//ExternalRedLED_Management = &TurnExternalRedLEDOff;
}

//------------------------------------------------------------------------------

void OVP_ignored__LVP_present(void)
{
#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
	DisableShortCutDetection();
#endif
	HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
	MOSFETS_Management = &ADC_OVP_ignored__LVP_present;
	sys_data.s.relay_status = RELAY_IS_OPENED;
	HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);

	ExternalGreenLED_Management = &ExternalGreenLEDShortBlinking;
	//ExternalRedLED_Management = &ExternalRedLEDShortBlinking;

	sys_data.s.lvp_cnt++;
	statDataChanged = 1;
}

//------------------------------------------------------------------------------

void OVP_ignored__LVP_present_NoAutoreconnect(void)
{
#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
	DisableShortCutDetection();
#endif
	HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
	MOSFETS_Management = &ADC_Open_Both_MOSFETs;
	sys_data.s.relay_status = RELAY_IS_OPENED;
	HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);

	sys_data.s.user_button_mode = SWITCH_OFF;

	ExternalGreenLED_Management = &TurnExternalGreenLEDOff;

	// After 100ms we turn off VBOOST voltage for power saving
	__HAL_TIM_CLEAR_FLAG(&VBOOST_OFF_TIMER, TIM_FLAG_UPDATE);
	__HAL_TIM_SET_COUNTER(&VBOOST_OFF_TIMER, 0);
	HAL_TIM_Base_Start_IT(&VBOOST_OFF_TIMER);

	sys_data.s.lvp_cnt++;
	statDataChanged = 1;
}

//------------------------------------------------------------------------------

void OVP_not_present__LVP_not_present(void)
{
	HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
#ifdef INVERTER_CAP_PRECHARGE
	SetReturnFunction(&ADC_OVP_not_present__LVP_not_present);
	MOSFETS_Management = &PreChargeStage;
#else
	MOSFETS_Management = &ADC_OVP_not_present__LVP_not_present;
#endif
	HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);
	sys_data.s.relay_status = RELAY_IS_CLOSED;

	ExternalGreenLED_Management = &ExternalGreenLEDShortBlinking;
	TurnExternalRedLEDOff();
}

//------------------------------------------------------------------------------

void OVP_not_present__LVP_present(void)
{
//#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
//	DisableShortCutDetection();
//#endif
	HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
	MOSFETS_Management = &ADC_OVP_not_present__LVP_present;
	sys_data.s.relay_status = ONLY_AB_OPENED;
	HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);

	ExternalGreenLED_Management = &ExternalGreenLEDShortBlinking;
	TurnExternalRedLEDOff();

	sys_data.s.lvp_cnt++;
	statDataChanged = 1;
}

//------------------------------------------------------------------------------

void OVP_present__LVP_not_present(void)
{
	HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
	MOSFETS_Management = &ADC_OVP_present__LVP_not_present;
	sys_data.s.relay_status = ONLY_BA_OPENED;
	HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);

	ExternalGreenLED_Management = &ExternalGreenLEDShortBlinking;
	TurnExternalRedLEDOff();
	//GreenLED_Management =

	sys_data.s.ovp_cnt++;
	statDataChanged = 1;
}

//------------------------------------------------------------------------------

void OVP_present__LVP_present(void)
{
#ifdef DISABLE_SHORTCUT_DETECTION_DURING_SWITCH_OFF
	DisableShortCutDetection();
#endif
	HAL_NVIC_DisableIRQ(ADC_DMA_IRQ);
	MOSFETS_Management = &ADC_OVP_present__LVP_present;
	sys_data.s.relay_status = RELAY_IS_OPENED;
	HAL_NVIC_EnableIRQ(ADC_DMA_IRQ);

	ExternalGreenLED_Management = &TurnExternalGreenLEDOff;
	ExternalRedLED_Management = &ExternalRedLED2ShortOnThen2LongOnThenLongPauseBlinking;

	sys_data.s.lvp_cnt++;
	sys_data.s.ovp_cnt++;
	statDataChanged = 1;
}

//------------------------------------------------------------------------------

#ifdef USE_RAM_FUNC
__RAM_FUNC void ADC_OVP_present__LVP_ignored(void)
#else
void ADC_OVP_present__LVP_ignored(void)
#endif
{
	OpenBothMOSFETS();
}

//------------------------------------------------------------------------------
#ifdef USE_RAM_FUNC
__RAM_FUNC void ADC_OVP_ignored__LVP_present(void)
#else
void ADC_OVP_ignored__LVP_present(void)
#endif
{
	OpenBothMOSFETS();
}

//------------------------------------------------------------------------------

#ifdef USE_RAM_FUNC
__RAM_FUNC void ADC_Open_Both_MOSFETs(void)
#else
void ADC_Open_Both_MOSFETs(void)
#endif
{
	OpenBothMOSFETS();
}

//------------------------------------------------------------------------------

#ifdef USE_RAM_FUNC
__RAM_FUNC void ADC_OVP_present__LVP_present(void)
#else
void ADC_OVP_present__LVP_present(void)
#endif
{
	OpenBothMOSFETS();
}

//------------------------------------------------------------------------------

#ifdef USE_RAM_FUNC
__RAM_FUNC void ADC_OVP_not_present__LVP_present(void)
#else
void ADC_OVP_not_present__LVP_present(void)
#endif
{
    // Reading current DAC value for MOSFET B
    uint32_t dacA_value = DAC_HANDLE.Instance->DAC_CH_A;
	// Checking voltage drop on MOSFETs
    if ((rawMOSFETsVoltageDrop + sys_data.s.ab_middle_point_offset) < POS_ALLOWED_MOSFETS_V_DROP_ADC)
    {
		if (dacA_value >= (DAC_0V + DAC_STEP)) dacA_value -= DAC_STEP;
    }
    else if ((rawMOSFETsVoltageDrop + sys_data.s.ab_middle_point_offset) > POS_ALLOWED_MOSFETS_V_DROP_ADC)
    {
		if (dacA_value <= (DAC_3V - DAC_STEP)) dacA_value += DAC_STEP;
    }
    // Writing DAC value for MOSFET B
	*(__IO uint32_t *)((uint32_t)DAC_HANDLE.Instance + DAC_CH_A_ALIGNMENT(DAC_ALIGN_12B_R)) = dacA_value;

	// Reading DAC value for MOSFET channel A
	uint32_t dacB_value = DAC_HANDLE.Instance->DAC_CH_B;
	// We do not allow DAC value to be greater than max value
	if (dacB_value <= (DAC_3V - DAC_STEP)) dacB_value += DAC_STEP;
    // Sending new DAC value to DAC
	*(__IO uint32_t *)((uint32_t)DAC_HANDLE.Instance + DAC_CH_B_ALIGNMENT(DAC_ALIGN_12B_R)) = dacB_value;
}

//------------------------------------------------------------------------------

#ifdef USE_RAM_FUNC
__RAM_FUNC void ADC_OVP_present__LVP_not_present(void)
#else
void ADC_OVP_present__LVP_not_present(void)
#endif
{
    // Reading current DAC value for MOSFET B channel
    uint32_t dacB_value = DAC_HANDLE.Instance->DAC_CH_B;
	// Making sure that BA voltage drop during discharging is within the limit
    if ((rawMOSFETsVoltageDrop + sys_data.s.ab_middle_point_offset) < NEG_ALLOWED_MOSFETS_V_DROP_ADC)
    {
		if (dacB_value <= (DAC_3V - DAC_STEP)) dacB_value += DAC_STEP;
    }
    else if ((rawMOSFETsVoltageDrop + sys_data.s.ab_middle_point_offset) > NEG_ALLOWED_MOSFETS_V_DROP_ADC)
    {
		if (dacB_value >= (DAC_0V + DAC_STEP)) dacB_value -= DAC_STEP;
    }
    // Writing DAC value for MOSFET A channel
	*(__IO uint32_t *)((uint32_t)DAC_HANDLE.Instance + DAC_CH_B_ALIGNMENT(DAC_ALIGN_12B_R)) = dacB_value;

    // Reading DAC value for MOSFET channel A
	uint32_t dacA_value = DAC_HANDLE.Instance->DAC_CH_A;
	// Channel A must be fully closed, so we increase smoothly DAC value
	// We do not allow DAC value to be greater than max value
	if (dacA_value <= (DAC_3V - DAC_STEP)) dacA_value += DAC_STEP;
    // Sending new DAC value to DAC
	*(__IO uint32_t *)((uint32_t)DAC_HANDLE.Instance + DAC_CH_A_ALIGNMENT(DAC_ALIGN_12B_R)) = dacA_value;
}

//------------------------------------------------------------------------------

#ifdef USE_RAM_FUNC
__RAM_FUNC void ADC_OVP_not_present__LVP_ignored(void)
#else
void ADC_OVP_not_present__LVP_ignored(void)
#endif
{
	CloseBothMOSFETS();
}

//------------------------------------------------------------------------------

#ifdef USE_RAM_FUNC
__RAM_FUNC void ADC_OVP_ignored__LVP_not_present(void)
#else
void ADC_OVP_ignored__LVP_not_present(void)
#endif
{
	CloseBothMOSFETS();
}

//------------------------------------------------------------------------------

#ifdef USE_RAM_FUNC
__RAM_FUNC void ADC_Close_Both_MOSFETs(void)
#else
void ADC_Close_Both_MOSFETs(void)
#endif
{
	CloseBothMOSFETS();
}

//------------------------------------------------------------------------------

#ifdef USE_RAM_FUNC
__RAM_FUNC void ADC_OVP_not_present__LVP_not_present(void)
#else
void ADC_OVP_not_present__LVP_not_present(void)
#endif
{
	CloseBothMOSFETS();
}

//-----------------------------------------------------------------------------

void CallibrateCurrentSensorZeroOffsetOnContactBB(void)
{
	// Assuming that load is not connected

	uint32_t adc_value_accum_i_plus_channel = 0;
	uint32_t adc_value_accum_i_minus_channel = 0;
	const uint32_t SAMPLE_COUNT = 50000;

	// Sampling N values
	for (int i = 0; i < SAMPLE_COUNT; i++)
	{
		// Calculating offsets on both current sensors
		adc_value_accum_i_plus_channel += rawContactVoltageDropPlus;
		adc_value_accum_i_minus_channel += rawContactVoltageDropMinus;
		SEGGER_RTT_printf(0, "\t[%4d] Sampled values: I+ = %6u    I- = %6u\n", i, rawContactVoltageDropPlus, rawContactVoltageDropMinus);
		//SEGGER_RTT_printf(0, "%u,%u\n", ADC_values[I_PLUS_CHANNEL], ADC_values[I_MINUS_CHANNEL]);
	}

	sys_data.s.i_plus_offset = adc_value_accum_i_plus_channel / SAMPLE_COUNT;
	sys_data.s.i_minus_offset = adc_value_accum_i_minus_channel / SAMPLE_COUNT;
	SEGGER_RTT_printf(0, "\t\tOffset values: I+ = %u    I- = %u\n", sys_data.s.i_plus_offset, sys_data.s.i_minus_offset);

	Callibration = &DoNothing;
}

//------------------------------------------------------------------------------

int32_t Rescale(int32_t x, int32_t x1, int32_t x2, int32_t y1, int32_t y2)
{
	int32_t res = ((y2 - y1) * (x - x1)) / (x2 - x1) + y1;
	return res;
}

//------------------------------------------------------------------------------

int32_t TemperatureCompensation(int32_t rawADCValueUnderMaxCurrent, int32_t temp_dGrad)
{
	if      (temp_dGrad < 200)                      rawADCValueUnderMaxCurrent = (rawADCValueUnderMaxCurrent * (100 + 10)) / 100;									// +10%
	else if (temp_dGrad >= 200 && temp_dGrad < 600) rawADCValueUnderMaxCurrent = (rawADCValueUnderMaxCurrent * (100 + Rescale(temp_dGrad, 200, 600, 10, 0))) / 100;	// +[0%...10%]
	else if (temp_dGrad >= 600)                     rawADCValueUnderMaxCurrent =  rawADCValueUnderMaxCurrent;														// +0%

	return rawADCValueUnderMaxCurrent;
}

//-----------------------------------------------------------------------------

void CallibrateControlCurrentVoltageDropOnContactBB(void)
{
	// Assuming that 500A load is connected

	LOG_I(TAG, "Current callibration sequence started.");

	// Turning off inrush current protection
	//InrushCurrentManagement = &DoNothing;

	uint32_t start_time = HAL_GetTick();
	int32_t ubbsense_adc_accum = 0;
	int32_t ubbsense_adc = 0;
	int32_t ubbsense_voltage = 0;

	while (HAL_GetTick() - start_time < 60000)
	{
		ubbsense_adc_accum -= ubbsense_adc;
		ubbsense_adc_accum += rawContactVoltageDropMinus;
		ubbsense_adc = ubbsense_adc_accum / 16;

		SEGGER_RTT_printf(0, "\t\tVoltage-drop ADC value: %5d.\n", ubbsense_adc);
		HAL_Delay(1);
	}

	ubbsense_adc -= sys_data.s.i_minus_offset;
	ubbsense_adc = TemperatureCompensation(ubbsense_adc, sys_data.s.temperature);
	ubbsense_voltage = (ubbsense_adc * ADC_VREF) / ADC_MAX_VALUE;

	// Recording copper-plate voltage-drop under control current
	sys_data.s.copper_v_drop = ubbsense_voltage;
	// Recording ADC value drop under control current
	sys_data.s.copper_v_drop_adc = ubbsense_adc;
	sys_data.s.copper_v_drop_adc_limit = (sys_data.s.copper_v_drop_adc * 110) / 100;

	SEGGER_RTT_printf(0, "\t\t\tFinal voltage-drop ADC value: %4u. Final voltage-drop value: %3u mV\n", sys_data.s.copper_v_drop_adc, sys_data.s.copper_v_drop);

	// Updating inrush current settings
	//sys_data.s.inrush_max_current_in_adc = (sys_data.s.inrush_max_current * sys_data.s.copper_v_drop_adc) / CONTROL_CURRENT_A;
	//maxIntegral = sys_data.s.inrush_max_current_in_adc * sys_data.s.inrush_curr_integral_steps;

	Callibration = &DoNothing;
	//InrushCurrentManagement = &InrushCurrentDetected; // Test program disables this, so we must re-enable it after callibration
}

//-----------------------------------------------------------------------------

void CallibrateVoltageDropABMiddlePointOffset(void)
{
	// Assuming that load is not connected

	uint32_t adc_value_accum = 0;
	const uint32_t SAMPLE_COUNT = 50000;

	// Sampling N values
	for (int i = 0; i < SAMPLE_COUNT; i++)
	{
		// Calculating averaged value for real voltage drop between contacts B and A in ADC values
		adc_value_accum += rawMOSFETsVoltageDrop;
		SEGGER_RTT_printf(0, "\t[%4d] Sampled value: %4d\n", i, rawMOSFETsVoltageDrop);
	}

	sys_data.s.ab_middle_point_offset = (ADC_MAX_VALUE>>1) - (adc_value_accum / SAMPLE_COUNT);
	SEGGER_RTT_printf(0, "\t\tOffset value: %4d\n", sys_data.s.ab_middle_point_offset);

	Callibration = &DoNothing;
}

//------------------------------------------------------------------------------

void InrushCurrentDetected(void)
{
	OpenBothMOSFETSVeryFast();
	MOSFETS_Management = &DoNothing;

	if (overcurrent_shutdown_is_active == 0 )
	{
		if (overload_shutdown_is_active == 0)
		{
			sys_data.s.overload_error_cnt++;
			statDataChanged = 1;
		}

		overload_shutdown_is_active = 1;
		overload_shutdown_time = HAL_GetTick();
	}
}

//------------------------------------------------------------------------------

inline __attribute__((always_inline)) void OpenBothMOSFETS(void)
{	// Reading DAC values for MOSFET channels A and B
    uint32_t dacA_value = DAC_HANDLE.Instance->DAC_CH_A;
    uint32_t dacB_value = DAC_HANDLE.Instance->DAC_CH_B;

	// Opening MOSFETS
	if (dacA_value >= (DAC_0V + DAC_STEP)) dacA_value -= DAC_STEP;
	if (dacB_value >= (DAC_0V + DAC_STEP)) dacB_value -= DAC_STEP;

	// Setting new values
	*(__IO uint32_t *)((uint32_t)DAC_HANDLE.Instance + DAC_CH_A_ALIGNMENT(DAC_ALIGN_12B_R)) = dacA_value;
	*(__IO uint32_t *)((uint32_t)DAC_HANDLE.Instance + DAC_CH_B_ALIGNMENT(DAC_ALIGN_12B_R)) = dacB_value;
}

//------------------------------------------------------------------------------

inline __attribute__((always_inline)) void CloseBothMOSFETS(void)
{
    // Reading DAC values for MOSFET channels A and B
    uint32_t dacA_value = DAC_HANDLE.Instance->DAC_CH_A;
    uint32_t dacB_value = DAC_HANDLE.Instance->DAC_CH_B;

	// Closing MOSFETS
	if (dacA_value <= (DAC_3V - DAC_STEP)) dacA_value += DAC_STEP;
	if (dacB_value <= (DAC_3V - DAC_STEP)) dacB_value += DAC_STEP;

	// Setting new values
	*(__IO uint32_t *)((uint32_t)DAC_HANDLE.Instance + DAC_CH_A_ALIGNMENT(DAC_ALIGN_12B_R)) = dacA_value;
	*(__IO uint32_t *)((uint32_t)DAC_HANDLE.Instance + DAC_CH_B_ALIGNMENT(DAC_ALIGN_12B_R)) = dacB_value;
}

//------------------------------------------------------------------------------

void CloseBothMOSFETSVeryFast(void)
{
	*(__IO uint32_t *)((uint32_t)DAC_HANDLE.Instance + DAC_CH_A_ALIGNMENT(DAC_ALIGN_12B_R)) = DAC_3V;  // Expression from HAL_DAC_SetValue function
	*(__IO uint32_t *)((uint32_t)DAC_HANDLE.Instance + DAC_CH_B_ALIGNMENT(DAC_ALIGN_12B_R)) = DAC_3V;  // Expression from HAL_DAC_SetValue function
	sys_data.s.relay_status = RELAY_IS_CLOSED;
}

//------------------------------------------------------------------------------

inline __attribute__((always_inline)) void OpenBothMOSFETSVeryFast(void)
{
	*(__IO uint32_t *)((uint32_t)DAC_HANDLE.Instance + DAC_CH_A_ALIGNMENT(DAC_ALIGN_12B_R)) = DAC_0V;  // Expression from HAL_DAC_SetValue function
	*(__IO uint32_t *)((uint32_t)DAC_HANDLE.Instance + DAC_CH_B_ALIGNMENT(DAC_ALIGN_12B_R)) = DAC_0V;  // Expression from HAL_DAC_SetValue function
	sys_data.s.relay_status = RELAY_IS_OPENED;
}

//------------------------------------------------------------------------------

void ShowSlaveAddressOnLED(uint16_t address, GPIO_TypeDef *port, uint16_t pin)
{
	for (int i = 0; i < address; i++)
	{
		HAL_GPIO_WritePin(port, pin, GPIO_PIN_SET);
		HAL_Delay(333);
		HAL_GPIO_WritePin(port, pin, GPIO_PIN_RESET);
		HAL_Delay(333);
	}
}

//------------------------------------------------------------------------------

void StartUpSequence(void)
{
    HAL_GPIO_WritePin(LED_FUNCTION_GPIO_Port, LED_FUNCTION_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(LED_ERROR_GPIO_Port, LED_ERROR_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(LED_STATE_GPIO_Port, LED_STATE_Pin, GPIO_PIN_SET);

    HAL_Delay(1000);

    HAL_GPIO_WritePin(LED_FUNCTION_GPIO_Port, LED_FUNCTION_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(LED_ERROR_GPIO_Port, LED_ERROR_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(LED_STATE_GPIO_Port, LED_STATE_Pin, GPIO_PIN_RESET);

    HAL_Delay(500);
}

//-----------------------------------------------------------------------------
#ifdef USE_RAM_FUNC
__RAM_FUNC void DoNothing(void)
#else
void DoNothing(void)
#endif
{

}

//-----------------------------------------------------------------------------

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  LOG_E(TAG, "HAL error!");
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
     SEGGER_RTT_printf(0, "Wrong parameters value: file %s on line %d\n", file, line);
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */