Module_CSU3_Relay_CSRHx/CSRHO-00-RW V2/Src/main.c

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2019 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 "cmsis_os.h"
#include "adc.h"
#include "can.h"
#include "crc.h"
#include "dma.h"
#include "iwdg.h"
#include "rtc.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "usart.h"
#include "sine.h"
#include "flash_if.h"
/* USER CODE END Includes */

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

/* USER CODE END PTD */

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

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

/* USER CODE END PM */

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

/* USER CODE BEGIN PV */
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
void MX_FREERTOS_Init(void);
/* USER CODE BEGIN PFP */
#ifdef __GNUC__
	#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
	#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif 

PUTCHAR_PROTOTYPE
{
    HAL_UART_Transmit(&DEBUG_USART , (uint8_t *)&ch, 1, 0xFFFF);
    return ch;
}
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
uint16_t serial_n;
//const uint16_t nRPM_Tbl[11][2] ={ {0,0}, {10,1800}, {20,2400}, {30,3000}, {40,3800}, {50,4300}, {60,4800}, {70,5300}, {80,5600}, {90,5940}, {100,6200} };

struct MODULE_INFO Module_Info;
RLY_ST	RelayStatus;
struct COUNTER Counter;
struct OP_FLAG OpFlag;
struct GFD Gfd;
SF_test sf_t;
struct EXT_inupt Exti;

float Avg_offset_v ;
uint16_t Freq_CLC_Grid_v ;
uint8_t bGfd_Correct[2];
uint32_t  nGfd_Temp[2][17];
uint32_t R_GFD_Total[2];
uint8_t	  nGfd_Idx[2];
uint8_t nGun;
uint16_t pwmVal;
uint8_t dir ;
uint8_t bRelayFeedback;
uint8_t nBoard_Addr;
float c_vadc[2];
bool  GainCaliFlag;
uint8_t EXTI_SMR1_Count;
uint8_t EXTI_SMR2_Count;
uint8_t EXTI_SMR3_Count;
uint8_t EXTI_SMR4_Count;
uint8_t EXTI_SMR5_Count;
uint8_t EXTI_SMR6_Count;
uint8_t EXTI_Test_Count;

uint8_t EXTI_TestFlag1;
uint8_t EXTI_TestFlag2;
uint8_t EXTI_TestFlag3;
uint8_t EXTI_TestFlag4;
uint8_t EXTI_TestFlag5;
uint8_t EXTI_TestFlag6;

