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

/**
 * @file Bikethrottle.c
 * @author Wang, Zhiyu(wangzy49@midea.com)
 * @brief throttle of ebike
 * @version 0.1
 * @date 2021-09-29
 *
 * @copyright Copyright (c) 2021
 *
 */

/************************************************************************
 Beginning of File, do not put anything above here except notes
 Compiler Directives:
*************************************************************************/
#include "syspar.h"
#include "typedefine.h"
#include "mathtool.h"
#include "torquesensor.h"
#include "hwsetup.h"
#include "CodePara.h"
#include "canAppl.h"
#include "FuncLayerAPI.h"
/******************************
 *
 * static  Parameter
 *
 ******************************/
TORQUESENSOR_COF torsensor_stTorSensorCof = TORQUESENSOR_COF_DEFAULT;
static LPF_OUT   scm_stTorSensorLpf;
TORQUESENSOR_OUT torsensor_stTorSensorOut = TORQUESENSOR_OUT_DEFAULT;

volatile SWORD TorqOffsetReg[TORQ_OFFSET_NUM]= {
640,    //-11 C
718,    //8 C
919,    //28 C
957,    //40 C
1051,     //55 C
1196,     //69 C
1352      //85 C
};

volatile SWORD TorqOffsetTemp[TORQ_OFFSET_NUM]= {
-1083,    //unit: 0.01C
843,    
2830,    
3997,    
5460,     
6930,     
8453      
};
volatile SWORD TorqOffsetCof[TORQ_OFFSET_NUM-1]= {
  0,0,0,0,0,0
};

volatile SWORD TorqSencitiveReg[TORQ_OFFSET_NUM]= {
4423,    //-20 C
6021,    //0 C
7048,    //20 C
7663,    //40 C
8008,     //60 C
8226,     //80 C
8459      //100 C
};
volatile SWORD TorqSencitiveTemp[TORQ_OFFSET_NUM]= {
-200,    //unit: 0.1C
0,    
200,    
400,    
600,     
800,     
1000      
};
volatile SLONG TorqSencitiveCof[TORQ_OFFSET_NUM-1]= {
  0,0,0,0,0,0
};
/******************************
 *
 * Extern  Parameter
 *
 ******************************/
UWORD TorSensor_uwDMAReg = 0;

POLY_COEF TorqSencitiveCoef = TORQUESENSITIVE_COF_DEFAULT;

