#include "stm32f10x_svpwm_3shunt.h"
#include "MC_const.h"
#include "adc.h"
#include "pwm_driver.h"
#include "log_save.h"

//全局变量定义

uint16_t	bSector;

uint8_t PWM4Direction=0;

typedef struct
{
  uint16_t hTimePhA;
	uint16_t hTimePhB;
	uint16_t hTimePhC;
	uint16_t hTimePhD;
}hTimePhase_Struct_t;

hTimePhase_Struct_t hTimePhase;

/**************************全局函数定义*************************/
//3相电流校零
void SVPWM_3ShuntCurrentReadingCalibration(MC_ErrorCode_Struct_t* p_MC_ErrorCode)
{
  uint16_t 	bIndex;
	uint32_t	ul_phase_a_offset_sum = 0;
	uint32_t	ul_phase_b_offset_sum = 0;
	uint32_t	ul_phase_c_offset_sum = 0;
	
	Disable_Pwm_Output();	
	
  for(bIndex=0; bIndex < 10; bIndex++)
  {
    //wait the ADC1 JEOC pending flag */
		do
    {
      ;
    }while(SET != ADC2_ConvCpmplete_Flag);
    ///Clear the ADC1 JEOC pending flag */
		ADC2_ConvCpmplete_Flag = RESET;

    ul_phase_a_offset_sum = ADC2_Result[ADC2_RANK_CURRENT_A] + ul_phase_a_offset_sum;
    ul_phase_b_offset_sum = ADC2_Result[ADC2_RANK_CURRENT_B] + ul_phase_b_offset_sum;
		ul_phase_c_offset_sum = ADC2_Result[ADC2_RANK_CURRENT_C] + ul_phase_c_offset_sum;		
		HAL_TIM_PWM_Start(&PWM_TIMER, TIM_CHANNEL_4);
  }
	ADC_3ShuntCurrent_OffSet.uw_phase_a_offset = ul_phase_a_offset_sum / 10;
	ADC_3ShuntCurrent_OffSet.uw_phase_b_offset = ul_phase_b_offset_sum / 10;
	ADC_3ShuntCurrent_OffSet.uw_phase_c_offset = ul_phase_c_offset_sum / 10;
	
	//判断零点值是否在正常范围内
	if(((ADC_3ShuntCurrent_OffSet.uw_phase_a_offset < 25000) || (ADC_3ShuntCurrent_OffSet.uw_phase_a_offset > 40000)) ||
		 ((ADC_3ShuntCurrent_OffSet.uw_phase_b_offset < 25000) || (ADC_3ShuntCurrent_OffSet.uw_phase_b_offset > 40000)) ||
	   ((ADC_3ShuntCurrent_OffSet.uw_phase_c_offset < 25000) || (ADC_3ShuntCurrent_OffSet.uw_phase_c_offset > 40000))
	  )
	{
	  p_MC_ErrorCode->ERROR_Bit.Fault_Circuit = 1;
		//记录故障日志
		MC_ErrorLogSaveInfo.NotesInfo1 = 6;
		ErrorLogSave_Update(&MC_ErrorLogSaveInfo);
		IsErrorLogSaveInfoUpdateFlag = TRUE;
		//存储故障次数
		MC_RunLog2.Circuit_FaultCnt++;
		RunLogSaveIndex = 2;
	}
	
	Disable_Pwm_Output();
}

