motor_ctl-4/User project/3.BasicFunction/Source/switchhall.c

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

/************************************************************************
 Beginning of File, do not put anything above here except notes
 Compiler Directives:
*************************************************************************/
#ifdef RUN_ARCH_SIM
#include "switchhall.h"

SWITCHHALL_OUT switchhall_stOut = SWITCHHALL_OUT_DEFAULT;

void switchhall_voInit(void)
{}
void switchhall_voTim3Int(void)
{}
void switchhall_voPosCalTbc(void)
{}
#else
#include "gd32f30x.h"
#include "typedefine.h"
#include "syspar.h"
#include "hwsetup.h"
#include "glbcof.h"
#include "switchhall.h"
#include "spdctrFSM.h"
#include "spdctrmode.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
static 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;
    switchhall_stOut.swLowThetaOffsetPu =-15000;

    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)
{
    /* HALLA(PC8), HALLB(PC7), PHALLC(PC6), wzy */
  
//    if(iGpio_Read(HW_GPIO_HALL_A_PIN) != 0)
//    {
//      switchhall_stOut.uwSectorNum = 0;
//    }
//    else
//    {
//      switchhall_stOut.uwSectorNum = 1;
//    }
//    if(iGpio_Read(HW_GPIO_HALL_B_PIN) != 0)
//    {
//      switchhall_stOut.uwSectorNum |= 0<<1;
//    }
//    else
//    {
//      switchhall_stOut.uwSectorNum |= 1<<1;
//    }
//    if(iGpio_Read(HW_GPIO_HALL_C_PIN) != 0)
//    {
//      switchhall_stOut.uwSectorNum |= 0<<2;
//    }
//    else
//    {
//      switchhall_stOut.uwSectorNum |= 1<<2;
//    }
    
    
//    switchhall_stOut.uwSectorNum = GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_6);
//    switchhall_stOut.uwSectorNum |= (GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_7) << 1);
//    switchhall_stOut.uwSectorNum |= (GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_8) << 2);
}