/***************************************************************
 Function: cadence_voFreGet;
 Description: cadence frequency get
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void torsensor_voTorSensorCof(void)
{

    ULONG ulLpfTm = 0;
    UWORD i = 0;
      
    // torsensor_stTorSensorCof.uwMaxSensorTorquePu = ((ULONG)TORQUE_MAX_RANGE << 14) / TORQUEBASE; // Q14
    torsensor_stTorSensorCof.uwMinSensorTorquePu = ((ULONG)TORQUE_MIN_RANGE << 14) / TORQUEBASE; // Q14
    torsensor_stTorSensorCof.uwMaxSensorVolOutputPu = ((ULONG)TORQUE_VOLTAGE_MAX_RANGE << 14 / VBASE);
    torsensor_stTorSensorCof.uwMinSensorVolOutputPu = ((ULONG)TORQUE_VOLTAGE_MIN_RANGE << 14 / VBASE);
    torsensor_stTorSensorCof.uwTorSensorLPFFrq = TORQUE_SENSOR_LPF_FRQ;
    torsensor_stTorSensorCof.uwTorVolLPFDisFrq = TORQUE_LPF_DISCRETEHZ;

    if(torsensor_stTorSensorCof.uwTorqueOffsetConfirmFlg == FALSE)
    {
        #if (TORSENSOR_USEMOL == TORSENSOR_USEDEFAULT)
        torsensor_stTorSensorCof.uwTorqueOffset = TORQUE_VOLTAGE_MIN_RANGE * 4096 / 3300;
        #elif (TORSENSOR_USEMOL == TORSENSOR_USEEE)
         torsensor_stTorSensorCof.uwTorqueOffsetPowerUp = hw_uwADC0[7]; 
        //torsensor_stTorSensorCof.uwTorqueOffsetPowerUp = PowerUpOffset;

        if(torsensor_stTorSensorCof.uwTorqueOffsetNow1 != 0 && torsensor_stTorSensorCof.uwTorqueOffsetNow2 != 0
            && torsensor_stTorSensorCof.uwTorqueOffsetNow3 != 0 && torsensor_stTorSensorCof.uwTorqueOffsetNow4 != 0)
        {
            torsensor_stTorSensorCof.uwTorqueNowAllHasValueFlg = TRUE;
        }

        if(torsensor_stTorSensorCof.uwTorqueOffsetOrign == 0 && torsensor_stTorSensorCof.uwTorqueNowAllHasValueFlg == 0)
        {
            torsensor_stTorSensorCof.uwTorqueOffset = torsensor_stTorSensorCof.uwTorqueOffsetPowerUp;
            torsensor_stTorSensorCof.uwTorqueOffsetOrign = torsensor_stTorSensorCof.uwTorqueOffsetPowerUp;
            cp_stFlg.ParaSaveEEFlg = TRUE;
            cp_stFlg.ParaUpdateFlg = TRUE;
            //cp_stFlg.ParaSensorInfoUpdateFlg = TRUE;
            //cp_stFlg.ParaAssistUpdateFinishFlg = TRUE;
            MC_UpcInfo.stSensorInfo.uwSaveFlg = TRUE;
        }
        else
        {
            SWORD AverageOffset = 0;
            AverageOffset = ((SLONG)torsensor_stTorSensorCof.uwTorqueOffsetNow1 + torsensor_stTorSensorCof.uwTorqueOffsetNow2 + 
                            torsensor_stTorSensorCof.uwTorqueOffsetNow3 + torsensor_stTorSensorCof.uwTorqueOffsetNow4)>>2;
            if( ((SWORD)torsensor_stTorSensorCof.uwTorqueOffsetPowerUp - torsensor_stTorSensorCof.uwTorqueOffsetOrign) > 200 )
            {
            }
            else
            {
                torsensor_stTorSensorCof.uwTorqueOffsetNow1 = torsensor_stTorSensorCof.uwTorqueOffsetNow2;
                torsensor_stTorSensorCof.uwTorqueOffsetNow2 = torsensor_stTorSensorCof.uwTorqueOffsetNow3;
                torsensor_stTorSensorCof.uwTorqueOffsetNow3 = torsensor_stTorSensorCof.uwTorqueOffsetNow4;
                torsensor_stTorSensorCof.uwTorqueOffsetNow4 = torsensor_stTorSensorCof.uwTorqueOffsetPowerUp;

                cp_stFlg.ParaSaveEEFlg = TRUE;
                cp_stFlg.ParaUpdateFlg = TRUE;
                //cp_stFlg.ParaSensorInfoUpdateFlg = TRUE;
                //cp_stFlg.ParaAssistUpdateFinishFlg = TRUE;
                MC_UpcInfo.stSensorInfo.uwSaveFlg = TRUE;
            }

            if( torsensor_stTorSensorCof.uwTorqueNowAllHasValueFlg == TRUE )
            {
                if((AverageOffset - torsensor_stTorSensorCof.uwTorqueOffsetOrign)>400 || (AverageOffset - torsensor_stTorSensorCof.uwTorqueOffsetOrign)<-400)
                {
                    torsensor_stTorSensorCof.uwTorqueOffset = torsensor_stTorSensorCof.uwTorqueOffsetOrign;
                }
                else
                {
                    torsensor_stTorSensorCof.uwTorqueOffset = AverageOffset;       
                }
            }
            else
            {
                torsensor_stTorSensorCof.uwTorqueOffset = torsensor_stTorSensorCof.uwTorqueOffsetOrign;
            }

        }
        #endif
        torsensor_stTorSensorCof.uwTorqueOffsetConfirmFlg = TRUE;
    }
    torsensor_stTorSensorCof.uwSensorVolPerTorqDefault = TORQUE_VOLTAGE_PER_NM;
    
    torsensor_stTorSensorCof.uwSensorVolPerTorq1 =
        (((ULONG)3300 * (torsensor_stTorSensorCof.uwBikeTorStep1ADC - torsensor_stTorSensorCof.uwTorqueOffset)) >> 12) *10 *10/
        (torsensor_stTorSensorCof.uwBikeTorStep1RealNm - 0);
    
    torsensor_stTorSensorCof.uwSensorVolPerTorq2 =
        (((ULONG)3300 * (torsensor_stTorSensorCof.uwBikeTorStep2ADC - torsensor_stTorSensorCof.uwBikeTorStep1ADC)) >> 12) *10 *10/
        (torsensor_stTorSensorCof.uwBikeTorStep2RealNm - torsensor_stTorSensorCof.uwBikeTorStep1RealNm);
    
    torsensor_stTorSensorCof.uwSensorVolPerTorq3 =
        (((ULONG)3300 * (torsensor_stTorSensorCof.uwBikeTorStep3ADC - torsensor_stTorSensorCof.uwBikeTorStep2ADC)) >> 12) *10 *10/
        (torsensor_stTorSensorCof.uwBikeTorStep3RealNm - torsensor_stTorSensorCof.uwBikeTorStep2RealNm);
    
    torsensor_stTorSensorCof.uwSensorVolPerTorq4 =
        (((ULONG)3300 * (torsensor_stTorSensorCof.uwBikeTorStep4ADC - torsensor_stTorSensorCof.uwBikeTorStep3ADC)) >> 12) *10*10 /
        (torsensor_stTorSensorCof.uwBikeTorStep4RealNm - torsensor_stTorSensorCof.uwBikeTorStep3RealNm);

    torsensor_stTorSensorCof.uwTorqueReg2PuDefault = (((SQWORD)33 << 24) / 10) / (1 << ADC_RESOLUTION_BIT) / TORQUE_VOLTAGE_SEN2MCUGAIN * 100 * 1000 /
                                                     torsensor_stTorSensorCof.uwSensorVolPerTorqDefault / TORQUEBASE *
                                                     10; // 3.3/4096/harwaregain/VolPerNm/TorqueBase;
    torsensor_stTorSensorCof.uwTorqueReg2Pu1 = (((SQWORD)33 << 24) / 10) / (1 << ADC_RESOLUTION_BIT) / TORQUE_VOLTAGE_SEN2MCUGAIN * 100 * 1000 /
                                               torsensor_stTorSensorCof.uwSensorVolPerTorq1 / TORQUEBASE *
                                               10*10; // 3.3/4096/harwaregain/VolPerNm/TorqueBase;
    torsensor_stTorSensorCof.uwTorqueReg2Pu2 = (((SQWORD)33 << 24) / 10) / (1 << ADC_RESOLUTION_BIT) / TORQUE_VOLTAGE_SEN2MCUGAIN * 100 * 1000 /
                                               torsensor_stTorSensorCof.uwSensorVolPerTorq2 / TORQUEBASE *
                                               10*10; // 3.3/4096/harwaregain/VolPerNm/TorqueBase;
    torsensor_stTorSensorCof.uwTorqueReg2Pu3 = (((SQWORD)33 << 24) / 10) / (1 << ADC_RESOLUTION_BIT) / TORQUE_VOLTAGE_SEN2MCUGAIN * 100 * 1000 /
                                               torsensor_stTorSensorCof.uwSensorVolPerTorq3 / TORQUEBASE *
                                               10*10; // 3.3/4096/harwaregain/VolPerNm/TorqueBase;
    torsensor_stTorSensorCof.uwTorqueReg2Pu4 = (((SQWORD)33 << 24) / 10) / (1 << ADC_RESOLUTION_BIT) / TORQUE_VOLTAGE_SEN2MCUGAIN * 100 * 1000 /
                                               torsensor_stTorSensorCof.uwSensorVolPerTorq4 / TORQUEBASE *
                                               10*10; // 3.3/4096/harwaregain/VolPerNm/TorqueBase;
    
    torsensor_stTorSensorCof.uwBikeTorStep1NmPu = ((ULONG)torsensor_stTorSensorCof.uwBikeTorStep1RealNm << 14)/TORQUEBASE;
    torsensor_stTorSensorCof.uwBikeTorStep2NmPu = ((ULONG)torsensor_stTorSensorCof.uwBikeTorStep2RealNm << 14)/TORQUEBASE;
    torsensor_stTorSensorCof.uwBikeTorStep3NmPu = ((ULONG)torsensor_stTorSensorCof.uwBikeTorStep3RealNm << 14)/TORQUEBASE;
    torsensor_stTorSensorCof.uwBikeTorStep4NmPu = ((ULONG)torsensor_stTorSensorCof.uwBikeTorStep4RealNm << 14)/TORQUEBASE;
    
    /* Torque Sensor limit coef */
    ulLpfTm = 1000000 / torsensor_stTorSensorCof.uwTorSensorLPFFrq;
    mth_voLPFilterCoef(ulLpfTm, torsensor_stTorSensorCof.uwTorVolLPFDisFrq, &scm_stTorSensorLpf.uwKx);
    
    for (i = 0; i < (TORQ_OFFSET_NUM - 1); i++)
    {
        TorqOffsetCof[i] =  (((SLONG)TorqOffsetReg[i+1] - (SLONG)TorqOffsetReg[i]) << 12) /(TorqOffsetTemp[i+1] - TorqOffsetTemp[i]); //Q12
    }
    
        for (i = 0; i < (TORQ_OFFSET_NUM - 1); i++)
    {
        TorqSencitiveCof[i] =  (((SLONG)TorqSencitiveReg[i+1] - (SLONG)TorqSencitiveReg[i]) << 10) /(TorqSencitiveTemp[i+1] - TorqSencitiveTemp[i]); //Q10
    }
}

