motor_ctl-1/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 "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;
LPF_OUT   torsensor_test_Lpf;
TORQUESENSOR_OUT torsensor_stTorSensorOut = TORQUESENSOR_OUT_DEFAULT;

//volatile  SWORD TorqOffsetReg[TORQ_OFFSET_NUM]= {
const  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
};

const 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
};

  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
};
  SWORD TorqSencitiveTemp[TORQ_OFFSET_NUM]= {
-200,    //unit: 0.1C
0,    
200,    
400,    
600,     
800,     
1000      
};
//static 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;//, 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;  
            }

        }
#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);   
    /////torq lpf test////
    //mth_voLPFilterCoef(100000/8, 1000, &torsensor_test_Lpf.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;
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  
    tst_dynOffsetLpf.slY.sw.hi = hw_uwADC1[0];
        /* Torque Sensor limit coef */
   
}

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

void torsensor_voTorADC(void) // need to match ADC_StartConversion(ADC1);
{
    if (torsensor_stTorSensorOut.blTorqueErrorFlg == TRUE)
    {
        torsensor_stTorSensorOut.uwTorquePu = 0;
        torsensor_stTorSensorOut.uwTorqueReg = TORQUEVOLREG();//DL_ADC12_getMemResult(ADC12_1_INST, DL_ADC12_MEM_IDX_1);
        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 = TORQUEVOLREG();//DL_ADC12_getMemResult(ADC12_1_INST, DL_ADC12_MEM_IDX_1);
#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!
*************************************************************************/