/***************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
****************************************************************/
static SLONG  switchhall_pvt_slLowSpdLpfPu;
static SLONG  switchhall_pvt_slHighSpdLpfPu;
static SLONG  Speed_buffer[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
static SQWORD Speed_fb_sum = 0;
static SLONG  Speed_Value = 0;
static SLONG  Speed_AvgValue = 0;
static SWORD  Speed_sample_index = 0;
static SWORD  HallFltFlg;
void   switchhall_voTim3Int(void)
{
    /*Sector Judgement*/
    switchhall_voSectorJudge();
    /* Capture Value */    
    if (timer_interrupt_flag_get(TIMER2, TIMER_INT_FLAG_UP) == SET)
    {
//        if(curSpeed_state.state == StartUp || curSpeed_state.state == Open2Clz || curSpeed_state.state == ClzLoop)
//        {     
//          if (timer_interrupt_flag_get(TIMER2, TIMER_INT_FLAG_CH0) == SET)
//          {
//              switchhall_stCap.uwState = (UWORD)timer_channel_capture_value_register_read(TIMER2, TIMER_INT_FLAG_CH0);
//              timer_interrupt_flag_clear(TIMER2, TIMER_INT_FLAG_CH0);
//          }
//          else if (timer_interrupt_flag_get(TIMER2, TIMER_INT_FLAG_CH1) == SET)
//          {
//              switchhall_stCap.uwState = (UWORD)timer_channel_capture_value_register_read(TIMER2, TIMER_INT_FLAG_CH1);
//              timer_interrupt_flag_clear(TIMER2, TIMER_INT_FLAG_CH1);
//          }
//          else if (timer_interrupt_flag_get(TIMER2, TIMER_INT_FLAG_CH2) == SET)
//          {
//              switchhall_stCap.uwState = (UWORD)timer_channel_capture_value_register_read(TIMER2, TIMER_INT_FLAG_CH2);
//              timer_interrupt_flag_clear(TIMER2, TIMER_INT_FLAG_CH2);
//          }
//          else
//          {
//              switchhall_stCap.uwState=0;
//          }  
//          
//          if (switchhall_stCap.uwState > timer_counter_read(TIMER2))
//          {
//              switchhall_stCap.ulCapValue = switchhall_stCap.uwState;
//              switchhall_stCap.slCapValueDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre;
//              switchhall_stCap.uwCaptureOverFlowCnt ++;              
//          }
//          else
//          {
//            switchhall_stCap.uwCaptureOverFlowCnt ++;
//          }
//          
//          if (switchhall_stCap.uwCaptureOverFlowCnt > 3000)
//          {
//            switchhall_stCap.uwCaptureOverFlowCnt=0;
//          }
//        }
//        else
//        {
//          switchhall_stCap.uwCaptureOverFlowCnt = 0;
//        }
        
        switchhall_stCap.uwCaptureOverFlowCnt ++;
        timer_interrupt_flag_clear(TIMER2, TIMER_INT_FLAG_UP);
    }
    else if (timer_interrupt_flag_get(TIMER2, TIMER_INT_FLAG_CH0)== SET)
    {
        switchhall_stCap.ulCapValue = timer_channel_capture_value_register_read(TIMER2, TIMER_INT_FLAG_CH0);  
        switchhall_stCap.slCapValueDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre;
        switchhall_stCap.uwCaptureOverFlowCnt = 0;
        timer_interrupt_flag_clear(TIMER2, TIMER_INT_FLAG_CH0);
    } 
    else if (timer_interrupt_flag_get(TIMER2, TIMER_INT_FLAG_CH1) == SET)
    {
        switchhall_stCap.ulCapValue = timer_channel_capture_value_register_read(TIMER2, TIMER_INT_FLAG_CH1);        
        switchhall_stCap.slCapValueDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre;
        switchhall_stCap.uwCaptureOverFlowCnt = 0;
        timer_interrupt_flag_clear(TIMER2, TIMER_INT_FLAG_CH1);  
    }
    else if (timer_interrupt_flag_get(TIMER2, TIMER_INT_FLAG_CH2) == SET)
    {     
      
         /* Low Speed*/
        switchhall_stCap.ulCapValue = timer_channel_capture_value_register_read(TIMER2, TIMER_INT_FLAG_CH2);      
        switchhall_stCap.slCapValueDelta = (SLONG)switchhall_stCap.uwCaptureOverFlowCnt * 60000 + (SLONG)switchhall_stCap.ulCapValue - (SLONG)switchhall_stCap.ulCapValuePre;
        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;
//        }       
        timer_interrupt_flag_clear(TIMER2, TIMER_INT_FLAG_CH2);
    }  
    else
    {
//        switchhall_stCap.slCapValueDelta = HALL3_SPD_CAP_LEVEL1;
    }
   
    
    /* Move Average Filter */
    Speed_Value = 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 >> 4) ;  
    Speed_sample_index++;
    if (Speed_sample_index >= 16)
    {
    	Speed_sample_index = 0;
    }

    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;
//      }
      switchhall_stCap.slCapValueDelta = Speed_AvgValue;
    }
    else
    {
      switchhall_stCap.slCapValueDelta = HALL3_SPD_CAL_COEF;
      
    }    
    
    switchhall_stCap.ulCapValuePre = switchhall_stCap.ulCapValue;
}
/***************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
****************************************************************/
void switchhall_voPosCalTbc(void) /* parasoft-suppress METRICS-28 "本项目圈复杂度无法更改，后续避免" */
{
    SLONG Hall_Theta_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;
    }
    else
    {
    	//do nothing
    }
    /*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*/
    switchhall_voSectorJudge();
    /*!< 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;
        }
        else
        {
        	//do nothing
        }

        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
        {
        	//do nothing
        }
        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;
        }
        else
        {
        	//do nothing
        }

        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
        {
        	//do nothing
        }
        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;
        }
        else
        {
        	//do nothing
        }
        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;
            }
            else
            {
            	//do nothing
            }
        }

        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;
        }
        else
        {
        	//do nothing
        }

        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
        {
        	//do nothing
        }
        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;
        }
        else
        {
        	//do nothing
        }

        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
        {
        	//do nothing
        }
        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;
        }
        else
        {
        	//do nothing
        }

        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
        {
        	//do nothing
        }
        break;
    default:
        switchhall_stOut.swSpdCWorCCW = 0;
        switchhall_stOut.swLowSpdPu = 0;
        break;
    }

    
//    /*!< Zero Correction 3*/
//    switch (switchhall_stOut.uwSectorNum)
//    {
//      case 1:
//          if (switchhall_stOut.uwSectorNumPre == 3)
//          {
//              switchhall_stOut.swSpdCWorCCW = 1;
//              switchhall_stOut.slLowThetaPu = cof_sl30DegreePu;
//          }
//          else if (switchhall_stOut.uwSectorNumPre == 5)
//          {
//              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;
//      case 2:
//          if (switchhall_stOut.uwSectorNumPre == 6)
//          {
//              switchhall_stOut.swSpdCWorCCW = 1;
//              switchhall_stOut.slLowThetaPu = cof_sl270DegreePu;
//          }
//          else if (switchhall_stOut.uwSectorNumPre == 3)
//          {
//              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 3:
//          if (switchhall_stOut.uwSectorNumPre == 2)
//          {
//              switchhall_stOut.swSpdCWorCCW = 1;
//              switchhall_stOut.slLowThetaPu = cof_sl330DegreePu;
//          }
//          else if (switchhall_stOut.uwSectorNumPre == 1)
//          {
//              switchhall_stOut.swSpdCWorCCW = -1;
//              switchhall_stOut.slLowThetaPu = cof_sl30DegreePu;
//          }
//
//          switchhall_stOut.slInitThetaPu = cof_sl360DegreePu;
//          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 4:
//          if (switchhall_stOut.uwSectorNumPre == 5)
//          {
//              switchhall_stOut.swSpdCWorCCW = 1;
//              switchhall_stOut.slLowThetaPu = cof_sl150DegreePu;
//          }
//          else if (switchhall_stOut.uwSectorNumPre == 6)
//          {
//              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 5:
//          if (switchhall_stOut.uwSectorNumPre == 1)
//          {
//              switchhall_stOut.swSpdCWorCCW = 1;
//              switchhall_stOut.slLowThetaPu = cof_sl90DegreePu;
//          }
//          else if (switchhall_stOut.uwSectorNumPre == 4)
//          {
//              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 6:
//          if (switchhall_stOut.uwSectorNumPre == 4)
//          {
//              switchhall_stOut.swSpdCWorCCW = 1;
//              switchhall_stOut.slLowThetaPu = cof_sl210DegreePu;
//          }
//          else if (switchhall_stOut.uwSectorNumPre == 2)
//          {
//              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;
//      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;
        if (Hall_Theta_Temp >= cof_sl360DegreePu)
        {
            Hall_Theta_Temp -= cof_sl360DegreePu;
        }
        else if (Hall_Theta_Temp < 0)
        {
            Hall_Theta_Temp += cof_sl360DegreePu;
        }
        else
        {
        	//do nothing
        }
        switchhall_stOut.uwLowThetaPu = (UWORD)Hall_Theta_Temp;        
    }
    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;
            if (Hall_Theta_Temp >= cof_sl360DegreePu)
            {
                Hall_Theta_Temp -= cof_sl360DegreePu;
            }
            else if (Hall_Theta_Temp < 0)
            {
                Hall_Theta_Temp += cof_sl360DegreePu;
            }
            else
            {
            	//do nothing
            }
            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;
            }
            else
            {
            	//do nothing
            }
            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;
        }

        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);
        }
        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);
        }
        else
        {
//            switchhall_stOut.swLowSpdPu = 0;
        }
    }
    else
    {
        // High speed: use single hall
    }
    
    /* Low Speed LPF */
    switchhall_pvt_slLowSpdLpfPu =
        (SLONG)0x00CB * (switchhall_stOut.swLowSpdPu - switchhall_stOut.swLowSpdLpfPu) + switchhall_pvt_slLowSpdLpfPu; //50Hz
    switchhall_stOut.swLowSpdLpfPu = (SWORD)(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;
}

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