/***************************************************************
 Function: torsensor_voTorSensorInit;
 Description: Torque initialization
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
LPF_OUT tst_dynOffsetLpf;
UWORD tsttorqCadCnt,tsttorqMin=4096,tstdynOffset;

void torsensor_voTorSensorInit(void)
{
    torsensor_stTorSensorOut.uwTorqueReg = 0;
    torsensor_stTorSensorOut.uwTorquePu = 0;
    torsensor_stTorSensorOut.uwTorqueLPFPu = 0;
    torsensor_stTorSensorOut.uwTorqueErrorCnt = 0;
    torsensor_stTorSensorOut.blTorqueCaliFlg = FALSE;
    torsensor_stTorSensorOut.blTorqueErrorFlg = FALSE;
    mth_voLPFilterCoef(1000000 / 1, EVENT_1MS_HZ, &tst_dynOffsetLpf.uwKx); //25Hz  
    tstdynOffset= hw_uwADC0[7];
    tst_dynOffsetLpf.slY.sw.hi =  hw_uwADC0[7];
        /* Torque Sensor limit coef */
   
}

/*************************************************************************
 Local Functions (N/A)
*************************************************************************/
BOOL tstDynCalibflg= TRUE;
UWORD tstTorqOffset,tstSensitiveset,TorqValue,TorqValuePu, TorqReg;
SWORD tstTorqTemp,tstTorqTemp111,tstSencitiveOrig;
void torsensor_voCadenceCnt(void)
{
  if (((cadence_stFreGetCof.uwNumbersPulses>>1)-1) != tsttorqCadCnt)
  {
      tsttorqCadCnt++;
  }
  else
  {
      tsttorqCadCnt = 0;
      tsttorqMin = 4096;
  }
}

