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 "torquesensor.h"
#include "syspar.h"
#include "mathtool.h"
#include "CodePara.h"
#include "canAppl.h"
#include "api.h"
#include "board_config.h"
/******************************
 *
 * static  Parameter
 *
 ******************************/
static LPF_OUT   torq_pvt_stTorSensorLpf;
static 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
};

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

static 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
};
static volatile SWORD TorqSencitiveTemp[TORQ_OFFSET_NUM]= {
-200,    //unit: 0.1C
0,    
200,    
400,    
600,     
800,     
1000      
};
static volatile SLONG TorqSencitiveCof[TORQ_OFFSET_NUM-1]= {
  0,0,0,0,0,0
};
static UWORD TorSensor_uwDMAReg = 0;
/******************************
 *
 * Extern  Parameter
 *
 ******************************/
TORQUESENSOR_COF torsensor_stTorSensorCof = TORQUESENSOR_COF_DEFAULT;
TORQUESENSOR_OUT torsensor_stTorSensorOut = TORQUESENSOR_OUT_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, uwAverageOffset = 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 = iAdc_GetResultPointer(0)[HW_ADC_TORQ_CH]; 
        //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 = 1;
        }
        else
        { 
            /* Compare with AvgOffset */ 
            if(torsensor_stTorSensorCof.uwTorqueNowAllHasValueFlg == TRUE)
            {
                uwAverageOffset = (UWORD)(((ULONG)torsensor_stTorSensorCof.uwTorqueOffsetNow1 + torsensor_stTorSensorCof.uwTorqueOffsetNow2 +
                                torsensor_stTorSensorCof.uwTorqueOffsetNow3 + torsensor_stTorSensorCof.uwTorqueOffsetNow4)>>2);
            }
            else
            {
                uwAverageOffset = torsensor_stTorSensorCof.uwTorqueOffsetOrign;
            }            

            if(ABS((SWORD)torsensor_stTorSensorCof.uwTorqueOffsetPowerUp - (SWORD)uwAverageOffset) > 200)   
            {
                torsensor_stTorSensorCof.uwTorqueOffset = uwAverageOffset;
            }
            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 = 1;

                torsensor_stTorSensorCof.uwTorqueOffset = torsensor_stTorSensorCof.uwTorqueOffsetPowerUp;  
            }
        }
#endif
        torsensor_stTorSensorCof.uwTorqueOffsetConfirmFlg = TRUE;
    }
    
#ifdef RUN_ARCH_SIM_2
    torsensor_stTorSensorCof.uwTorqueOffset = 868;
    torsensor_stTorSensorCof.uwBikeTorStep1ADC = 1427;
    torsensor_stTorSensorCof.uwBikeTorStep1RealNm = 250;
    torsensor_stTorSensorCof.uwBikeTorStep2ADC = 1985;
    torsensor_stTorSensorCof.uwBikeTorStep2RealNm = 500;
    torsensor_stTorSensorCof.uwBikeTorStep3ADC = 2544;
    torsensor_stTorSensorCof.uwBikeTorStep3RealNm = 750;
    torsensor_stTorSensorCof.uwBikeTorStep4ADC = 3103;
    torsensor_stTorSensorCof.uwBikeTorStep4RealNm = 1000;
