motor_ctl-1/3.BasicFunction/Source/switchhall.c

/**
 * @file switchhall.c
 * @author
 * @brief
 * @version
 * @date 2023.11.8
 *
 * @copyright Copyright (c) 2021
 *
 */

/************************************************************************
 Beginning of File, do not put anything above here except notes
 Compiler Directives:
*************************************************************************/
//#include "at32f421.h"//// #include "stm32f10x.h"
#include "typedefine.h"
#include "syspar.h"
#include "hwsetup.h"
#include "glbcof.h"
#include "switchhall.h"
#include "spdctrFSM.h"
#include "spdctrmode.h"
#include "UserGpio_Config.h"
#include "board_config.h"
#include "emcdeal.h"
#include "user.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
SWITCHHALL_CAP switchhall_stCap = SWITCHHALL_CAP_DEFAULT;
SWITCHHALL_OUT switchhall_stOut = SWITCHHALL_OUT_DEFAULT;


/***************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
****************************************************************/
void switchhall_voInit(void)
{
    switchhall_stOut.swLowSpdLpfPu = 0;
    switchhall_stOut.swLowSpdPu = 0;
    switchhall_stOut.swSpdCWorCCW = 0;
    switchhall_stOut.slInitThetaPu = 0;
    switchhall_stOut.uwLowThetaPu = 0;
//    switchhall_stOut.uwSectorNum = 0;
//    switchhall_stOut.uwSectorNumPre = 0;
    switchhall_stOut.slLowThetaPu = 0;
#if((MOTOR_ID_SEL == MOTOR_LUNGU_WELLING_HAL_POLE10_750W_500Ratio) ||(MOTOR_ID_SEL ==MOTOR_LUNGU_WELLING_HAL_POLE10_500W_500Ratio)||(MOTOR_ID_SEL == MOTOR_LUNGU_WELLING_HAL_POLE10_1000W_500Ratio))
    switchhall_stOut.swLowThetaOffsetPu = cof_sl180DegreePu; //-683  -7 degree
#else
    switchhall_stOut.swLowThetaOffsetPu = 0; //-683  -7 degree
#endif
    switchhall_stCap.slCapValueDelta = HALL3_SPD_CAP_LEVEL1;
    switchhall_stCap.ulCapValue = 0;
    switchhall_stCap.ulCapValuePre = 0;
    switchhall_stCap.uwCaptureOverFlowCnt = 0;
}
/***************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
****************************************************************/
static void switchhall_voSectorJudge(void)
{
    UWORD uwsectorNum;
    UWORD Jcnt=0;
    UWORD uwHallSta;
    UWORD uwActtime;
uwsectorNum=0;
//    uint32_t gpioA = DL_GPIO_getEnabledInterruptStatus(GPIOA,
//                                                       HALL_HALLA_PIN | HALL_HALLB_PIN | HALL_HALLC_PIN);
////if((GPIOB->idt & GPIO_PINS_5) ==0)//(Hall_HB_PORT()== 1)//if(iGpio_Read(HW_GPIO_HALLB_PIN) == 1)
//    if ((gpioA & HALL_HALLA_PIN) == 0)
//    {
//        uwsectorNum |= 0x01;
//    }
//
//// if((GPIOB->idt &GPIO_PINS_0) ==0)//(Hall_HA_PORT() == 1) //if(iGpio_Read(HW_GPIO_HALLC_PIN) == 1)
//    if ((gpioA & HALL_HALLB_PIN) ==0 )
//    {
//        uwsectorNum |= 0x02;
//    }
////  if((GPIOB->idt &GPIO_PINS_4) ==0)//(Hall_HC_PORT() == 1) // if(iGpio_Read(HW_GPIO_HALLA_PIN) == 1)
//    if ((gpioA & HALL_HALLC_PIN) == 0)
//    {
//        uwsectorNum |= 0x04;
//    }
//
//    uint32_t gpioA = DL_GPIO_getEnabledInterruptStatus(GPIOA,
//                                                       HALL_HALLA_PIN | HALL_HALLB_PIN | HALL_HALLC_PIN);
    /*
     * Bitwise AND the pending interrupt with the pin you want to check,
     * then check if it is equal to the pins. Clear the interrupt status.
     */
if (Hall_HA_PORT() ==0)// if ((Hall_HA_PORT() &  HALL_HALLA_PIN) ==0)//  if ((gpioA & HALL_HALLA_PIN) == HALL_HALLA_PIN)
    {

 //        HALLACnt++;
        uwsectorNum |= 0x02;
//        DL_GPIO_clearInterruptStatus(GPIOA, HALL_HALLA_PIN);
    }

if (Hall_HB_PORT() ==0) // if ((Hall_HB_PORT() &  HALL_HALLB_PIN) ==0)//  if ((gpioA & HALL_HALLB_PIN) == HALL_HALLB_PIN)
    {

        uwsectorNum |= 0x01;
//        HALLBCnt++;
//        DL_GPIO_clearInterruptStatus(GPIOA, HALL_HALLB_PIN);
    }

if (Hall_HC_PORT() ==0) //if ((Hall_HC_PORT() &  HALL_HALLC_PIN) ==0)// if ((gpioA & HALL_HALLC_PIN) == HALL_HALLC_PIN)
    {
        uwsectorNum |= 0x04;
//        HALLCCnt++;
//        DL_GPIO_clearInterruptStatus(GPIOA, HALL_HALLC_PIN);
    }

//   if((GPIOB->idt & GPIO_PINS_5) !=0)//(Hall_HB_PORT()== 1)//if(iGpio_Read(HW_GPIO_HALLB_PIN) == 1)
//    {
//        uwsectorNum = 0;
//    }
//    else
//    {
//        uwsectorNum = 1;
//    }
//    if((GPIOB->idt &GPIO_PINS_0) !=0)//(Hall_HA_PORT() == 1) //if(iGpio_Read(HW_GPIO_HALLC_PIN) == 1)  
//    {
//        uwsectorNum |= (0 << 1);
//    }
//    else
//    {
//        uwsectorNum |= (1 << 1);
//    }
//    if((GPIOB->idt &GPIO_PINS_4) !=0)//(Hall_HC_PORT() == 1) // if(iGpio_Read(HW_GPIO_HALLA_PIN) == 1)
//    {
//        uwsectorNum |= (0 << 2);
//    }
//    else
//    {
//        uwsectorNum |= (1 << 2);
//    }
//  uwHallSta=uwsectorNum;
//  for(Jcnt=0;Jcnt<=15;Jcnt++)
//  {
////      if((GPIOB->idt & GPIO_PINS_5) ==0)//(Hall_HB_PORT()== 1)//if(iGpio_Read(HW_GPIO_HALLB_PIN) == 1)
//          {
//              uwsectorNum |= 0x01;
//          }
//
////       if((GPIOB->idt &GPIO_PINS_0) ==0)//(Hall_HA_PORT() == 1) //if(iGpio_Read(HW_GPIO_HALLC_PIN) == 1)
//           {
//              uwsectorNum |= 0x02;
//          }
////        if((GPIOB->idt &GPIO_PINS_4) ==0)//(Hall_HC_PORT() == 1) // if(iGpio_Read(HW_GPIO_HALLA_PIN) == 1)
//          {
//              uwsectorNum |= 0x04;
//          }
//
//      if(uwHallSta==uwsectorNum)
//      {
//          uwActtime++;
//      }
//   }
 // if(uwActtime>10)
  {

      switchhall_stOut.uwSectorNum = uwsectorNum;
  }

    
    
}