void torsensor_voDynamicOffset(void)
{
   if(cadence_stFreGetOut.uwLPFFrequencyPu != 0)
   {
     tstDynCalibflg = TRUE;
     if(tsttorqMin > hw_uwADC0[7])
     {
        tsttorqMin = hw_uwADC0[7];
     }
     if(tsttorqCadCnt == ((cadence_stFreGetCof.uwNumbersPulses>>1) -1))
     {       
        tstdynOffset = tsttorqMin;
     }
   }
   else
   {
     if( tstDynCalibflg == TRUE && TorqValuePu <= 500)
     {
        tstdynOffset = hw_uwADC0[7];
        tstDynCalibflg = FALSE;
     }
   }
   mth_voLPFilter(tstdynOffset, &tst_dynOffsetLpf);
}


void torsensor_voTorADCwithTemp(void)
{
    tstTorqTemp = temp_swTorqTempCal(hw_uwADC0[4]); // 0.1 C
//    tstTorqOffset = torsensor_uwTorqOffsetCal(tstTorqTemp); // Torque AD 
    tstTorqOffset= tst_dynOffsetLpf.slY.sw.hi;//  1478;   //tstdynOffset;
    tstSensitiveset = torsensor_uwTorqSencitiveCal(tstTorqTemp/10, 250); //Q12

    torsensor_stTorSensorOut.uwTorqueReg = hw_uwADC0[7];
    
    if(((SLONG)torsensor_stTorSensorOut.uwTorqueReg - tstTorqOffset) < 0)
    {
      TorqReg = torsensor_stTorSensorCof.uwTorqueOffset;
    }
    else
    {
      TorqReg = (((SLONG)torsensor_stTorSensorOut.uwTorqueReg - tstTorqOffset) *10000) / tstSensitiveset + torsensor_stTorSensorCof.uwTorqueOffset;
    }
    
      
        if (TorqReg <= torsensor_stTorSensorCof.uwTorqueOffset)
        {
            TorqValuePu = 0;
        }
        else if (TorqReg <= torsensor_stTorSensorCof.uwBikeTorStep1ADC)
        {
            TorqValuePu = 0 +
                ((((SQWORD)abs((SWORD)(TorqReg) - torsensor_stTorSensorCof.uwTorqueOffset)) *
                 torsensor_stTorSensorCof.uwTorqueReg2Pu1) >>
                10); // Q14
        }
        else if (TorqReg <= torsensor_stTorSensorCof.uwBikeTorStep2ADC)
        {
            TorqValuePu = torsensor_stTorSensorCof.uwBikeTorStep1NmPu + 
                ((((SQWORD)abs((SWORD)(TorqReg) - torsensor_stTorSensorCof.uwBikeTorStep1ADC)) *
                 torsensor_stTorSensorCof.uwTorqueReg2Pu2) >>
                10); // Q14
        }
        else if (TorqReg <= torsensor_stTorSensorCof.uwBikeTorStep3ADC)
        {
            TorqValuePu = torsensor_stTorSensorCof.uwBikeTorStep2NmPu +
                ((((SQWORD)abs((SWORD)(TorqReg) - torsensor_stTorSensorCof.uwBikeTorStep2ADC)) *
                 torsensor_stTorSensorCof.uwTorqueReg2Pu3) >>
                10); // Q14
        }
        else if (TorqReg<= torsensor_stTorSensorCof.uwBikeTorStep4ADC)
        {
            TorqValuePu = torsensor_stTorSensorCof.uwBikeTorStep3NmPu + 
                ((((SQWORD)abs((SWORD)(TorqReg) - torsensor_stTorSensorCof.uwBikeTorStep3ADC)) *
                 torsensor_stTorSensorCof.uwTorqueReg2Pu4) >>
                10); // Q14
        }
        else
        {
            TorqValuePu = torsensor_stTorSensorCof.uwBikeTorStep4NmPu;
        }
        torsensor_stTorSensorOut.uwTorquePu=TorqValuePu;
        mth_voLPFilter(torsensor_stTorSensorOut.uwTorquePu, &scm_stTorSensorLpf);
        torsensor_stTorSensorOut.uwTorqueLPFPu = scm_stTorSensorLpf.slY.sw.hi;
        TorqValue = (ULONG)TorqValuePu * TORQUEBASE  >> 14;
        
    //TorqValue = ((torsensor_stTorSensorOut.uwTorqueReg - tstTorqOffset) << 12 )/tstSencitiveset;
}