#endif
    
    torsensor_stTorSensorCof.uwSensorVolPerTorqDefault = TORQUE_VOLTAGE_PER_NM;

    torsensor_stTorSensorCof.ulTorqueReg2PuDefault = (ULONG)((((UQWORD)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.ulTorqueReg2Pu1 = (ULONG)(((UQWORD)(torsensor_stTorSensorCof.uwBikeTorStep1RealNm - 0) << 24) / 
                                               (torsensor_stTorSensorCof.uwBikeTorStep1ADC - torsensor_stTorSensorCof.uwTorqueOffset) /TORQUEBASE);
    torsensor_stTorSensorCof.ulTorqueReg2Pu2 = (ULONG)(((UQWORD)(torsensor_stTorSensorCof.uwBikeTorStep2RealNm - torsensor_stTorSensorCof.uwBikeTorStep1RealNm) << 24) / 
                                               (torsensor_stTorSensorCof.uwBikeTorStep2ADC - torsensor_stTorSensorCof.uwBikeTorStep1ADC) /TORQUEBASE);        
    torsensor_stTorSensorCof.ulTorqueReg2Pu3 = (ULONG)(((UQWORD)(torsensor_stTorSensorCof.uwBikeTorStep3RealNm - torsensor_stTorSensorCof.uwBikeTorStep2RealNm) << 24) / 
                                               (torsensor_stTorSensorCof.uwBikeTorStep3ADC - torsensor_stTorSensorCof.uwBikeTorStep2ADC) /TORQUEBASE);
    torsensor_stTorSensorCof.ulTorqueReg2Pu4 = (ULONG)(((UQWORD)(torsensor_stTorSensorCof.uwBikeTorStep4RealNm - torsensor_stTorSensorCof.uwBikeTorStep3RealNm) << 24) / 
                                               (torsensor_stTorSensorCof.uwBikeTorStep4ADC - torsensor_stTorSensorCof.uwBikeTorStep3ADC) /TORQUEBASE);
    
    torsensor_stTorSensorCof.uwBikeTorStep1NmPu = (UWORD)(((ULONG)torsensor_stTorSensorCof.uwBikeTorStep1RealNm << 14)/TORQUEBASE);
    torsensor_stTorSensorCof.uwBikeTorStep2NmPu = (UWORD)(((ULONG)torsensor_stTorSensorCof.uwBikeTorStep2RealNm << 14)/TORQUEBASE);
    torsensor_stTorSensorCof.uwBikeTorStep3NmPu = (UWORD)(((ULONG)torsensor_stTorSensorCof.uwBikeTorStep3RealNm << 14)/TORQUEBASE);
    torsensor_stTorSensorCof.uwBikeTorStep4NmPu = (UWORD)(((ULONG)torsensor_stTorSensorCof.uwBikeTorStep4RealNm << 14)/TORQUEBASE);
    
    /* Torque Sensor limit coef */
    ulLpfTm = 1000000 / torsensor_stTorSensorCof.uwTorSensorLPFFrq;
    mth_voLPFilterCoef(ulLpfTm, torsensor_stTorSensorCof.uwTorVolLPFDisFrq, &torq_pvt_stTorSensorLpf.uwKx);
    
    /* Torque Offset Correction Coef */
//    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
//    }
}


/*************************************************************************
 Local Functions (N/A)
*************************************************************************/
//static BOOL tstDynCalibflg= TRUE;
//static UWORD tstTorqOffset,tstSensitiveset,TorqValue,TorqValuePu, TorqReg;
//static 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 > iAdc_GetResultPointer(0)[HW_ADC_TORQ_CH])
//     {
//        tsttorqMin = iAdc_GetResultPointer(0)[HW_ADC_TORQ_CH];
//     }
//     if(tsttorqCadCnt == ((cadence_stFreGetCof.uwNumbersPulses>>1) -1))
//     {       
//        tstdynOffset = tsttorqMin;
//     }
//   }
//   else
//   {
//     if( tstDynCalibflg == TRUE && TorqValuePu <= 500)
//     {
//        tstdynOffset = iAdc_GetResultPointer(0)[HW_ADC_TORQ_CH];
//        tstDynCalibflg = FALSE;
//     }
//   }
//   mth_voLPFilter((SWORD)tstdynOffset, &tst_dynOffsetLpf);
//}


//static 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.ulTorqueReg2Pu1) >>
//                10); // Q14
//        }
//        else if (TorqReg <= torsensor_stTorSensorCof.uwBikeTorStep2ADC)
//        {
//            TorqValuePu = torsensor_stTorSensorCof.uwBikeTorStep1NmPu + 
//                ((((SQWORD)ABS((SWORD)(TorqReg) - torsensor_stTorSensorCof.uwBikeTorStep1ADC)) *
//                 torsensor_stTorSensorCof.ulTorqueReg2Pu2) >>
//                10); // Q14
//        }
//        else if (TorqReg <= torsensor_stTorSensorCof.uwBikeTorStep3ADC)
//        {
//            TorqValuePu = torsensor_stTorSensorCof.uwBikeTorStep2NmPu +
//                ((((SQWORD)ABS((SWORD)(TorqReg) - torsensor_stTorSensorCof.uwBikeTorStep2ADC)) *
//                 torsensor_stTorSensorCof.ulTorqueReg2Pu3) >>
//                10); // Q14
//        }
//        else if (TorqReg<= torsensor_stTorSensorCof.uwBikeTorStep4ADC)
//        {
//            TorqValuePu = torsensor_stTorSensorCof.uwBikeTorStep3NmPu + 
//                ((((SQWORD)ABS((SWORD)(TorqReg) - torsensor_stTorSensorCof.uwBikeTorStep3ADC)) *
//                 torsensor_stTorSensorCof.ulTorqueReg2Pu4) >>
//                10); // Q14
//        }
//        else
//        {
//            TorqValuePu = torsensor_stTorSensorCof.uwBikeTorStep4NmPu;
//        }
//        torsensor_stTorSensorOut.uwTorquePu=TorqValuePu;
//        mth_voLPFilter(torsensor_stTorSensorOut.uwTorquePu, &torq_pvt_stTorSensorLpf);
//        torsensor_stTorSensorOut.uwTorqueLPFPu = torq_pvt_stTorSensorLpf.slY.sw.hi;
//        TorqValue = (ULONG)TorqValuePu * TORQUEBASE  >> 14;
//        
//    //TorqValue = ((torsensor_stTorSensorOut.uwTorqueReg - tstTorqOffset) << 12 )/tstSencitiveset;
//}
/***************************************************************
 Function:
 Description:
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
static UWORD tor_pvt_uwOffsetTarget = 0;
static UWORD tor_pvt_uwOffsetMax = 0,tor_pvt_uwOffsetMin = 4096;
static ULONG tor_pvt_ulCnt = 0, tor_pvt_ulCnt2 = 0;
void torsensor_voOffsetUpdate(void) 
{
    SWORD swTorDelta;
    ++tor_pvt_ulCnt;
    if(0 == (tor_pvt_ulCnt % TORQUE_1S_1MSCNT))
    {
        swTorDelta = (SWORD)tor_pvt_uwOffsetMax - (SWORD)tor_pvt_uwOffsetMin;
        if(swTorDelta > 40)
        {   
            tor_pvt_ulCnt = 0;   
            tor_pvt_uwOffsetTarget = torsensor_stTorSensorCof.uwTorqueOffset;           
        }
        else 
        {
            tor_pvt_uwOffsetTarget = (tor_pvt_uwOffsetMax + tor_pvt_uwOffsetMin) >> 1;
        }
        tor_pvt_uwOffsetMax = 0;
        tor_pvt_uwOffsetMin = 4096; 
    }
    else
    {
        if(tor_pvt_uwOffsetMin > torsensor_stTorSensorOut.uwTorqueReg)
        {
            tor_pvt_uwOffsetMin = torsensor_stTorSensorOut.uwTorqueReg;
        }
        
        if(tor_pvt_uwOffsetMax < torsensor_stTorSensorOut.uwTorqueReg)
        {
            tor_pvt_uwOffsetMax = torsensor_stTorSensorOut.uwTorqueReg;
        }    
    }
   
    if(tor_pvt_ulCnt > TORQUE_90S_1MSCNT)
    {
        if(tor_pvt_uwOffsetTarget != torsensor_stTorSensorCof.uwTorqueOffset)
        {
            if(torsensor_stTorSensorCof.uwTorqueOffset < tor_pvt_uwOffsetTarget - 20)
            {
                torsensor_stTorSensorCof.uwTorqueOffset += 20;
            }
            else if (torsensor_stTorSensorCof.uwTorqueOffset > tor_pvt_uwOffsetTarget + 20)
            {
                torsensor_stTorSensorCof.uwTorqueOffset -= 20;
            }
            else
            {
                torsensor_stTorSensorCof.uwTorqueOffset = tor_pvt_uwOffsetTarget;
            }
            
            torsensor_stTorSensorCof.ulTorqueReg2Pu1 = (ULONG)(((UQWORD)(torsensor_stTorSensorCof.uwBikeTorStep1RealNm - 0) << 24) / 
                                                (torsensor_stTorSensorCof.uwBikeTorStep1ADC - torsensor_stTorSensorCof.uwTorqueOffset) /TORQUEBASE);
        }

        tor_pvt_ulCnt = 0;   
    }       
}
/***************************************************************
 Function:
 Description:
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
void torsensor_voTorADC(void) // need to match ADC_StartConversion(ADC1);
{
    torsensor_stTorSensorOut.uwTorqueReg = iAdc_GetResultPointer(0)[HW_ADC_TORQ_CH];
#if (TORSENSOR_USEMOL == TORSENSOR_USEDEFAULT)
    torsensor_stTorSensorOut.uwTorquePu =
        (((SQWORD)ABS((SWORD)(torsensor_stTorSensorOut.uwTorqueReg) - torsensor_stTorSensorCof.uwTorqueOffset)) *
            torsensor_stTorSensorCof.ulTorqueReg2PuDefault) >>
        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 = (UWORD)(0 +
            ((((UQWORD)ABS((SWORD)torsensor_stTorSensorOut.uwTorqueReg - torsensor_stTorSensorCof.uwTorqueOffset)) *
                torsensor_stTorSensorCof.ulTorqueReg2Pu1) >> 10)); // Q14
    }
    else if (torsensor_stTorSensorOut.uwTorqueReg <= torsensor_stTorSensorCof.uwBikeTorStep2ADC)
    {
        torsensor_stTorSensorOut.uwTorquePu = (UWORD)(torsensor_stTorSensorCof.uwBikeTorStep1NmPu +
            ((((UQWORD)ABS((SWORD)torsensor_stTorSensorOut.uwTorqueReg - torsensor_stTorSensorCof.uwBikeTorStep1ADC)) *
                torsensor_stTorSensorCof.ulTorqueReg2Pu2) >> 10)); // Q14

    }
    else if (torsensor_stTorSensorOut.uwTorqueReg <= torsensor_stTorSensorCof.uwBikeTorStep3ADC)
    {
        torsensor_stTorSensorOut.uwTorquePu = (UWORD)(torsensor_stTorSensorCof.uwBikeTorStep2NmPu +
            ((((UQWORD)ABS((SWORD)torsensor_stTorSensorOut.uwTorqueReg - torsensor_stTorSensorCof.uwBikeTorStep2ADC)) *
                torsensor_stTorSensorCof.ulTorqueReg2Pu3) >> 10)); // Q14

    }
    else if (torsensor_stTorSensorOut.uwTorqueReg <= torsensor_stTorSensorCof.uwBikeTorStep4ADC)
    {
        torsensor_stTorSensorOut.uwTorquePu = (UWORD)(torsensor_stTorSensorCof.uwBikeTorStep3NmPu +
            ((((UQWORD)ABS((SWORD)torsensor_stTorSensorOut.uwTorqueReg - torsensor_stTorSensorCof.uwBikeTorStep3ADC)) *
                torsensor_stTorSensorCof.ulTorqueReg2Pu4) >> 10)); // Q14

    }
    else
    {
        torsensor_stTorSensorOut.uwTorquePu = torsensor_stTorSensorCof.uwBikeTorStep4NmPu;
    }

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

}

/***************************************************************
 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;
//
//}

/***************************************************************
 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
****************************************************************/
static LPF_OUT tst_dynOffsetLpf;
static 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= iAdc_GetResultPointer(0)[HW_ADC_TORQ_CH]; 
    tst_dynOffsetLpf.slY.sw.hi =  (SWORD)iAdc_GetResultPointer(0)[HW_ADC_TORQ_CH];   

    tor_pvt_uwOffsetTarget = 0;
    tor_pvt_uwOffsetMax = 0;
    tor_pvt_uwOffsetMin = 4096;
    tor_pvt_ulCnt = 0;
    tor_pvt_ulCnt2 = 0;
}


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

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