/***************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
****************************************************************/
SLONG  switchhall_pvt_slLowSpdLpfPu;
SLONG  switchhall_pvt_slHighSpdLpfPu;
SLONG  Speed_buffer[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
SQWORD Speed_fb_sum = 0;
SLONG  Speed_Value = 0;
SLONG  Speed_AvgValue = 0;
SWORD  Speed_sample_index = 0;

SWORD tstDeltaCap;
SWORD tstCap;
SWORD tstFlag = 0;
SLONG tmp_ValueCapDelta = 0;
void   switchhall_voTim3Int(void)
{

    switchhall_stCap.ulCapValue= DL_Timer_getTimerCount(HALLTIMER_INST);
    DL_Timer_setTimerCount(HALLTIMER_INST,0);

    switchhall_stCap.uwCaptureOverFlowCnt = 0;
#if(EMCDEAL_EN!=0)
    if(EcmDeal.EmcModeFlag==FALSE)
#endif
    {
    /*Sector Judgement*/

    switchhall_voSectorJudge();

//    DL_GPIO_togglePins(LED_PORT, LED_LED1_PIN);
    tmp_ValueCapDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60011 + (SLONG)switchhall_stCap.ulCapValue  ; //


        Speed_Value = tmp_ValueCapDelta; //switchhall_stCap.slCapValueDelta;
        Speed_fb_sum -= Speed_buffer[Speed_sample_index];
        Speed_buffer[Speed_sample_index] = Speed_Value;
        Speed_fb_sum += (SQWORD)Speed_Value;
        Speed_AvgValue = (SLONG)((Speed_fb_sum * 341)>>12);
        Speed_sample_index++;
        if (Speed_sample_index >= 12)
            Speed_sample_index = 0;
        switchhall_stCap.slCapValueDelta = Speed_AvgValue;
        switchhall_stCap.ulCapValuePre = switchhall_stCap.ulCapValue;
         EcmDeal.slCapValueDelta=switchhall_stCap.slCapValueDelta;
    }


    //-------------------------------------
    /* Capture Value */
//    if ((tmr_interrupt_flag_get(TMR3, TMR_OVF_FLAG)) == SET)//((TIM_GetITStatus(TIM3, TIM_IT_Update)) == SET)
//    {
//        tstFlag = 1000;
////        if(curSpeed_state.state == StartUp || curSpeed_state.state == Open2Clz || curSpeed_state.state == ClzLoop)
////        {
////        if (tmr_interrupt_flag_get(TMR3, TMR_C1_FLAG) == SET)//  if (TIM_GetITStatus(TIM3, TIM_IT_CC1) == SET)
//          {
//              if(Hall_HC_PORT())// if(GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_6))
//              {
////                 tmr_output_channel_polarity_set(TMR3,TMR_SELECT_CHANNEL_1,TMR_POLARITY_ACTIVE_LOW);//  TIM_OC1PolarityConfig(TIM3,TIM_ICPolarity_Falling);
//              }
//              else
//              {
////                 tmr_output_channel_polarity_set(TMR3,TMR_SELECT_CHANNEL_1,TMR_POLARITY_ACTIVE_HIGH);//  TIM_OC1PolarityConfig(TIM3,TIM_ICPolarity_Rising);
//              }
//
//
//              {
////                  switchhall_stCap.ulSTATE =  tmr_channel_value_get(TMR3,TMR_SELECT_CHANNEL_1);
//
////                  if (switchhall_stCap.ulSTATE > tmr_counter_value_get(TMR3))
//                  {
//                      switchhall_stCap.ulCapValue = switchhall_stCap.ulSTATE;
//                      tmp_ValueCapDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre;  //switchhall_stCap.slCapValueDelta
//                      switchhall_stCap.uwCaptureOverFlowCnt = 1;
//
//                      Speed_Value = tmp_ValueCapDelta; //switchhall_stCap.slCapValueDelta;
//                      Speed_fb_sum -= Speed_buffer[Speed_sample_index];
//                      Speed_buffer[Speed_sample_index] = Speed_Value;
//                      Speed_fb_sum += (SQWORD)Speed_Value;
//                      Speed_AvgValue = (SLONG)((Speed_fb_sum * 341)>>12);
//                      Speed_sample_index++;
//                      if (Speed_sample_index >= 12)
//                          Speed_sample_index = 0;
//                      switchhall_stCap.slCapValueDelta = Speed_AvgValue;
//                      switchhall_stCap.ulCapValuePre = switchhall_stCap.ulCapValue;
//
//                  }
////                  else
//                  {
//                      switchhall_stCap.uwCaptureOverFlowCnt ++;
//                      switchhall_stCap.ulCapValue = switchhall_stCap.ulSTATE;
//                      tmp_ValueCapDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre;  //switchhall_stCap.slCapValueDelta
//                      switchhall_stCap.uwCaptureOverFlowCnt = 0;
//
//                      Speed_Value = tmp_ValueCapDelta; //switchhall_stCap.slCapValueDelta;
//                      Speed_fb_sum -= Speed_buffer[Speed_sample_index];
//                      Speed_buffer[Speed_sample_index] = Speed_Value;
//                      Speed_fb_sum += (SQWORD)Speed_Value;
//                      Speed_AvgValue = (SLONG)((Speed_fb_sum * 341)>>12);
//                      Speed_sample_index++;
//                      if (Speed_sample_index >= 12)
//                          Speed_sample_index = 0;
//                      switchhall_stCap.slCapValueDelta = Speed_AvgValue;
//                      switchhall_stCap.ulCapValuePre = switchhall_stCap.ulCapValue;
//                  }
//              }
//
////            tmr_flag_clear(TMR3, TMR_C1_FLAG);//  TIM_ClearFlag(TIM3, TIM_IT_CC1);
//          }
////          else if (tmr_interrupt_flag_get(TMR3, TMR_C2_FLAG) == SET)
//          {
//              if(Hall_HB_PORT())
//              {
////                  tmr_output_channel_polarity_set(TMR3,TMR_SELECT_CHANNEL_2,TMR_POLARITY_ACTIVE_LOW);
//              }
//              else
//              {
////                   tmr_output_channel_polarity_set(TMR3,TMR_SELECT_CHANNEL_2,TMR_POLARITY_ACTIVE_HIGH);
//              }
//
//
//              {
////                  switchhall_stCap.ulSTATE=tmr_channel_value_get(TMR3,TMR_SELECT_CHANNEL_2);
//
////                  if (switchhall_stCap.ulSTATE > tmr_counter_value_get(TMR3))
//                  {
//                      switchhall_stCap.ulCapValue = switchhall_stCap.ulSTATE;
//                      tmp_ValueCapDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre;  //switchhall_stCap.slCapValueDelta
//                      switchhall_stCap.uwCaptureOverFlowCnt = 1;
//
//                      Speed_Value = tmp_ValueCapDelta; //switchhall_stCap.slCapValueDelta;
//                      Speed_fb_sum -= Speed_buffer[Speed_sample_index];
//                      Speed_buffer[Speed_sample_index] = Speed_Value;
//                      Speed_fb_sum += (SQWORD)Speed_Value;
//                      Speed_AvgValue = (SLONG)((Speed_fb_sum * 341)>>12);
//                      Speed_sample_index++;
//                      if (Speed_sample_index >= 12)
//                          Speed_sample_index = 0;
//                      switchhall_stCap.slCapValueDelta = Speed_AvgValue;
//                      switchhall_stCap.ulCapValuePre = switchhall_stCap.ulCapValue;
//
//                  }
////                  else
//                  {
//                      switchhall_stCap.uwCaptureOverFlowCnt ++;
//                      switchhall_stCap.ulCapValue = switchhall_stCap.ulSTATE;
//                      tmp_ValueCapDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre;  //switchhall_stCap.slCapValueDelta
//                      switchhall_stCap.uwCaptureOverFlowCnt = 0;
//
//                      Speed_Value = tmp_ValueCapDelta; //switchhall_stCap.slCapValueDelta;
//                      Speed_fb_sum -= Speed_buffer[Speed_sample_index];
//                      Speed_buffer[Speed_sample_index] = Speed_Value;
//                      Speed_fb_sum += (SQWORD)Speed_Value;
//                      Speed_AvgValue = (SLONG)((Speed_fb_sum * 341)>>12);
//                      Speed_sample_index++;
//                      if (Speed_sample_index >= 12)
//                          Speed_sample_index = 0;
//                      switchhall_stCap.slCapValueDelta = Speed_AvgValue;
//                      switchhall_stCap.ulCapValuePre = switchhall_stCap.ulCapValue;
//                  }
//              }
//
////              tmr_flag_clear(TMR3, TMR_C2_FLAG);//TIM_ClearFlag(TIM3, TIM_IT_CC2);
//          }
////          else if (tmr_interrupt_flag_get(TMR3, TMR_C3_FLAG) == SET)
//          {
//              if(Hall_HA_PORT())
//              {
////                 tmr_output_channel_polarity_set(TMR3,TMR_SELECT_CHANNEL_3,TMR_POLARITY_ACTIVE_LOW);// TIM_OC3PolarityConfig(TIM3,TIM_ICPolarity_Falling);
//              }
//              else
//              {
////                   tmr_output_channel_polarity_set(TMR3,TMR_SELECT_CHANNEL_3,TMR_POLARITY_ACTIVE_HIGH);// TIM_OC3PolarityConfig(TIM3,TIM_ICPolarity_Rising);
//              }
//
//
//
//              {
////                  switchhall_stCap.ulSTATE =  tmr_channel_value_get(TMR3,TMR_SELECT_CHANNEL_3);
//
////                  if (switchhall_stCap.ulSTATE >tmr_counter_value_get(TMR3))
//                  {
//                      switchhall_stCap.ulCapValue = switchhall_stCap.ulSTATE;
//                      tmp_ValueCapDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre;  //switchhall_stCap.slCapValueDelta
//                      switchhall_stCap.uwCaptureOverFlowCnt = 1;
//
//                      Speed_Value = tmp_ValueCapDelta; //switchhall_stCap.slCapValueDelta;
//                      Speed_fb_sum -= Speed_buffer[Speed_sample_index];
//                      Speed_buffer[Speed_sample_index] = Speed_Value;
//                      Speed_fb_sum += (SQWORD)Speed_Value;
//                      Speed_AvgValue = (SLONG)((Speed_fb_sum * 341)>>12);
//                      Speed_sample_index++;
//                      if (Speed_sample_index >= 12)
//                          Speed_sample_index = 0;
//                      switchhall_stCap.slCapValueDelta = Speed_AvgValue;
//                      switchhall_stCap.ulCapValuePre = switchhall_stCap.ulCapValue;
//
//                  }
////                  else
//                  {
//                      switchhall_stCap.uwCaptureOverFlowCnt ++;
//                      switchhall_stCap.ulCapValue = switchhall_stCap.ulSTATE;
//                      tmp_ValueCapDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre;  //switchhall_stCap.slCapValueDelta
//                      switchhall_stCap.uwCaptureOverFlowCnt = 0;
//
//                      Speed_Value = tmp_ValueCapDelta; //switchhall_stCap.slCapValueDelta;
//                      Speed_fb_sum -= Speed_buffer[Speed_sample_index];
//                      Speed_buffer[Speed_sample_index] = Speed_Value;
//                      Speed_fb_sum += (SQWORD)Speed_Value;
//                      Speed_AvgValue = (SLONG)((Speed_fb_sum * 341)>>12);
//                      Speed_sample_index++;
//                      if (Speed_sample_index >= 12)
//                          Speed_sample_index = 0;
//                      switchhall_stCap.slCapValueDelta = Speed_AvgValue;
//                      switchhall_stCap.ulCapValuePre = switchhall_stCap.ulCapValue;
//                  }
//              }
//
////             tmr_flag_clear(TMR3, TMR_C3_FLAG);//  TIM_ClearFlag(TIM3, TIM_IT_CC3);
//          }
////          else
//          {
//              switchhall_stCap.ulSTATE=0;
//              switchhall_stCap.uwCaptureOverFlowCnt ++;
//
//              if(switchhall_stCap.uwCaptureOverFlowCnt >= 5)
//              {
//                  tmp_ValueCapDelta = (SLONG)(switchhall_stCap.uwCaptureOverFlowCnt+1) * 60000; //switchhall_stCap.slCapValueDelta
//                  switchhall_stCap.slCapValueDelta = tmp_ValueCapDelta;
//              }
//          }
//
//          if (switchhall_stCap.uwCaptureOverFlowCnt > 9)  //1HZ
//          {
//            /* Linmit the max  CapValueDelta */
//            switchhall_stCap.uwCaptureOverFlowCnt=10;
//          }
////        }
////        else
////        {
////          switchhall_stCap.uwCaptureOverFlowCnt = 0;
////        }
////       tmr_flag_clear(TMR3, TMR_OVF_FLAG);//  TIM_ClearFlag(TIM3, TIM_IT_Update);
//
//    }
////    else if((tmr_interrupt_flag_get(TMR3, TMR_C1_FLAG)) == SET)
//    {
//        tstFlag = 2000;
//        if(Hall_HC_PORT())
//        {
////           tmr_output_channel_polarity_set(TMR3,TMR_SELECT_CHANNEL_1,TMR_POLARITY_ACTIVE_LOW);//  TIM_OC1PolarityConfig(TIM3,TIM_ICPolarity_Falling);
//        }
//        else
//        {
////          tmr_output_channel_polarity_set(TMR3,TMR_SELECT_CHANNEL_1,TMR_POLARITY_ACTIVE_HIGH);//  TIM_OC1PolarityConfig(TIM3,TIM_ICPolarity_Rising);
//        }
//
//
//        {
////            switchhall_stCap.ulCapValue = tmr_channel_value_get(TMR3,TMR_SELECT_CHANNEL_1);
//            tmp_ValueCapDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre; //switchhall_stCap.slCapValueDelta
//            switchhall_stCap.uwCaptureOverFlowCnt = 0;
//
//            Speed_Value = tmp_ValueCapDelta; //switchhall_stCap.slCapValueDelta;
//            Speed_fb_sum -= Speed_buffer[Speed_sample_index];
//            Speed_buffer[Speed_sample_index] = Speed_Value;
//            Speed_fb_sum += (SQWORD)Speed_Value;
//            Speed_AvgValue = (SLONG)((Speed_fb_sum * 341)>>12);
//            Speed_sample_index++;
//            if (Speed_sample_index >= 12)
//                Speed_sample_index = 0;
//            switchhall_stCap.slCapValueDelta = Speed_AvgValue;
//            switchhall_stCap.ulCapValuePre = switchhall_stCap.ulCapValue;
//        }
//
////       tmr_flag_clear(TMR3, TMR_C1_FLAG);// TIM_ClearFlag(TIM3, TIM_IT_CC1);
//    }
////    else if((tmr_interrupt_flag_get(TMR3, TMR_C2_FLAG) )  == SET)
//    {
//        tstFlag = 3000;
//        if(Hall_HB_PORT())
//        {
////            tmr_output_channel_polarity_set(TMR3,TMR_SELECT_CHANNEL_2,TMR_POLARITY_ACTIVE_LOW);
//        }
//        else
//        {
////           tmr_output_channel_polarity_set(TMR3,TMR_SELECT_CHANNEL_2,TMR_POLARITY_ACTIVE_HIGH);
//        }
//
//
//        {
////            switchhall_stCap.ulCapValue =tmr_channel_value_get(TMR3,TMR_SELECT_CHANNEL_2);
//            tmp_ValueCapDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre; //switchhall_stCap.slCapValueDelta
//            switchhall_stCap.uwCaptureOverFlowCnt = 0;
//
//            Speed_Value = tmp_ValueCapDelta; //switchhall_stCap.slCapValueDelta;
//            Speed_fb_sum -= Speed_buffer[Speed_sample_index];
//            Speed_buffer[Speed_sample_index] = Speed_Value;
//            Speed_fb_sum += (SQWORD)Speed_Value;
//            Speed_AvgValue = (SLONG)((Speed_fb_sum * 341)>>12);
//            Speed_sample_index++;
//            if (Speed_sample_index >= 12)
//                Speed_sample_index = 0;
//            switchhall_stCap.slCapValueDelta = Speed_AvgValue;
//            switchhall_stCap.ulCapValuePre = switchhall_stCap.ulCapValue;
//        }
//
////         tmr_flag_clear(TMR3, TMR_C2_FLAG);
//    }
////    else if ((tmr_interrupt_flag_get(TMR3, TMR_C3_FLAG)) == SET)
//    {
//
//       tstFlag = 4000;
//         /* Low Speed*/
//       if(Hall_HA_PORT())
//        {
////           tmr_output_channel_polarity_set(TMR3,TMR_SELECT_CHANNEL_3,TMR_POLARITY_ACTIVE_LOW);
//        }
//        else
//        {
////            tmr_output_channel_polarity_set(TMR3,TMR_SELECT_CHANNEL_3,TMR_POLARITY_ACTIVE_HIGH);
//        }
//
//
//        {
////            switchhall_stCap.ulCapValue = tmr_channel_value_get(TMR3,TMR_SELECT_CHANNEL_3);
//            tmp_ValueCapDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre; //switchhall_stCap.slCapValueDelta
//            switchhall_stCap.uwCaptureOverFlowCnt = 0;
//    //        /* High Speed*/
//    //        switchhall_stCap.ulHighCapValue = switchhall_stCap.ulCapValue;
//    //        switchhall_stCap.slHighCapValueDelta = (SLONG)switchhall_stCap.ulHighCapValue - switchhall_stCap.ulHighCapValuePre;
//    //        if(switchhall_stCap.slHighCapValueDelta < 0)
//    //        {
//    //            switchhall_stCap.slHighCapValueDelta += 60000;
//    //        }
//                /* Move Average Filter */
//
//            Speed_Value = tmp_ValueCapDelta; //switchhall_stCap.slCapValueDelta;
//            Speed_fb_sum -= Speed_buffer[Speed_sample_index];
//            Speed_buffer[Speed_sample_index] = Speed_Value;
//            Speed_fb_sum += (SQWORD)Speed_Value;
//            Speed_AvgValue = (SLONG)((Speed_fb_sum * 341)>>12);
//            Speed_sample_index++;
//            if (Speed_sample_index >= 12)
//                Speed_sample_index = 0;
//            switchhall_stCap.slCapValueDelta = Speed_AvgValue;
//            switchhall_stCap.ulCapValuePre = switchhall_stCap.ulCapValue;
//        }
////         tmr_flag_clear(TMR3, TMR_C3_FLAG);
//    }
////    else
//    {
//       tstFlag = 5000;
////        tmp_ValueCapDelta = HALL3_SPD_CAP_LEVEL1;  //switchhall_stCap.slCapValueDelta
//    }
//
//
////    if(curSpeed_state.state ==StartUp || curSpeed_state.state == Open2Clz || curSpeed_state.state == ClzLoop)
////    {
////      if ((switchhall_stOut.swLowSpdLpfPu > 1950)||(switchhall_stOut.swLowSpdLpfPu < -1950))//500rpm
////      {
////          switchhall_stCap.slCapValueDelta = Speed_AvgValue;
////      }
////      else
//      //{
////          switchhall_stCap.slCapValueDelta = Speed_Value;
////      }
//      
////    }
////    else
////    {
////      switchhall_stCap.slCapValueDelta = HALL3_SPD_CAL_COEF;
////
////    }
    tstCap = switchhall_stCap.ulCapValue >> 4;
    tstDeltaCap = switchhall_stCap.slCapValueDelta >> 4;
}

/***************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
****************************************************************/
//void switchhall_voCapIsr(UWORD source)
//{
//    if(source == 0)
//    {      
//        /* 鏇存柊涓柇澶勭悊 */
//        switchhall_stCap.uwCaptureOverFlowCnt ++;
//        /* 閫熷害鎻掑�硷細澶勭悊闀夸簨浠舵棤鑴夊啿鎯呭喌 */
//        if(switchhall_stCap.uwCaptureOverFlowCnt >= 5)
//        {
//            tmp_ValueCapDelta = (SLONG)(switchhall_stCap.uwCaptureOverFlowCnt+1) * 60000; //switchhall_stCap.slCapValueDelta
//            switchhall_stCap.slCapValueDelta = tmp_ValueCapDelta;
//        }
//        /* 闄愬埗鏈�澶у�� */
//        if (switchhall_stCap.uwCaptureOverFlowCnt > 9)  
//        {
//            switchhall_stCap.uwCaptureOverFlowCnt=10;
//        }
//    }
//    else 
//    {
//        /* 鎹曡幏涓柇澶勭悊 */
//        /* 鏇存敼鎹曡幏瑙﹀彂杈规部 */
////        if(iGpio_Read(HW_GPIO_HALLA_PIN + source - 1) != 0)
////        {
////            iCap_SetEdgeType(HW_MOTOR_HALL_CAP, CAP_CH(source-1), ApiCap_FallingEdge);
////        }
////        else
////        {
////            iCap_SetEdgeType(HW_MOTOR_HALL_CAP, CAP_CH(source-1), ApiCap_RisingEdge);
////        }
//        /* 璁＄畻鎹曡幏宸�� */
//        switchhall_stCap.ulCapValue = iCap_GetCaptureValue(HW_MOTOR_HALL_CAP, CAP_CH(source-1));      
//        tmp_ValueCapDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre; //switchhall_stCap.slCapValueDelta
//        switchhall_stCap.uwCaptureOverFlowCnt = 0;
//        /* 婊戝姩骞冲潎婊ゆ尝 */
//        Speed_Value = tmp_ValueCapDelta; 
//        Speed_fb_sum -= Speed_buffer[Speed_sample_index];
//        Speed_buffer[Speed_sample_index] = Speed_Value;
//        Speed_fb_sum += (SQWORD)Speed_Value;
//        Speed_AvgValue = (SLONG)((Speed_fb_sum * 341)>>12);
//        Speed_sample_index++;
//        if (Speed_sample_index >= 12)
//            Speed_sample_index = 0;
//        switchhall_stCap.slCapValueDelta = Speed_AvgValue;
//        switchhall_stCap.ulCapValuePre = switchhall_stCap.ulCapValue;
//    }
//}

/***************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
****************************************************************/
void switchhall_voPosCalTbc(void)
{
    SLONG Hall_Theta_Temp = 0;
    SLONG Hall_Speed_Temp = 0;
    /*Low Speed Theta Update*/ 
    switchhall_stOut.slLowThetaPu += (((SLONG)TBC_TM * switchhall_stOut.swLowSpdLpfPu) >> 10); // Q10+Q15-Q10=Q15 
    //switchhall_stOut.slLowThetaPu += (((SLONG)TBC_TM * scm_stSpdFbkLpf.slY.sw.hi) >> 10); // Q10+Q15-Q10=Q15 
    if (switchhall_stOut.slLowThetaPu >= (cof_sl360DegreePu + switchhall_stOut.swLowThetaOffsetPu))
    {
        switchhall_stOut.slLowThetaPu -= cof_sl360DegreePu;
    }
    else if (switchhall_stOut.slLowThetaPu < 0 + switchhall_stOut.swLowThetaOffsetPu)
    {
        switchhall_stOut.slLowThetaPu += cof_sl360DegreePu;
    }
    
    /*High Speed Theta Update*/ 
//     switchhall_stOut.slHighThetaPu += (((SLONG)TBC_TM * switchhall_stOut.swHighSpdLpfPu) >> 10); // Q10+Q15-Q10=Q15 
//    if (switchhall_stOut.slHighThetaPu >= cof_sl360DegreePu + switchhall_stOut.swLowThetaOffsetPu)
//    {
//        switchhall_stOut.slHighThetaPu -= cof_sl360DegreePu;
//    }
//    else if (switchhall_stOut.slHighThetaPu < switchhall_stOut.swLowThetaOffsetPu)
//    {
//        switchhall_stOut.slHighThetaPu += cof_sl360DegreePu;
//    } 
    
    /*Sector Judgement*/
#if(EMCDEAL_EN==0)
    switchhall_voSectorJudge();
#endif
    /*!< Zero Correction 1*/
    //    switch(switchhall_stOut.uwSectorNum)
    //    {
    //    case 1:
    //      if(switchhall_stOut.uwSectorNumPre == 3)
    //      {
    //          switchhall_stOut.swSpdCWorCCW = 1;
    //            switchhall_stOut.slLowThetaPu = cof_sl210DegreePu;
    //      }
    //      else if(switchhall_stOut.uwSectorNumPre == 5)
    //      {
    //         switchhall_stOut.swSpdCWorCCW =-1;
    //           switchhall_stOut.slLowThetaPu = cof_sl270DegreePu;
    //      }
    //
    //      switchhall_stOut.slInitThetaPu = cof_sl240DegreePu;
    //      if(switchhall_stOut.slLowThetaPu < cof_sl210DegreePu)
    //      {
    //          switchhall_stOut.slLowThetaPu = cof_sl210DegreePu;
    //      }
    //      else if(switchhall_stOut.slLowThetaPu > cof_sl270DegreePu)
    //      {
    //          switchhall_stOut.slLowThetaPu = cof_sl270DegreePu;
    //      }
    //      else
    //      {
    //      }
    //      break;
    //    case 2:
    //      if(switchhall_stOut.uwSectorNumPre == 6)
    //      {
    //          switchhall_stOut.swSpdCWorCCW = 1;
    //          switchhall_stOut.slLowThetaPu = cof_sl90DegreePu;
    //      }
    //      else if(switchhall_stOut.uwSectorNumPre == 3)
    //      {
    //         switchhall_stOut.swSpdCWorCCW =-1;
    //         switchhall_stOut.slLowThetaPu = cof_sl150DegreePu;
    //      }
    //
    //      switchhall_stOut.slInitThetaPu = cof_sl120DegreePu;
    //      if(switchhall_stOut.slLowThetaPu < cof_sl90DegreePu)
    //      {
    //          switchhall_stOut.slLowThetaPu = cof_sl90DegreePu;
    //      }
    //      else if(switchhall_stOut.slLowThetaPu > cof_sl150DegreePu)
    //      {
    //          switchhall_stOut.slLowThetaPu = cof_sl150DegreePu;
    //      }
    //      else
    //      {
    //      }
    //      break;
    //    case 3:
    //      if(switchhall_stOut.uwSectorNumPre == 2)
    //      {
    //          switchhall_stOut.swSpdCWorCCW = 1;
    //          switchhall_stOut.slLowThetaPu = cof_sl150DegreePu;
    //      }
    //      else if(switchhall_stOut.uwSectorNumPre == 1)
    //      {
    //         switchhall_stOut.swSpdCWorCCW =-1;
    //         switchhall_stOut.slLowThetaPu = cof_sl210DegreePu;
    //      }
    //
    //      switchhall_stOut.slInitThetaPu = cof_sl180DegreePu;
    //      if(switchhall_stOut.slLowThetaPu < cof_sl150DegreePu)
    //      {
    //          switchhall_stOut.slLowThetaPu = cof_sl150DegreePu;
    //      }
    //      else if(switchhall_stOut.slLowThetaPu > cof_sl210DegreePu)
    //      {
    //          switchhall_stOut.slLowThetaPu = cof_sl210DegreePu;
    //      }
    //      else
    //      {
    //      }
    //      break;
    //    case 4:
    //      if(switchhall_stOut.uwSectorNumPre == 5)
    //      {
    //          switchhall_stOut.swSpdCWorCCW = 1;
    //          switchhall_stOut.slLowThetaPu = cof_sl330DegreePu;
    //      }
    //      else if(switchhall_stOut.uwSectorNumPre == 6)
    //      {
    //         switchhall_stOut.swSpdCWorCCW =-1;
    //         switchhall_stOut.slLowThetaPu = cof_sl30DegreePu;
    //      }
    //
    //      switchhall_stOut.slInitThetaPu = 0;
    //      if((switchhall_stOut.slLowThetaPu > cof_sl30DegreePu)&&(switchhall_stOut.slLowThetaPu < cof_sl330DegreePu))
    //      {
    //          if(switchhall_stOut.swSpdCWorCCW == 1)
    //          {
    //              switchhall_stOut.slLowThetaPu = cof_sl30DegreePu;
    //          }
    //          else if(switchhall_stOut.swSpdCWorCCW == -1)
    //          {
    //              switchhall_stOut.slLowThetaPu = cof_sl330DegreePu;
    //          }
    //      }
    //      break;
    //    case 5:
    //      if(switchhall_stOut.uwSectorNumPre == 1)
    //      {
    //          switchhall_stOut.swSpdCWorCCW = 1;
    //          switchhall_stOut.slLowThetaPu = cof_sl270DegreePu;
    //      }
    //      else if(switchhall_stOut.uwSectorNumPre == 4)
    //      {
    //         switchhall_stOut.swSpdCWorCCW =-1;
    //         switchhall_stOut.slLowThetaPu = cof_sl330DegreePu;
    //      }
    //
    //      switchhall_stOut.slInitThetaPu = cof_sl300DegreePu;
    //      if(switchhall_stOut.slLowThetaPu < cof_sl270DegreePu)
    //      {
    //          switchhall_stOut.slLowThetaPu = cof_sl270DegreePu;
    //      }
    //      else if(switchhall_stOut.slLowThetaPu > cof_sl330DegreePu)
    //      {
    //          switchhall_stOut.slLowThetaPu = cof_sl330DegreePu;
    //      }
    //      else
    //      {
    //      }
    //      break;
    //    case 6:
    //      if(switchhall_stOut.uwSectorNumPre == 4)
    //      {
    //          switchhall_stOut.swSpdCWorCCW = 1;
    //          switchhall_stOut.slLowThetaPu = cof_sl30DegreePu;
    //      }
    //      else if(switchhall_stOut.uwSectorNumPre == 2)
    //      {
    //         switchhall_stOut.swSpdCWorCCW = -1;
    //         switchhall_stOut.slLowThetaPu = cof_sl90DegreePu;
    //      }
    //
    //      switchhall_stOut.slInitThetaPu = cof_sl60DegreePu;
    //      if(switchhall_stOut.slLowThetaPu < cof_sl30DegreePu)
    //      {
    //          switchhall_stOut.slLowThetaPu = cof_sl30DegreePu;
    //      }
    //      else if(switchhall_stOut.slLowThetaPu > cof_sl90DegreePu)
    //      {
    //          switchhall_stOut.slLowThetaPu = cof_sl90DegreePu;
    //      }
    //      else
    //      {
    //      }
    //      break;
    //    default:
    //      switchhall_stOut.swSpdCWorCCW =0;
    //      switchhall_stOut.swLowSpdPu = 0;
    //      break;
    //    }
    //
    //

    /*!< Zero Correction 2*/
    switch (switchhall_stOut.uwSectorNum)
    {
    case 1:
        if (switchhall_stOut.uwSectorNumPre == 3)
        {
            switchhall_stOut.swSpdCWorCCW = 1;
            switchhall_stOut.slLowThetaPu = cof_sl30DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else if (switchhall_stOut.uwSectorNumPre == 5)
        {
            switchhall_stOut.swSpdCWorCCW = -1;
            switchhall_stOut.slLowThetaPu = cof_sl90DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }

        switchhall_stOut.slInitThetaPu = cof_sl60DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        
        if (switchhall_stOut.slLowThetaPu < (cof_sl30DegreePu + switchhall_stOut.swLowThetaOffsetPu))
        {
            switchhall_stOut.slLowThetaPu = cof_sl30DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else if (switchhall_stOut.slLowThetaPu > (cof_sl90DegreePu + switchhall_stOut.swLowThetaOffsetPu))
        {
            switchhall_stOut.slLowThetaPu = cof_sl90DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else
        {}
        break;
    case 2:
        if (switchhall_stOut.uwSectorNumPre == 6)
        {
            switchhall_stOut.swSpdCWorCCW = 1;
            switchhall_stOut.slLowThetaPu = cof_sl270DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else if (switchhall_stOut.uwSectorNumPre == 3)
        {
            switchhall_stOut.swSpdCWorCCW = -1;
            switchhall_stOut.slLowThetaPu = cof_sl330DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }

        switchhall_stOut.slInitThetaPu = cof_sl300DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        if (switchhall_stOut.slLowThetaPu < (cof_sl270DegreePu+ switchhall_stOut.swLowThetaOffsetPu))
        {
            switchhall_stOut.slLowThetaPu = cof_sl270DegreePu + switchhall_stOut.swLowThetaOffsetPu ;
        }
        else if (switchhall_stOut.slLowThetaPu > (cof_sl330DegreePu + switchhall_stOut.swLowThetaOffsetPu))
        {
            switchhall_stOut.slLowThetaPu = cof_sl330DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else
        {}
        break;
    case 3:
        if (switchhall_stOut.uwSectorNumPre == 2)
        {
            switchhall_stOut.swSpdCWorCCW = 1;
            switchhall_stOut.slLowThetaPu = cof_sl330DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else if (switchhall_stOut.uwSectorNumPre == 1)
        {
            switchhall_stOut.swSpdCWorCCW = -1;
            switchhall_stOut.slLowThetaPu = cof_sl30DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        switchhall_stOut.slInitThetaPu = cof_sl360DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        if ((switchhall_stOut.slLowThetaPu > (cof_sl30DegreePu + switchhall_stOut.swLowThetaOffsetPu)) && (switchhall_stOut.slLowThetaPu < (cof_sl330DegreePu + switchhall_stOut.swLowThetaOffsetPu)))
        {
            if (switchhall_stOut.swSpdCWorCCW == 1)
            {
                switchhall_stOut.slLowThetaPu = cof_sl30DegreePu + switchhall_stOut.swLowThetaOffsetPu ;
            }
            else if (switchhall_stOut.swSpdCWorCCW == -1)
            {
                switchhall_stOut.slLowThetaPu = cof_sl330DegreePu + switchhall_stOut.swLowThetaOffsetPu;
            }
        }

        break;
    case 4:
        if (switchhall_stOut.uwSectorNumPre == 5)
        {
            switchhall_stOut.swSpdCWorCCW = 1;
            switchhall_stOut.slLowThetaPu = cof_sl150DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else if (switchhall_stOut.uwSectorNumPre == 6)
        {
            switchhall_stOut.swSpdCWorCCW = -1;
            switchhall_stOut.slLowThetaPu = cof_sl210DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }

        switchhall_stOut.slInitThetaPu = cof_sl180DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        if (switchhall_stOut.slLowThetaPu < (cof_sl150DegreePu + switchhall_stOut.swLowThetaOffsetPu))
        {
            switchhall_stOut.slLowThetaPu = cof_sl150DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else if (switchhall_stOut.slLowThetaPu > (cof_sl210DegreePu + switchhall_stOut.swLowThetaOffsetPu))
        {
            switchhall_stOut.slLowThetaPu = cof_sl210DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else
        {}
        break;
    case 5:
        if (switchhall_stOut.uwSectorNumPre == 1)
        {
            switchhall_stOut.swSpdCWorCCW = 1;
            switchhall_stOut.slLowThetaPu = cof_sl90DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else if (switchhall_stOut.uwSectorNumPre == 4)
        {
            switchhall_stOut.swSpdCWorCCW = -1;
            switchhall_stOut.slLowThetaPu = cof_sl150DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }

        switchhall_stOut.slInitThetaPu = cof_sl120DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        if (switchhall_stOut.slLowThetaPu < (cof_sl90DegreePu + switchhall_stOut.swLowThetaOffsetPu))
        {
            switchhall_stOut.slLowThetaPu = cof_sl90DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else if (switchhall_stOut.slLowThetaPu > (cof_sl150DegreePu + switchhall_stOut.swLowThetaOffsetPu))
        {
            switchhall_stOut.slLowThetaPu = cof_sl150DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else
        {}
        break;
    case 6:
        if (switchhall_stOut.uwSectorNumPre == 4)
        {
            switchhall_stOut.swSpdCWorCCW = 1;
            switchhall_stOut.slLowThetaPu = cof_sl210DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else if (switchhall_stOut.uwSectorNumPre == 2)
        {
            switchhall_stOut.swSpdCWorCCW = -1;
            switchhall_stOut.slLowThetaPu = cof_sl270DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }

        switchhall_stOut.slInitThetaPu = cof_sl240DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        if (switchhall_stOut.slLowThetaPu < (cof_sl210DegreePu + switchhall_stOut.swLowThetaOffsetPu))
        {
            switchhall_stOut.slLowThetaPu = cof_sl210DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else if (switchhall_stOut.slLowThetaPu > (cof_sl270DegreePu + switchhall_stOut.swLowThetaOffsetPu))
        {
            switchhall_stOut.slLowThetaPu = cof_sl270DegreePu + switchhall_stOut.swLowThetaOffsetPu;
        }
        else
        {}
        break;
    default:
//        switchhall_stOut.swSpdCWorCCW = 0;
//        switchhall_stOut.swLowSpdPu = 0;
        break;
    }
    
    switchhall_stOut.uwSectorNumPre = switchhall_stOut.uwSectorNum;

    /* Theta Compensation */
  //     if (switchhall_stOut.swSpdCWorCCW == -1)
  //    {
  //      switchhall_stOut.swLowThetaOffsetPu = switchhall_stOut.swLowThetaOffsetPu + HALL_THETEA_OFFSET_FANXIANG;
  //    }

//
//    /* Speed Calculation */
//    if (switchhall_stOut.swSpdCWorCCW == 1)
//    {
//        switchhall_stOut.swLowSpdPu = (SWORD)(HALL3_SPD_CAL_COEF / switchhall_stCap.slCapValueDelta);
//    }
//    else if (switchhall_stOut.swSpdCWorCCW == -1)
//    {
//        switchhall_stOut.swLowSpdPu = -(SWORD)(HALL3_SPD_CAL_COEF / switchhall_stCap.slCapValueDelta);
//    }
//    else
//    {
//        switchhall_stOut.swLowSpdPu = 0;
//    }
    
    if (switchhall_stCap.slCapValueDelta >= HALL3_SPD_CAP_LEVEL1)   //lower than level1
    {
//        switchhall_stOut.swLowSpdPu = 0;       
//        switchhall_stOut.uwLowThetaPu = switchhall_stOut.slInitThetaPu;
//        Hall_Theta_Temp = (UWORD)(switchhall_stOut.slInitThetaPu+switchhall_stOut.swLowThetaOffsetPu); // Q15
        
        Hall_Theta_Temp = switchhall_stOut.slInitThetaPu;
//        Hall_Theta_Temp = switchhall_stOut.slLowThetaPu;
        if (Hall_Theta_Temp >= cof_sl360DegreePu)
        {
            Hall_Theta_Temp -= cof_sl360DegreePu;
        }
        else if (Hall_Theta_Temp < 0)
        {
            Hall_Theta_Temp += cof_sl360DegreePu;
        }
        switchhall_stOut.uwLowThetaPu = (UWORD)Hall_Theta_Temp;        
        
        if (switchhall_stOut.swSpdCWorCCW == 1)
        {
            switchhall_stOut.swLowSpdPu = (SWORD)(HALL3_SPD_CAL_COEF / switchhall_stCap.slCapValueDelta);
            switchhall_stOut.swHighSpdPu = (SWORD)(HALL1_SPD_CAL_COEF / switchhall_stCap.slHighCapValueDelta);
//            switchhall_stOut.swLowSpdPu = (SWORD)(HALL3_SPD_CAL_COEF / tmp_ValueCapDelta);
        }
        else if (switchhall_stOut.swSpdCWorCCW == -1)
        {
            switchhall_stOut.swLowSpdPu = -(SWORD)(HALL3_SPD_CAL_COEF / switchhall_stCap.slCapValueDelta);
            switchhall_stOut.swHighSpdPu = -(SWORD)(HALL1_SPD_CAL_COEF / switchhall_stCap.slHighCapValueDelta);
//            switchhall_stOut.swLowSpdPu = -(SWORD)(HALL3_SPD_CAL_COEF / tmp_ValueCapDelta);
        }
        else
        {
//            switchhall_stOut.swLowSpdPu = 0;
        }
    }
    else if (switchhall_stCap.slCapValueDelta >= HALL3_SPD_CAP_LEVEL2)   // lower than level2 
    {
        /* Use fixed angle(middle of sector) at low speed*/
        if (switchhall_stCap.slCapValueDelta >= HALL3_SPD_CAP_STARTUP_LEVEL) // lower than Startup level
        {
            // switchhall_stOut.uwLowThetaPu = switchhall_stOut.slInitThetaPu; 
            //Hall_Theta_Temp = (UWORD)(switchhall_stOut.slInitThetaPu+switchhall_stOut.swLowThetaOffsetPu); // Q15
            
//            Hall_Theta_Temp = switchhall_stOut.slInitThetaPu;
            Hall_Theta_Temp = switchhall_stOut.slLowThetaPu;
            if (Hall_Theta_Temp >= cof_sl360DegreePu)
            {
                Hall_Theta_Temp -= cof_sl360DegreePu;
            }
            else if (Hall_Theta_Temp < 0)
            {
                Hall_Theta_Temp += cof_sl360DegreePu;
            }
            switchhall_stOut.uwLowThetaPu = (UWORD)Hall_Theta_Temp;                
        }
        else
        {
            /*Low Speed Theta */
            //Hall_Theta_Temp = (UWORD)(switchhall_stOut.slLowThetaPu+switchhall_stOut.swLowThetaOffsetPu); // Q15
            Hall_Theta_Temp = switchhall_stOut.slLowThetaPu;
            if (Hall_Theta_Temp >= cof_sl360DegreePu)
            {
                Hall_Theta_Temp -= cof_sl360DegreePu;
            }
            else if (Hall_Theta_Temp < 0)
            {
                Hall_Theta_Temp += cof_sl360DegreePu;
            }
            switchhall_stOut.uwLowThetaPu = (UWORD)Hall_Theta_Temp;     
            
            /*High Speed Theta */
//            if (switchhall_stOut.slHighThetaPu >= cof_sl360DegreePu)
//            {
//                switchhall_stOut.slHighThetaPu -= cof_sl360DegreePu;
//            }
//            else if (switchhall_stOut.slHighThetaPu < 0)
//            {
//                switchhall_stOut.slHighThetaPu += cof_sl360DegreePu;
//            }
//            switchhall_stOut.uwHighThetaPu = cof_sl360DegreePu;
        }
        
        Hall_Speed_Temp = (SWORD)(HALL3_SPD_CAL_COEF / switchhall_stCap.slCapValueDelta);
//        Hall_Speed_Temp = (SWORD)(HALL3_SPD_CAL_COEF / tmp_ValueCapDelta);
        if (switchhall_stOut.swSpdCWorCCW == 1)
        {
            if(switchhall_stOut.swLowSpdPu < 0)
            {
                switchhall_stOut.swLowSpdPu += (_IQ15(0.0007));   //1hz
            }
            else
            {
                switchhall_stOut.swLowSpdPu = Hall_Speed_Temp;
            }
            
            switchhall_stOut.swHighSpdPu = (SWORD)(HALL1_SPD_CAL_COEF / switchhall_stCap.slHighCapValueDelta);
        }
        else if (switchhall_stOut.swSpdCWorCCW == -1)
        {
            if(switchhall_stOut.swLowSpdPu > 0)
            {
                switchhall_stOut.swLowSpdPu -= (_IQ15(0.0007));   //1hz
            }
            else
            {
                switchhall_stOut.swLowSpdPu = -Hall_Speed_Temp;
            }

            switchhall_stOut.swHighSpdPu = -(SWORD)(HALL1_SPD_CAL_COEF / switchhall_stCap.slHighCapValueDelta);
        }
        else
        {
//            switchhall_stOut.swLowSpdPu = 0;
        }
    }
    else
    {
        // High speed: use single hall
    }
#if(EMCDEAL_EN!=0)
   // switchhall_stOut.swLowSpdPu=EcmDeal.swLowSpdPu;
#endif
    /* Low Speed LPF */
    switchhall_pvt_slLowSpdLpfPu =
        (SLONG)203 * (switchhall_stOut.swLowSpdPu - switchhall_stOut.swLowSpdLpfPu) + switchhall_pvt_slLowSpdLpfPu; //50Hz
//    switchhall_pvt_slLowSpdLpfPu =
//        (SLONG)1286 * (switchhall_stOut.swLowSpdPu - switchhall_stOut.swLowSpdLpfPu) + switchhall_pvt_slLowSpdLpfPu; //50Hz
    switchhall_stOut.swLowSpdLpfPu = switchhall_pvt_slLowSpdLpfPu >> 15;
    
    /* High Speed LPF */
//    switchhall_pvt_slHighSpdLpfPu =
//    (SLONG)0x00CB * (switchhall_stOut.swHighSpdPu - switchhall_stOut.swHighSpdLpfPu) + switchhall_pvt_slHighSpdLpfPu; 
//    switchhall_stOut.swHighSpdLpfPu = switchhall_pvt_slHighSpdLpfPu >> 15;
}
UWORD Hcnt;
void HALLTIMER_INST_IRQHandler(void) //void TMR16_GLOBAL_IRQHandler(void)
{

   switch (DL_Timer_getPendingInterrupt(HALLTIMER_INST))
   {
       case DL_TIMER_IIDX_ZERO:
#if(EMCDEAL_EN!=0)
          if( EcmDeal.EmcModeFlag==TRUE)
           switchhall_stCap.slCapValueDelta= EcmDeal.slCapValueDelta;
          else
#endif
          {
           switchhall_voSectorJudge();
           Hcnt++;
           switchhall_stCap.uwCaptureOverFlowCnt ++;
           if(switchhall_stCap.uwCaptureOverFlowCnt >= 5)
            {
                tmp_ValueCapDelta = (SLONG)(switchhall_stCap.uwCaptureOverFlowCnt+1) * 60011; //switchhall_stCap.slCapValueDelta
                switchhall_stCap.slCapValueDelta = tmp_ValueCapDelta;
            }

           if (switchhall_stCap.uwCaptureOverFlowCnt > 9)  //1HZ
            {
              /* Linmit the max  CapValueDelta */
              switchhall_stCap.uwCaptureOverFlowCnt=10;
            }
          }
           break;
       default:
           break;
   }

}


/*************************************************************************
 End of this File (EOF)!
 Do not put anything after this part!
*************************************************************************/