void torsensor_voTorADC(void) // need to match ADC_StartConversion(ADC1);
{


    if (torsensor_stTorSensorOut.blTorqueErrorFlg == TRUE)
    {
        torsensor_stTorSensorOut.uwTorquePu = 0;
        torsensor_stTorSensorOut.uwTorqueReg = hw_uwADC0[7];
        if (torsensor_stTorSensorOut.uwTorqueReg < 4000 && torsensor_stTorSensorOut.uwTorqueReg > 10) // output 0mv - 3000mv
        {
            torsensor_stTorSensorOut.uwTorqueErrorCnt++;
            if (torsensor_stTorSensorOut.uwTorqueErrorCnt > 1000)
            {
                torsensor_stTorSensorOut.blTorqueErrorFlg = FALSE;
                torsensor_voTorSensorInit();
            }
        }
        else
        {
            torsensor_stTorSensorOut.uwTorqueErrorCnt = 0;
        }
    }
    else
    {
        torsensor_stTorSensorOut.uwTorqueReg = hw_uwADC0[7]; // TorSensor_uwDMAReg;
#if (TORSENSOR_USEMOL == TORSENSOR_USEDEFAULT)
        torsensor_stTorSensorOut.uwTorquePu =
            (((SQWORD)abs((SWORD)(torsensor_stTorSensorOut.uwTorqueReg) - torsensor_stTorSensorCof.uwTorqueOffset)) *
             torsensor_stTorSensorCof.uwTorqueReg2PuDefault) >>
            10; // Q14
#elif (TORSENSOR_USEMOL == TORSENSOR_USEEE)
        
        if (torsensor_stTorSensorOut.uwTorqueReg <= torsensor_stTorSensorCof.uwTorqueOffset)
        {
            torsensor_stTorSensorOut.uwTorquePu = 0;
        }
        else if (torsensor_stTorSensorOut.uwTorqueReg <= torsensor_stTorSensorCof.uwBikeTorStep1ADC)
        {
            torsensor_stTorSensorOut.uwTorquePu = 0 +
                ((((SQWORD)abs((SWORD)(torsensor_stTorSensorOut.uwTorqueReg) - torsensor_stTorSensorCof.uwTorqueOffset)) *
                 torsensor_stTorSensorCof.uwTorqueReg2Pu1) >>
                10); // Q14
        }
        else if (torsensor_stTorSensorOut.uwTorqueReg <= torsensor_stTorSensorCof.uwBikeTorStep2ADC)
        {
            torsensor_stTorSensorOut.uwTorquePu = torsensor_stTorSensorCof.uwBikeTorStep1NmPu + 
                ((((SQWORD)abs((SWORD)(torsensor_stTorSensorOut.uwTorqueReg) - torsensor_stTorSensorCof.uwBikeTorStep1ADC)) *
                 torsensor_stTorSensorCof.uwTorqueReg2Pu2) >>
                10); // Q14
        }
        else if (torsensor_stTorSensorOut.uwTorqueReg <= torsensor_stTorSensorCof.uwBikeTorStep3ADC)
        {
            torsensor_stTorSensorOut.uwTorquePu = torsensor_stTorSensorCof.uwBikeTorStep2NmPu +
                ((((SQWORD)abs((SWORD)(torsensor_stTorSensorOut.uwTorqueReg) - torsensor_stTorSensorCof.uwBikeTorStep2ADC)) *
                 torsensor_stTorSensorCof.uwTorqueReg2Pu3) >>
                10); // Q14
        }
        else if (torsensor_stTorSensorOut.uwTorqueReg <= torsensor_stTorSensorCof.uwBikeTorStep4ADC)
        {
            torsensor_stTorSensorOut.uwTorquePu = torsensor_stTorSensorCof.uwBikeTorStep3NmPu + 
                ((((SQWORD)abs((SWORD)(torsensor_stTorSensorOut.uwTorqueReg) - torsensor_stTorSensorCof.uwBikeTorStep3ADC)) *
                 torsensor_stTorSensorCof.uwTorqueReg2Pu4) >>
                10); // Q14
        }
        else
        {
            torsensor_stTorSensorOut.uwTorquePu = torsensor_stTorSensorCof.uwBikeTorStep4NmPu;
        }

#endif
        mth_voLPFilter(torsensor_stTorSensorOut.uwTorquePu, &scm_stTorSensorLpf);
        torsensor_stTorSensorOut.uwTorqueLPFPu = scm_stTorSensorLpf.slY.sw.hi;
        torsensor_stTorSensorOut.uwTorquePercent =
            (((ULONG)torsensor_stTorSensorOut.uwTorqueLPFPu) << 14) /
            (torsensor_stTorSensorCof.uwMaxSensorTorquePu - torsensor_stTorSensorCof.uwMinSensorTorquePu); // Q15

        if (torsensor_stTorSensorOut.uwTorqueReg > 4000 || torsensor_stTorSensorOut.uwTorqueReg < 10) // output 0mv - 3000mv
        {
            torsensor_stTorSensorOut.uwTorqueErrorCnt++;
            if (torsensor_stTorSensorOut.uwTorqueErrorCnt > 5000)
            {
                torsensor_stTorSensorOut.blTorqueErrorFlg = TRUE;
                torsensor_stTorSensorOut.uwTorquePu = 0;
                torsensor_stTorSensorOut.uwTorqueErrorCnt = 0;
                torsensor_stTorSensorOut.uwTorqueLPFPu = 0;
                cp_stHistoryPara.uwTorSensorAlamTimes++;
            }
        }
        else
        {
            torsensor_stTorSensorOut.uwTorqueErrorCnt = 0;
        }
    }
}