//母线电流校零
void CurrentReadingCalibration(MC_ErrorCode_Struct_t* p_MC_ErrorCode)
{
  static TrueOrFalse_Flag_Struct_t IsFirstEnterFlag = TRUE;
  static uint32_t PeriodTimeCnt = 0;
	static uint32_t LeaveTime = 0;
	uint32_t	ul_current_offset_sum = 0;
	uint16_t 	bIndex;	

  if(IsFirstEnterFlag == TRUE)
	{
		for(bIndex=0; bIndex < 10; bIndex++)
		{
			ul_current_offset_sum = ADC1_Result[ADC1_RANK_CURRENT] + ul_current_offset_sum;
		}
		uw_current_offset = ul_current_offset_sum / 10;
		PeriodTimeCnt = HAL_GetTick();
		LeaveTime = HAL_GetTick();
		IsFirstEnterFlag = FALSE;
	}		
	else
	{
		if((HAL_GetTick() - LeaveTime) > 500)
		{
	  	PeriodTimeCnt = HAL_GetTick();
		}
		
		if((HAL_GetTick() - PeriodTimeCnt) > 2000)
		{
			for(bIndex=0; bIndex < 10; bIndex++)
			{
				ul_current_offset_sum = ADC1_Result[ADC1_RANK_CURRENT] + ul_current_offset_sum;
			}
			uw_current_offset = ul_current_offset_sum / 10;
			if((uw_current_offset < 1000) || (uw_current_offset > 3000))
			{
			  p_MC_ErrorCode->ERROR_Bit.Fault_Circuit = 1;
				//记录故障日志
				MC_ErrorLogSaveInfo.NotesInfo1 = 7;
				MC_ErrorLogSaveInfo.NotesInfo2 = uw_current_offset;
				ErrorLogSave_Update(&MC_ErrorLogSaveInfo);
				IsErrorLogSaveInfoUpdateFlag = TRUE;
				//存储故障次数
				MC_RunLog2.Circuit_FaultCnt++;
				RunLogSaveIndex = 2;
			}
			PeriodTimeCnt = HAL_GetTick();
		}
	}
	
	LeaveTime = HAL_GetTick();
}