uint8_t ModelName[15];
uint8_t DoGfdBridge[2];
//for test
uint8_t temp1,temp2,temp3,temp4;
float temp5,temp6,temp7,temp8;
uint8_t Cmdcount; 

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */
  /* USER CODE END 1 */

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

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

  /* USER CODE BEGIN Init */
  
  memset(&Module_Info, 0x00, sizeof(struct MODULE_INFO));
  memset(&Counter, 0x00, sizeof(struct COUNTER));
  memset(&OpFlag, 0x00, sizeof(struct OP_FLAG));
  serial_n = 0;
  bGfd_Correct[0] = bGfd_Correct[1] = 0;
  bRelayFeedback = 0;
  
  for (int i=0; i<2; i++)
  {
  	nGfd_Idx[i] = 0;
  	for (int j=0; j<GFD_FILTER_LIMIT; j++)
  	{
  		nGfd_Temp[i][j] = 0;
  	}
  }
  
  sf_t.SF_Config.data.value = 0;
  sf_t.SF_Config.SF_test_status = 2;	// Wait for result.
  sf_t.SF_Config.SF_Act = 0;
  sf_t.SF_Config.SF_State = 0;
  Gfd.GfdMode[0] =	GFD_UNBALANCE;
  Gfd.GfdMode[1] =	GFD_UNBALANCE;
  Gfd.ToggleBridge[0] = 0;
  Gfd.ToggleBridge[1] = 0;
  
  pwmVal = 0;
  dir = 0;

  nBoard_Addr = 0x03;				// default old relay board
  
  /* USER CODE END Init */

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

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_RTC_Init();
  MX_USART1_UART_Init();
  MX_ADC1_Init();
  MX_ADC3_Init();
  MX_ADC2_Init();
  MX_TIM2_Init();
  MX_TIM5_Init();
  MX_IWDG_Init();
  MX_CRC_Init();
  MX_TIM3_Init();
  MX_USART6_UART_Init();
  MX_TIM4_Init();
  MX_CAN1_Init();
  /* USER CODE BEGIN 2 */
  
  HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_2);		// Led1
  HAL_TIM_IC_Start_IT(&htim2,TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim5, TIM_CHANNEL_3);
  HAL_TIM_Base_Start_IT(&htim3);

  // //Can Filter
  // ConfigFliter();
  
  // if(HAL_CAN_Start(&hcan1) != HAL_OK)
  // {
	// 			Error_Handler();
  // }      
  // //Enable Rx receice
  // HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
  
  HAL_ADC_Start_DMA(&hadc1, ADC1_Buf, (ADC1_CHANEL_COUNT * ADC1_SAMPLE_COUNT));  // start adc in DMA mode
  HAL_ADC_Start_DMA(&hadc2, ADC2_Buf, (ADC2_SAMPLE_COUNT * ADC2_CHANEL_COUNT));  // start adc in DMA mode
  HAL_ADC_Start_DMA(&hadc3, ADC3_Buf, (ADC3_CHANEL_COUNT * ADC3_SAMPLE_COUNT));  // start adc in DMA mode
  
  HAL_UART_Receive_DMA(&huart1, uart_rx_buffer, UART_BUFFER_SIZE);
  __HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);
	
	// Relay IO Reset All ....	
	
	HAL_GPIO_WritePin(SMR1_RLY_n_Enable_GPIO_Port, SMR1_RLY_n_Enable_Pin, GPIO_PIN_RESET);	
	HAL_GPIO_WritePin(SMR1_RLY_p_Enable_GPIO_Port, SMR1_RLY_p_Enable_Pin, GPIO_PIN_RESET);	
	HAL_GPIO_WritePin(SMR2_RLY_n_Enable_GPIO_Port, SMR2_RLY_n_Enable_Pin, GPIO_PIN_RESET);	
	HAL_GPIO_WritePin(SMR2_RLY_p_Enable_GPIO_Port, SMR2_RLY_p_Enable_Pin, GPIO_PIN_RESET);	
  HAL_GPIO_WritePin(SMR3_RLY_n_Enable_GPIO_Port, SMR3_RLY_n_Enable_Pin, GPIO_PIN_RESET);	
	HAL_GPIO_WritePin(SMR3_RLY_p_Enable_GPIO_Port, SMR3_RLY_p_Enable_Pin, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(SMR4_RLY_n_Enable_GPIO_Port, SMR4_RLY_n_Enable_Pin, GPIO_PIN_RESET);	
	HAL_GPIO_WritePin(SMR4_RLY_p_Enable_GPIO_Port, SMR4_RLY_p_Enable_Pin, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(SMR5_RLY_n_Enable_GPIO_Port, SMR5_RLY_n_Enable_Pin, GPIO_PIN_RESET);	
	HAL_GPIO_WritePin(SMR5_RLY_p_Enable_GPIO_Port, SMR5_RLY_p_Enable_Pin, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(SMR6_RLY_n_Enable_GPIO_Port, SMR6_RLY_n_Enable_Pin, GPIO_PIN_RESET);	
	HAL_GPIO_WritePin(SMR6_RLY_p_Enable_GPIO_Port, SMR6_RLY_p_Enable_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(Contactor_Enable_GPIO_Port, Contactor_Enable_Pin, GPIO_PIN_RESET);
	
	HAL_GPIO_WritePin(PSU_Enable1_GPIO_Port, PSU_Enable1_Pin, GPIO_PIN_RESET);	// no used
  HAL_GPIO_WritePin(PSU_Enable2_GPIO_Port, PSU_Enable2_Pin, GPIO_PIN_RESET);	// no used

  //LED2,LED3 OFF
  HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET);	
	HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_SET);	
			
	FLASH_If_Init();  
	
	//  Read board Dip address and redefine address	
	Module_Info.Dip_status.flags.SW2 =~HAL_GPIO_ReadPin(SW2_GPIO_Port, SW2_Pin);	// SW_2
	Module_Info.Dip_status.flags.SW0 =~HAL_GPIO_ReadPin(SW1_GPIO_Port, SW1_Pin);	// SW_1
	Module_Info.Dip_status.flags.SW1 =~HAL_GPIO_ReadPin(SW0_GPIO_Port, SW0_Pin);	// SW_0
	
	switch(Module_Info.Dip_status.Mode)
	{
	case 01:
		nBoard_Addr	= MainRelay1;
		break;
  case 02:
		nBoard_Addr	= MainRelay2;
		break;
	case 03:
		nBoard_Addr	= GunRelay;
		break;
	case 04:
		nBoard_Addr = MainBridge1;
		break;
  case 05:
		nBoard_Addr = MainBridge2;
		break;
  case 06:
		nBoard_Addr = MainBridge3;
		break;    
  case 07:
		nBoard_Addr = MainBridge4;
		break;      
	default:
		nBoard_Addr	= 0xFF;     
		break;
	}	
	
	//Module_Info.Dip_status.Mode = m_ALL;
	
  /* USER CODE END 2 */

  /* Call init function for freertos objects (in freertos.c) */
  MX_FREERTOS_Init();

  /* Start scheduler */
  osKernelStart();

  /* We should never get here as control is now taken by the scheduler */
  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
    
  }
  /* 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_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSI|RCC_OSCILLATORTYPE_HSE
                              |RCC_OSCILLATORTYPE_LSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.LSEState = RCC_LSE_ON;
  RCC_OscInitStruct.LSIState = RCC_LSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 25;
  RCC_OscInitStruct.PLL.PLLN = 336;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  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_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV4;

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

/* USER CODE BEGIN 4 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	// Sure in the charging state.



  if (sf_t.SF_Config.SF_Act){
    if(GPIO_Pin == GPIO_PIN_0){
      printf("SMR1 OFF\r\n");
      EXTI_TestFlag1 = 1; 		
    }  
    if(GPIO_Pin == GPIO_PIN_1){
      printf("SMR2 OFF\r\n");
      EXTI_TestFlag2 = 1;		
    }  
    if(GPIO_Pin == GPIO_PIN_2){
      printf("SMR3 OFF\r\n");
      EXTI_TestFlag3 = 1;
    }  
      if(GPIO_Pin == GPIO_PIN_3){
      printf("SMR4 OFF\r\n");
      EXTI_TestFlag4 = 1;
    }  
      if(GPIO_Pin == GPIO_PIN_4){
      printf("SMR5 OFF\r\n");
      EXTI_TestFlag5 = 1;
    }  
      if(GPIO_Pin == GPIO_PIN_5){
      printf("SMR6 OFF\r\n");
      EXTI_TestFlag6 = 1;
    }  
  }else{  		
    if(Module_Info.SMR1_Relay_V > 200){
        if(GPIO_Pin == GPIO_PIN_0){
          // Disable SMR1 DC Output.		
          Exti.EXTI_SMR1_Flag = true;    		
          HAL_GPIO_WritePin(SMR1_RLY_n_Enable_GPIO_Port, SMR1_RLY_n_Enable_Pin, GPIO_PIN_RESET);
          HAL_GPIO_WritePin(SMR1_RLY_p_Enable_GPIO_Port, SMR1_RLY_p_Enable_Pin, GPIO_PIN_RESET);    
          printf(" SMR1 Relay Off, SMR1 Voltage = %d \n\r",  Module_Info.SMR1_Relay_V);					
          HAL_Delay(50);			
        }		
    }
      
    if(Module_Info.SMR2_Relay_V > 200){	
        if(GPIO_Pin == GPIO_PIN_1){
          // Disable SMR2 DC Output.				
          Exti.EXTI_SMR2_Flag = true; 			
          HAL_GPIO_WritePin(SMR2_RLY_n_Enable_GPIO_Port, SMR2_RLY_n_Enable_Pin, GPIO_PIN_RESET);
          HAL_GPIO_WritePin(SMR2_RLY_p_Enable_GPIO_Port, SMR2_RLY_p_Enable_Pin, GPIO_PIN_RESET);  
          printf(" SMR2 Relay Off, SMR2 Voltage = %d \n\r",  Module_Info.SMR2_Relay_V);			
          HAL_Delay(50);				
        }		
    }

    if(Module_Info.SMR3_Relay_V > 200){	
        if(GPIO_Pin == GPIO_PIN_2){
          // Disable SMR3 DC Output.				
          Exti.EXTI_SMR3_Flag = true; 			
          HAL_GPIO_WritePin(SMR3_RLY_n_Enable_GPIO_Port, SMR3_RLY_n_Enable_Pin, GPIO_PIN_RESET);
          HAL_GPIO_WritePin(SMR3_RLY_p_Enable_GPIO_Port, SMR3_RLY_p_Enable_Pin, GPIO_PIN_RESET);  
          printf(" SMR3 Relay Off, SMR2 Voltage = %d \n\r",  Module_Info.SMR3_Relay_V);			
          HAL_Delay(50);				
        }		
    }

    if(Module_Info.SMR4_Relay_V > 200){	
        if(GPIO_Pin == GPIO_PIN_3){
          // Disable SMR4 DC Output.				
          Exti.EXTI_SMR4_Flag = true; 			
          HAL_GPIO_WritePin(SMR4_RLY_n_Enable_GPIO_Port, SMR4_RLY_n_Enable_Pin, GPIO_PIN_RESET);
          HAL_GPIO_WritePin(SMR4_RLY_p_Enable_GPIO_Port, SMR4_RLY_p_Enable_Pin, GPIO_PIN_RESET);  
          printf(" SMR2 Relay Off, SMR4 Voltage = %d \n\r",  Module_Info.SMR4_Relay_V);			
          HAL_Delay(50);				
        }		
    }

    if(Module_Info.SMR5_Relay_V > 200){	
        if(GPIO_Pin == GPIO_PIN_4){
          // Disable SMR5 DC Output.				
          Exti.EXTI_SMR5_Flag = true; 			
          HAL_GPIO_WritePin(SMR5_RLY_n_Enable_GPIO_Port, SMR5_RLY_n_Enable_Pin, GPIO_PIN_RESET);
          HAL_GPIO_WritePin(SMR5_RLY_p_Enable_GPIO_Port, SMR5_RLY_p_Enable_Pin, GPIO_PIN_RESET);  
          printf(" SMR5 Relay Off, SMR2 Voltage = %d \n\r",  Module_Info.SMR5_Relay_V);			
          HAL_Delay(50);				
        }		
    }

    if(Module_Info.SMR6_Relay_V > 200){	
        if(GPIO_Pin == GPIO_PIN_5){
          // Disable SMR6 DC Output.				
          Exti.EXTI_SMR6_Flag = true; 			
          HAL_GPIO_WritePin(SMR6_RLY_n_Enable_GPIO_Port, SMR6_RLY_n_Enable_Pin, GPIO_PIN_RESET);
          HAL_GPIO_WritePin(SMR6_RLY_p_Enable_GPIO_Port, SMR6_RLY_p_Enable_Pin, GPIO_PIN_RESET);  
          printf(" SMR6 Relay Off, SMR2 Voltage = %d \n\r",  Module_Info.SMR6_Relay_V);			
          HAL_Delay(50);				
        }		
    }
  }
}

// void ConfigFliter(void)
// {
//   CAN_FilterTypeDef sFilterConfig;
//   sFilterConfig.FilterBank = 0;
//   sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
//   sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
//   sFilterConfig.FilterIdHigh = 0x0000;
//   sFilterConfig.FilterIdLow = 0x0000;
//   sFilterConfig.FilterMaskIdHigh = 0x0000;
//   sFilterConfig.FilterMaskIdLow = 0x0000;
//   sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
//   sFilterConfig.FilterActivation = ENABLE;
//   sFilterConfig.SlaveStartFilterBank = 0;
  
//   if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK)
//   {
//     Error_Handler();
//   }
// }
			
/* USER CODE END 4 */

/**
  * @brief  Period elapsed callback in non blocking mode
  * @note   This function is called  when TIM7 interrupt took place, inside
  * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
  * a global variable "uwTick" used as application time base.
  * @param  htim : TIM handle
  * @retval None
  */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  /* USER CODE BEGIN Callback 0 */

  /* USER CODE END Callback 0 */
  if (htim->Instance == TIM7) {
    HAL_IncTick();
  }
  /* USER CODE BEGIN Callback 1 */
	if (htim->Instance==TIM3)			// 100ms
	{
    if(Exti.EXTI_SMR1_Flag && EXTI_SMR1_Count < 20)
      EXTI_SMR1_Count ++;
    
    if(Exti.EXTI_SMR2_Flag && EXTI_SMR2_Count < 20)
      EXTI_SMR2_Count ++;

    if(Exti.EXTI_SMR3_Flag && EXTI_SMR3_Count < 20)
      EXTI_SMR3_Count ++;

    if(Exti.EXTI_SMR4_Flag && EXTI_SMR4_Count < 20)
      EXTI_SMR4_Count ++;

    if(Exti.EXTI_SMR5_Flag && EXTI_SMR5_Count < 20)
      EXTI_SMR5_Count ++;

    if(Exti.EXTI_SMR6_Flag && EXTI_SMR6_Count < 20)
      EXTI_SMR6_Count ++;   

  for(int i=0; i<2; i++){
      if(Gfd.SCBwaitfg[i] == 1){
          if(Gfd.SCBwaitcount[i] <= 13){     
            Gfd.SCBwaitcount[i]++;
          }else{
            Gfd.SCBwaitcount[i] = 0;
            Gfd.SCBwaitfg[i] = 0;
            Gfd.GfdAvgCount[i] = 0;
          }
      }  

      if(Module_Info.gfd_chk[i].Csu_State != IDLE || bGfd_Correct[i] == 1){

          if(Gfd.operation[i] == GFD_BALANCE && Gfd.SCBwaitfg[i] == 0){      
              if(++Gfd.GfdAvgCount[i] > 5){
                Gfd.GfdAvgCount[i] = 0;        
              }else{
                Gfd.GfdAvgCount[i]++;   
              }      
              if(i == 0){         
                HAL_GPIO_WritePin(Drv_Up_GPIO_Port, Drv_Up_Pin, GPIO_PIN_SET);
                HAL_GPIO_WritePin(Drv_Down_GPIO_Port, Drv_Down_Pin, GPIO_PIN_SET);
              }else{
                HAL_GPIO_WritePin(Drv_Up_2_GPIO_Port, Drv_Up_2_Pin, GPIO_PIN_SET);
                HAL_GPIO_WritePin(Drv_Down_2_GPIO_Port, Drv_Down_2_Pin, GPIO_PIN_SET);
              }    
          }  
      }  
  }  
      DoGfdBridge[0] = 0; 
      DoGfdBridge[1] = 0; 
      if(Gfd.operation[0] == GFD_UNBALANCE){
          if((Module_Info.gfd_chk[0].Csu_State == CABLE_CHECK && Module_Info.gfd_chk[0].Rfd_State > 2) || 
             (Module_Info.gfd_chk[0].Csu_State == PRECHARGE && Module_Info.SMR1_Relay_V > GFD_WORKING_VOLTAGE) ||
             (Module_Info.gfd_chk[0].Csu_State == CHARGE    && Module_Info.SMR1_Relay_V > GFD_WORKING_VOLTAGE))  {
             DoGfdBridge[0] = 1; 
          } 
      }    
      if(Gfd.operation[1] == GFD_UNBALANCE){
          if((Module_Info.gfd_chk[1].Csu_State == CABLE_CHECK && Module_Info.gfd_chk[1].Rfd_State > 2) || 
             (Module_Info.gfd_chk[1].Csu_State == PRECHARGE && Module_Info.SMR2_Relay_V > GFD_WORKING_VOLTAGE) ||
             (Module_Info.gfd_chk[1].Csu_State == CHARGE    && Module_Info.SMR2_Relay_V > GFD_WORKING_VOLTAGE))  {
              DoGfdBridge[1] = 1; 
          } 
      }    


    if(DoGfdBridge[0] == 1 || DoGfdBridge[1] == 1){     
        Gfd.Gfdtimer++;    
        if(Gfd.Gfdtimer < GFD_SWITCH_TIME*2){
            if(DoGfdBridge[0] == 0 || 
              Gfd.operation[0] != GFD_UNBALANCE){
              Gfd.Gfdtimer = (GFD_SWITCH_TIME*2)+1;
            }
        }  

         if(Gfd.Gfdtimer > GFD_SWITCH_TIME*2){
            if(DoGfdBridge[1] == 0  || 
              Gfd.operation[1] != GFD_UNBALANCE){
              Gfd.Gfdtimer = 0;
            }
        }          
          

          if(Gfd.Gfdtimer < GFD_SWITCH_TIME){
            Gfd.GfdSwitch = 0;           
          }else if (Gfd.Gfdtimer > GFD_SWITCH_TIME   && Gfd.Gfdtimer <= GFD_SWITCH_TIME*2){
            Gfd.GfdSwitch = 1;
          }else if (Gfd.Gfdtimer > GFD_SWITCH_TIME*2 && Gfd.Gfdtimer <= GFD_SWITCH_TIME*3){
            Gfd.GfdSwitch = 2;
          }else if (Gfd.Gfdtimer > GFD_SWITCH_TIME*3 && Gfd.Gfdtimer <= GFD_SWITCH_TIME*4){     
            Gfd.GfdSwitch = 3;   
          }     
         
          if(Gfd.Gfdtimer > (GFD_SWITCH_TIME*4)+1){
            Gfd.Gfdtimer = 0;
          }

          switch (Gfd.GfdSwitch){
              case 0:
                if(Gfd.operation[0] == GFD_UNBALANCE){
                  HAL_GPIO_WritePin(Drv_Up_GPIO_Port, Drv_Up_Pin, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(Drv_Down_GPIO_Port, Drv_Down_Pin, GPIO_PIN_RESET);    
                }  
                if(Gfd.operation[1] == GFD_UNBALANCE){
                  HAL_GPIO_WritePin(Drv_Up_2_GPIO_Port, Drv_Up_2_Pin, GPIO_PIN_RESET);
                  HAL_GPIO_WritePin(Drv_Down_2_GPIO_Port, Drv_Down_2_Pin, GPIO_PIN_RESET);  
                }          
                break;
              case 1:
                if(Gfd.operation[0] == GFD_UNBALANCE){
                  HAL_GPIO_WritePin(Drv_Up_GPIO_Port, Drv_Up_Pin, GPIO_PIN_RESET);
                  HAL_GPIO_WritePin(Drv_Down_GPIO_Port, Drv_Down_Pin, GPIO_PIN_SET);    
                }     
                break;  
              case 2:
                if(Gfd.operation[0] == GFD_UNBALANCE){
                  HAL_GPIO_WritePin(Drv_Up_GPIO_Port, Drv_Up_Pin, GPIO_PIN_RESET);
                  HAL_GPIO_WritePin(Drv_Down_GPIO_Port, Drv_Down_Pin, GPIO_PIN_RESET); 
                }
                if(Gfd.operation[1] == GFD_UNBALANCE){  
                  HAL_GPIO_WritePin(Drv_Up_2_GPIO_Port, Drv_Up_2_Pin, GPIO_PIN_SET);
                  HAL_GPIO_WritePin(Drv_Down_2_GPIO_Port, Drv_Down_2_Pin, GPIO_PIN_RESET); 
                }       
                break;  
              case 3:
                if(Gfd.operation[1] == GFD_UNBALANCE){
                  HAL_GPIO_WritePin(Drv_Up_2_GPIO_Port, Drv_Up_2_Pin, GPIO_PIN_RESET);
                  HAL_GPIO_WritePin(Drv_Down_2_GPIO_Port, Drv_Down_2_Pin, GPIO_PIN_SET);    
                }     
                break;    
          } 
    }else{
      Gfd.Gfdtimer = 0; 
    }   
  } 	
  /* USER CODE END Callback 1 */
}

/**
  * @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 */

  /* 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) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */