/**
 * @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 "CodePara.h"
#include "canAppl.h"
#include "FuncLayerAPI.h"
//#include "api.h"
#include "board_config.h"
#include "UserGpio_Config.h"

//#include "api_rt_adc.h"
/******************************
 *
 * static  Parameter
 *
 ******************************/
TORQUESENSOR_COF torsensor_stTorSensorCof = TORQUESENSOR_COF_DEFAULT;
static LPF_OUT   scm_stTorSensorLpf;
TORQUESENSOR_OUT torsensor_stTorSensorOut = TORQUESENSOR_OUT_DEFAULT;
TORQUESENSOR_REG_FLT TorqueSensorRegFlt;

/******************************
 *
 * Extern  Parameter
 *
 ******************************/

/***************************************************************
 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;//, delay_cnt;
    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_USEMOL == TORSENSOR_USEDEFAULT)
    torsensor_stTorSensorCof.uwTorqueOffset = TORQUE_VOLTAGE_MIN_RANGE * 4096 / 3300;
#elif (TORSENSOR_USEMOL == TORSENSOR_USEEE)
    torsensor_stTorSensorCof.uwTorqueOffsetPowerUp = TORQUEVOLREG();//DL_ADC12_getMemResult(ADC12_0_INST, DL_ADC12_MEM_IDX_2);
    //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;
        /* Compare with AvgOffset */
        if(torsensor_stTorSensorCof.uwTorqueNowAllHasValueFlg == TRUE)
        {
            AverageOffset = ((SLONG)torsensor_stTorSensorCof.uwTorqueOffsetNow1 + torsensor_stTorSensorCof.uwTorqueOffsetNow2 +
                            torsensor_stTorSensorCof.uwTorqueOffsetNow3 + torsensor_stTorSensorCof.uwTorqueOffsetNow4)>>2;
        }
        else
        {
            AverageOffset = torsensor_stTorSensorCof.uwTorqueOffsetOrign;
        }
        if(abs((SWORD)torsensor_stTorSensorCof.uwTorqueOffsetPowerUp - AverageOffset) > 200)
        {
            torsensor_stTorSensorCof.uwTorqueOffset = AverageOffset;
        }
        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;

            torsensor_stTorSensorCof.uwTorqueOffset = torsensor_stTorSensorCof.uwTorqueOffsetPowerUp;
        }

    }
#elif (TORSENSOR_USEMOL == TORSENSOR_USEAUTOZERO)
    //分别读三个力矩传感器零点
    TorqueSensorOffSetAuto();
    torsensor_stTorSensorCof.uwTorqueOffsetPowerUp = torsensor_stTorSensorCof.uwTorqueOffset;
    torsensor_stTorSensorCof.uwTorqueOffsetOrign = torsensor_stTorSensorCof.uwTorqueOffset;
    cp_stFlg.ParaSaveEEFlg = TRUE;
    cp_stFlg.ParaUpdateFlg = TRUE;
    MC_UpcInfo.stSensorInfo.uwSaveFlg = TRUE;
#endif

    torsensor_stTorSensorCof.uwSensorVolPerTorqDefault = TORQUE_VOLTAGE_PER_NM;
    
    torsensor_stTorSensorCof.uwSensorVolPerTorq1 =
        (((ULONG)3300 * (torsensor_stTorSensorCof.uwBikeTorStep1ADC - torsensor_stTorSensorCof.uwTorqueOffset)) >> 12) *10 /
        (torsensor_stTorSensorCof.uwBikeTorStep1RealNm - 0);
    
    torsensor_stTorSensorCof.uwSensorVolPerTorq2 =
        (((ULONG)3300 * (torsensor_stTorSensorCof.uwBikeTorStep2ADC - torsensor_stTorSensorCof.uwBikeTorStep1ADC)) >> 12) *10 /
        (torsensor_stTorSensorCof.uwBikeTorStep2RealNm - torsensor_stTorSensorCof.uwBikeTorStep1RealNm);
    
    torsensor_stTorSensorCof.uwSensorVolPerTorq3 =
        (((ULONG)3300 * (torsensor_stTorSensorCof.uwBikeTorStep3ADC - torsensor_stTorSensorCof.uwBikeTorStep2ADC)) >> 12) *10 /
        (torsensor_stTorSensorCof.uwBikeTorStep3RealNm - torsensor_stTorSensorCof.uwBikeTorStep2RealNm);
    
    torsensor_stTorSensorCof.uwSensorVolPerTorq4 =
        (((ULONG)3300 * (torsensor_stTorSensorCof.uwBikeTorStep4ADC - torsensor_stTorSensorCof.uwBikeTorStep3ADC)) >> 12) *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; // 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; // 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; // 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; // 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);   
    
}

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

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

void torsensor_voTorADC(void) // need to match ADC_StartConversion(ADC1);
{
    if (torsensor_stTorSensorOut.blTorqueErrorFlg == TRUE)
    {
        torsensor_stTorSensorOut.uwTorquePu = 0;
        #ifdef TORSENSOR3NUM
        if( ((TORQUEVOLREG1() < 4000) && (TORQUEVOLREG1() > 10)) &&
            ((TORQUEVOLREG2() < 4000) && (TORQUEVOLREG2() > 10)) &&
            ((TORQUEVOLREG3() < 4000) && (TORQUEVOLREG3() > 10))
          )
        #else
        if((TORQUEVOLREG1() < 4000) && (TORQUEVOLREG1() > 10))
        #endif
        {
            torsensor_stTorSensorOut.uwTorqueErrorCnt++;
            if (torsensor_stTorSensorOut.uwTorqueErrorCnt > 1000)
            {
                torsensor_stTorSensorOut.blTorqueErrorFlg = FALSE;
                torsensor_voTorSensorInit();
            }
        }
        else
        {
            torsensor_stTorSensorOut.uwTorqueErrorCnt = 0;
        }
    }
    else
    {
#if (TORSENSOR_USEMOL == TORSENSOR_USEDEFAULT)
        torsensor_stTorSensorOut.uwTorqueReg = TORQUEVOLREG();
        torsensor_stTorSensorOut.uwTorquePu =
            (((SQWORD)abs((SWORD)(torsensor_stTorSensorOut.uwTorqueReg) - torsensor_stTorSensorCof.uwTorqueOffset)) *
             torsensor_stTorSensorCof.uwTorqueReg2PuDefault) >> 10; // Q14
#elif (TORSENSOR_USEMOL == TORSENSOR_USEEE)
        torsensor_stTorSensorOut.uwTorqueReg = TORQUEVOLREG();
        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;
        }
#elif (TORSENSOR_USEMOL == TORSENSOR_USEAUTOZERO)
        #ifdef TORSENSOR3NUM
        torsensor_stTorSensorOut.uwTorqueReg = abs( (SWORD)(TORQUEVOLREG1() - torsensor_stTorSensorCof.uwTorqueOffsetNow1) +
                                                    (SWORD)(TORQUEVOLREG2() - torsensor_stTorSensorCof.uwTorqueOffsetNow2) +
                                                    (SWORD)(TORQUEVOLREG3() - torsensor_stTorSensorCof.uwTorqueOffsetNow3)
                                                  ) / 3;
        #else
        torsensor_stTorSensorOut.uwTorqueReg = abs( (SWORD)(TORQUEVOLREG1() - torsensor_stTorSensorCof.uwTorqueOffsetNow1));
        #endif
        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
        #ifdef TORSENSOR3NUM
        if(((TORQUEVOLREG1() > 4000) || (TORQUEVOLREG1() < 10)) ||
           ((TORQUEVOLREG2() > 4000) || (TORQUEVOLREG2() < 10)) ||
           ((TORQUEVOLREG3() > 4000) || (TORQUEVOLREG3() < 10))
          )
        #else
        if((TORQUEVOLREG1() > 4000) || (TORQUEVOLREG1() < 10))
        #endif
        {
            torsensor_stTorSensorOut.uwTorqueErrorCnt++;
            if (torsensor_stTorSensorOut.uwTorqueErrorCnt > 5000)
            {
                torsensor_stTorSensorOut.blTorqueErrorFlg = TRUE;
                torsensor_stTorSensorOut.uwTorquePu = 0;
                torsensor_stTorSensorOut.uwTorqueErrorCnt = 0;
                torsensor_stTorSensorOut.uwTorqueLPFPu = 0;
            }
        }
        else
        {
            torsensor_stTorSensorOut.uwTorqueErrorCnt = 0;
        }
    }
}

void TorqueSensorRegFltCal(void)
{
    #ifdef TORSENSOR3NUM
    TorqueSensorRegFlt.SensorPer1 = TORQUEVOLREG1();
    TorqueSensorRegFlt.SensorPer2 = TORQUEVOLREG2();
    TorqueSensorRegFlt.SensorPer3 = TORQUEVOLREG3();
    TorqueSensorRegFlt.SensorFltSum1 += ((TorqueSensorRegFlt.SensorPer1 << 10) - TorqueSensorRegFlt.SensorFltSum1) >> 8;
    TorqueSensorRegFlt.SensorFlt1 = TorqueSensorRegFlt.SensorFltSum1 >> 10;
    TorqueSensorRegFlt.SensorFltSum2 += ((TorqueSensorRegFlt.SensorPer2 << 10) - TorqueSensorRegFlt.SensorFltSum2) >> 8;
    TorqueSensorRegFlt.SensorFlt2 = TorqueSensorRegFlt.SensorFltSum2 >> 10;
    TorqueSensorRegFlt.SensorFltSum3 += ((TorqueSensorRegFlt.SensorPer3 << 10) - TorqueSensorRegFlt.SensorFltSum3) >> 8;
    TorqueSensorRegFlt.SensorFlt3 = TorqueSensorRegFlt.SensorFltSum3 >> 10;
    #else
    TorqueSensorRegFlt.SensorPer1 = TORQUEVOLREG1();
    TorqueSensorRegFlt.SensorFltSum1 += ((TorqueSensorRegFlt.SensorPer1 << 10) - TorqueSensorRegFlt.SensorFltSum1) >> 8;
    TorqueSensorRegFlt.SensorFlt1 = TorqueSensorRegFlt.SensorFltSum1 >> 10;
    #endif
}

//零点更新
void TorqueSensorOffSetAuto()
{
    static UBYTE SetFlag = 0;
    static BOOL OffSet_RefreshFlag = FALSE;
    if(SetFlag == 0) //开机后执行一次获取初始值
    {
        #ifdef TORSENSOR3NUM
        torsensor_stTorSensorCof.uwTorqueOffsetNow1 = TORQUEVOLREG1();
        torsensor_stTorSensorCof.uwTorqueOffsetNow2 = TORQUEVOLREG2();
        torsensor_stTorSensorCof.uwTorqueOffsetNow3 = TORQUEVOLREG3();
        #else
        torsensor_stTorSensorCof.uwTorqueOffsetNow1 = TORQUEVOLREG1();
        #endif
        SetFlag = 1;
        OffSet_RefreshFlag = TRUE;
    }
    else //更新为较小值
    {
        #ifdef TORSENSOR3NUM
        if(TorqueSensorRegFlt.SensorFlt1 < torsensor_stTorSensorCof.uwTorqueOffsetNow1)
        {
            torsensor_stTorSensorCof.uwTorqueOffsetNow1 = TorqueSensorRegFlt.SensorFlt1;
            OffSet_RefreshFlag = TRUE;
        }
        if(TorqueSensorRegFlt.SensorFlt2 < torsensor_stTorSensorCof.uwTorqueOffsetNow2)
        {
            torsensor_stTorSensorCof.uwTorqueOffsetNow2 = TorqueSensorRegFlt.SensorFlt2;
            OffSet_RefreshFlag = TRUE;
        }
        if(TorqueSensorRegFlt.SensorFlt3 < torsensor_stTorSensorCof.uwTorqueOffsetNow3)
        {
            torsensor_stTorSensorCof.uwTorqueOffsetNow3 = TorqueSensorRegFlt.SensorFlt3;
            OffSet_RefreshFlag = TRUE;
        }
        #else
        if(TorqueSensorRegFlt.SensorFlt1 < torsensor_stTorSensorCof.uwTorqueOffsetNow1)
        {
            torsensor_stTorSensorCof.uwTorqueOffsetNow1 = TorqueSensorRegFlt.SensorFlt1;
            OffSet_RefreshFlag = TRUE;
        }
        #endif
    }
    if(OffSet_RefreshFlag == TRUE)
    {
        torsensor_stTorSensorCof.uwTorqueOffset = 0;
        torsensor_stTorSensorCof.uwTorqueOffsetPowerUp = torsensor_stTorSensorCof.uwTorqueOffset;
        OffSet_RefreshFlag = FALSE;
    }
}

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

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