//3相占空比设置
void SVPWM_3ShuntCalcDutyCycles (Volt_Components Stat_Volt_Input)
{
  int32_t wX, wY, wZ, wUAlpha, wUBeta;

	uint16_t  hDeltaDuty;

	wUAlpha = Stat_Volt_Input.qV_Component1 * T_SQRT3 ;
	wUBeta = -(Stat_Volt_Input.qV_Component2 * T);

	wX = wUBeta;
	wY = (wUBeta + wUAlpha) >> 1;
	wZ = (wUBeta - wUAlpha) >> 1;

	// Sector calculation from wX, wY, wZ
	if (wY<0)
	{
		if (wZ<0)
		{
			bSector = SECTOR_5;
		}
		else // wZ >= 0
			if (wX<=0)
			{
				bSector = SECTOR_4;
			}
			else // wX > 0
			{
				bSector = SECTOR_3;
			}
	}
  else // wY > 0
	{
     if (wZ>=0)
     {
       bSector = SECTOR_2;
     }
     else // wZ < 0
		 if (wX<=0)
		 {  
			 bSector = SECTOR_6;
		 }
		 else // wX > 0
		 {
			 bSector = SECTOR_1;
		 }
	}
   
   
  PWM4Direction=0;
    
  switch(bSector)
  {  
    case SECTOR_1:
                hTimePhase.hTimePhA = (T >> 3) + ((((T + wX) - wZ) >> 1) >> 17);
								hTimePhase.hTimePhB = hTimePhase.hTimePhA + (wZ >> 17);
								hTimePhase.hTimePhC = hTimePhase.hTimePhB - (wX >> 17);

//                hTimePhD = PWM_PERIOD - 1;
                // ADC Syncronization setting value 
								           
                if ((uint16_t)(PWM_PERIOD-hTimePhase.hTimePhA) > TW_AFTER)
                {
									hTimePhase.hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
									hDeltaDuty = (uint16_t)(hTimePhase.hTimePhA - hTimePhase.hTimePhB);
                  
									// Definition of crossing point
                  if (hDeltaDuty > TW_DT_TR_TS) 
                  {
                      hTimePhase.hTimePhD = hTimePhase.hTimePhA - TW_BEFORE; // Ts before Phase A 
                  }
                  else if((uint16_t)(PWM_PERIOD-hTimePhase.hTimePhA) >TW_DT_TN_TS_HALF)
                  {
										hTimePhase.hTimePhD = hTimePhase.hTimePhA + TW_AFTER; // DT + Tn after Phase A
                     
                    if (hTimePhase.hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
												PWM4Direction=1;
												
												hTimePhase.hTimePhD = (2 * PWM_PERIOD) - hTimePhase.hTimePhD-1;
										}
									}
//									else
//									{
//										while(1);
//									}
								}
                                                      
                break;
    case SECTOR_2:
                hTimePhase.hTimePhA = (T >> 3) + ((((T + wY) - wZ) >> 1) >> 17);
								hTimePhase.hTimePhB = hTimePhase.hTimePhA + (wZ >> 17);
								hTimePhase.hTimePhC = hTimePhase.hTimePhA - (wY >> 17);
                
//								hTimePhD = PWM_PERIOD - 1;
                // ADC Syncronization setting value
                if ((uint16_t)(PWM_PERIOD-hTimePhase.hTimePhB) > TW_AFTER)
                {
                  hTimePhase.hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (uint16_t)(hTimePhase.hTimePhB - hTimePhase.hTimePhA);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > TW_DT_TR_TS) 
                  {
                    hTimePhase.hTimePhD = hTimePhase.hTimePhB - TW_BEFORE; // Ts before Phase B 
                  }
                  else if((uint16_t)(PWM_PERIOD-hTimePhase.hTimePhB) > TW_DT_TN_TS_HALF)
                  {
                    hTimePhase.hTimePhD = hTimePhase.hTimePhB + TW_AFTER; // DT + Tn after Phase B
                    
                    if (hTimePhase.hTimePhD >= PWM_PERIOD)
                    {
                      PWM4Direction=1;
                      
                      hTimePhase.hTimePhD = (2 * PWM_PERIOD) - hTimePhase.hTimePhD-1;
                    }
                  }
//									else
//									{
//										while(1);
//									}
                }            	
                break;

    case SECTOR_3:
                hTimePhase.hTimePhA = (T >> 3) + ((((T - wX) + wY) >> 1) >> 17);
								hTimePhase.hTimePhC = hTimePhase.hTimePhA - (wY >> 17);
								hTimePhase.hTimePhB = hTimePhase.hTimePhC + (wX >> 17);
		
//		            hTimePhD = PWM_PERIOD - 1;
                // ADC Syncronization setting value
								
                if ((uint16_t)(PWM_PERIOD-hTimePhase.hTimePhB) > TW_AFTER)
                {
                  hTimePhase.hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (uint16_t)(hTimePhase.hTimePhB - hTimePhase.hTimePhC);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > TW_DT_TR_TS) 
                  {
                    hTimePhase.hTimePhD = hTimePhase.hTimePhB - TW_BEFORE; // Ts before Phase B 
                  }
                  else if((uint16_t)(PWM_PERIOD-hTimePhase.hTimePhB) > TW_DT_TN_TS_HALF)
                  {
                    hTimePhase.hTimePhD = hTimePhase.hTimePhB + TW_AFTER; // DT + Tn after Phase B
                    
                    if (hTimePhase.hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=1;
                      
                      hTimePhase.hTimePhD = (2 * PWM_PERIOD) - hTimePhase.hTimePhD-1;
                    }
                  }
//									else
//									{
//										while(1);
//									}
                }
			
                break;
    
    case SECTOR_4:
                hTimePhase.hTimePhA = (T>> 3) + ((((T + wX) - wZ) >> 1) >> 17);
                hTimePhase.hTimePhB = hTimePhase.hTimePhA + (wZ >> 17);
                hTimePhase.hTimePhC = hTimePhase.hTimePhB - (wX >> 17);
                
//								hTimePhD = PWM_PERIOD - 1;
                // ADC Syncronization setting value
								
                if ((uint16_t)(PWM_PERIOD-hTimePhase.hTimePhC) > TW_AFTER)
                {
                  hTimePhase.hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (uint16_t)(hTimePhase.hTimePhC - hTimePhase.hTimePhB);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > TW_DT_TR_TS)
                  {
                    hTimePhase.hTimePhD = hTimePhase.hTimePhC - TW_BEFORE; // Ts before Phase C 
                  }
                  else if((uint16_t)(PWM_PERIOD-hTimePhase.hTimePhC) > TW_DT_TN_TS_HALF)
                  {
                    hTimePhase.hTimePhD = hTimePhase.hTimePhC + TW_AFTER; // DT + Tn after Phase C
                    
                    if (hTimePhase.hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=1;
                      
                      hTimePhase.hTimePhD = (2 * PWM_PERIOD) - hTimePhase.hTimePhD-1;
                    }
                  }
