/** * @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; UWORD TorqueTemp[1024], TorqueFltTemp[1024]; UWORD TorqueTempCnt = 0; //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.uwTorqueReg, &scm_stTorSensorLpf); torsensor_stTorSensorOut.uwTorqueLPFPu = scm_stTorSensorLpf.slY.sw.hi; //测试力矩噪声 TorqueTemp[TorqueTempCnt] = torsensor_stTorSensorOut.uwTorqueReg; TorqueFltTemp[TorqueTempCnt] = torsensor_stTorSensorOut.uwTorqueLPFPu; TorqueTempCnt++; if(TorqueTempCnt >= 1024) TorqueTempCnt = 0; 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! *************************************************************************/