/***************************************************************
 Function:
 Description:
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
UWORD torsensor_uwTorqOffsetCal(SWORD Temp)  
{
    UWORD Offset = 0, i = 0;
    
    if(Temp < TorqOffsetTemp[0])
    {
       Offset = TorqOffsetReg[0];
    }
    else if(Temp >= TorqOffsetTemp[TORQ_OFFSET_NUM - 1])
    {
       Offset = TorqOffsetReg[TORQ_OFFSET_NUM - 1];
    }
    else
    {
        for (i = 0; i < (TORQ_OFFSET_NUM - 1); i++)
        {    
            if(Temp >= TorqOffsetTemp[i] && Temp < TorqOffsetTemp[i+1])
            {
               Offset = TorqOffsetReg[i] + (TorqOffsetCof[i] * (Temp - TorqOffsetTemp[i]) >> 12);
               break;
            }        
        } 
    }
   
    return  Offset;
}

UWORD torsensor_uwTorqSencitiveCal(SWORD Temp, SWORD T0)  
{
//    UWORD Sencitive = 0, i = 0;
//    
//    if(Temp < TorqSencitiveTemp[0])
//    {
//       Sencitive = TorqSencitiveReg[0];
//    }
//    else if(Temp >= TorqSencitiveTemp[TORQ_OFFSET_NUM - 1])
//    {
//       Sencitive = TorqSencitiveReg[TORQ_OFFSET_NUM - 1];
//    }
//    else
//    {
//        for (i = 0; i < (TORQ_OFFSET_NUM - 1); i++)
//        {    
//            if(Temp >= TorqSencitiveTemp[i] && Temp < TorqSencitiveTemp[i+1])
//            {
//               Sencitive = TorqSencitiveReg[i] + (TorqSencitiveCof[i] * (Temp - TorqSencitiveTemp[i]) >> 10); // Q10
//               break;
//            }        
//        } 
//    }
//    
//    return  Sencitive;
  
     UWORD a = 108, b = 939, sensitive = 0; //a=0.00010846, b= 0.93899723
     SWORD DeltaTemp = 0;  
     SLONG g = 0;  

     DeltaTemp = Temp - T0; //unit: 0.1 C
     
     g =(SLONG)b * DeltaTemp +  (SLONG)a * DeltaTemp * DeltaTemp / 1000;
     
     sensitive = 10000 + g / 100;  
     
     return sensitive;
     
}


/*************************************************************************
 Local Functions (N/A)
*************************************************************************/

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