//									else
//									{
//										while(1);
//									}
                }
			
                break;  
    
    case SECTOR_5:
                hTimePhase.hTimePhA = (T >> 3) + ((((T + wY) - wZ) >> 1) >> 17);
								hTimePhase.hTimePhB = hTimePhase.hTimePhA + (wZ >> 17);
								hTimePhase.hTimePhC = hTimePhase.hTimePhA - (wY >> 17);
                
//								hTimePhD = PWM_PERIOD - 1;
                // ADC Syncronization setting value
                if ((uint16_t)(PWM_PERIOD-hTimePhase.hTimePhC) > TW_AFTER)
                {
                  hTimePhase.hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (uint16_t)(hTimePhase.hTimePhC - hTimePhase.hTimePhA);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > TW_DT_TR_TS) 
                  {
                    hTimePhase.hTimePhD = hTimePhase.hTimePhC - TW_BEFORE; // Ts before Phase C 
                  }
                  else if((uint16_t)(PWM_PERIOD-hTimePhase.hTimePhC) > TW_DT_TN_TS_HALF)
                  {
                    hTimePhase.hTimePhD = hTimePhase.hTimePhC + TW_AFTER; // DT + Tn after Phase C
                    
                    if (hTimePhase.hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=1;
                      
                      hTimePhase.hTimePhD = (2 * PWM_PERIOD) - hTimePhase.hTimePhD-1;
                    }
                  }
//									else
//									{
//										while(1);
//									}
                }
				
		break;
                
    case SECTOR_6:
                hTimePhase.hTimePhA = (T >> 3) + ((((T - wX) + wY) >> 1) >> 17);
								hTimePhase.hTimePhC = hTimePhase.hTimePhA - (wY >> 17);
								hTimePhase.hTimePhB = hTimePhase.hTimePhC + (wX >> 17);
												
								hTimePhase.hTimePhD = PWM_PERIOD - 1;
                // ADC Syncronization setting value
								
                if ((uint16_t)(PWM_PERIOD-hTimePhase.hTimePhA) > TW_AFTER)
                {
                  hTimePhase.hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (uint16_t)(hTimePhase.hTimePhA - hTimePhase.hTimePhC);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > TW_DT_TR_TS) 
                  {
                    hTimePhase.hTimePhD = hTimePhase.hTimePhA - TW_BEFORE; // Ts before Phase A 
                  }
                  else if((uint16_t)(PWM_PERIOD-hTimePhase.hTimePhA) > TW_DT_TN_TS_HALF)
                  {
                    hTimePhase.hTimePhD = hTimePhase.hTimePhA + TW_AFTER; // DT + Tn after Phase A
                    
                    if (hTimePhase.hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=1;
                      
                      hTimePhase.hTimePhD = (2 * PWM_PERIOD) - hTimePhase.hTimePhD-1;
                    }
                  }
//									else
//									{
//										while(1);
//									}
                }
				
                break;
    default:
		break;
   }
  
  if (PWM4Direction == 0)
  {
    //Set Polarity of CC4 High
		Set_Pwm_Chanle4_Polarity(TIM_OCPOLARITY_HIGH);		
	//	Set_Pwm_Chanle4_Polarity(TIM_OCPOLARITY_LOW);
  }
  else
  {
    //Set Polarity of CC4 Low
		Set_Pwm_Chanle4_Polarity(TIM_OCPOLARITY_LOW);
	//	Set_Pwm_Chanle4_Polarity(TIM_OCPOLARITY_HIGH);
  }
//  Set_Pwm_Chanle4_Polarity(TIM_OCPOLARITY_LOW); 
	hTimePhase.hTimePhD=PWM_PERIOD-20;
  //Load compare registers values 
	Set_Pwm_Chanle1_Compare(hTimePhase.hTimePhA);
	Set_Pwm_Chanle2_Compare(hTimePhase.hTimePhB);
	Set_Pwm_Chanle3_Compare(hTimePhase.hTimePhC);
	Set_Pwm_Chanle4_Compare(hTimePhase.hTimePhD);
}

//读取三相电流
Curr_Components SVPWM_3ShuntGetPhaseCurrentValues(void)
{
  Curr_Components Local_Stator_Currents = {0, 0};
  int16_t wAux;
  switch (bSector)
  {
    case 4:
    case 5: //Current on Phase C not accessible      
						wAux =-(ADC_3ShuntCurrent.uw_phase_a);
           //Saturation of Ia 
            if (wAux < -32768)
            {
              Local_Stator_Currents.qI_Component1= -32768;
            }  
            else  if (wAux > 32767)
						{ 
							Local_Stator_Currents.qI_Component1= 32767;
						}
						else
						{
							Local_Stator_Currents.qI_Component1= wAux;
						}
						wAux =-(ADC_3ShuntCurrent.uw_phase_b);
           // Saturation of Ib
            if (wAux < -32768)
            {
              Local_Stator_Currents.qI_Component2= -32768;
            }  
            else  if (wAux > 32767)
						{ 
							Local_Stator_Currents.qI_Component2= 32767;
						}
						else
						{
							Local_Stator_Currents.qI_Component2= wAux;
						}
						break;
           
   case 6:
   case 1: 
						wAux =-(ADC_3ShuntCurrent.uw_phase_b);
            //Saturation of Ib 
            if (wAux < -32768)
            {
              Local_Stator_Currents.qI_Component2= -32768;
            }  
            else  if (wAux > 32767)
						{ 
							Local_Stator_Currents.qI_Component2= 32767;
						}
						else
						{
							Local_Stator_Currents.qI_Component2= wAux;
						}
						
						//Ia = -Ib - Ic;
						wAux = ADC_3ShuntCurrent.uw_phase_c - Local_Stator_Currents.qI_Component2;
            if (wAux> 32767)
            {
               Local_Stator_Currents.qI_Component1 = 32767;
            }
            else  if (wAux <-32768)
						{
						 Local_Stator_Currents.qI_Component1 = -32768;
						}
						else
						{  
							Local_Stator_Currents.qI_Component1 = wAux;
						}

           break;
           
   case 2:
   case 3: 
						// Current on Phase B not accessible
						wAux =-(ADC_3ShuntCurrent.uw_phase_a);  
            // Saturation of Ia
            if (wAux> 32767)
            {
              Local_Stator_Currents.qI_Component1=32767;
            }
            else  if (wAux <-32768)
						{  
							Local_Stator_Currents.qI_Component1 = -32768;
						}
						else  
						{
							Local_Stator_Currents.qI_Component1 = wAux;
						} 


						wAux = ADC_3ShuntCurrent.uw_phase_c - Local_Stator_Currents.qI_Component1;		  //Ib = -Ia - Ic;
            //Saturation of Ib 
            if (wAux < -32768)
            {
              Local_Stator_Currents.qI_Component2= -32768;
            }  
            else  if (wAux > 32767)
						{ 
							Local_Stator_Currents.qI_Component2= 32767;
						}
						else
						{
							Local_Stator_Currents.qI_Component2= wAux;
						}
                      
           break;

   default:
           break;
   }
	 
		Local_Stator_Currents.qI_Component1 = Local_Stator_Currents.qI_Component1 >> 4;
		Local_Stator_Currents.qI_Component2 = Local_Stator_Currents.qI_Component2 >> 4;		

		return(Local_Stator_Currents); 
}

//读母线电流
int16_t GetCurrentValues(void)
{
  int16_t Result;
	
	Result = ADC1_Result[ADC1_RANK_CURRENT] - (int16_t)uw_current_offset;
	
	return Result;
}