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

/**
 * @file AssistCurve.c
 * @author Zhang, Kai(zhangkai71@midea.com)
 * @brief
 * @version 0.1
 * @date 2021-11-15
 *
 * @copyright Copyright (c) 2021
 *
 */
/******************************
 *
 * Include File
 *
 ******************************/
#include "string.h"
#include "Syspar.h"
#include "user.h"
#include "AssistCurve.h"
#include "bikebrake.h"
#include "Cadence.h"
#include "flash_master.h"

/******************************
 *
 *  Parameter
 *
 ******************************/
ASS_FSM_STATUS Ass_FSM;
ASS_PER_IN   ass_stCalIn = TORQUE_CAL_IN_DEFAULT;
ASS_PER_COEF ass_stCalCoef;
ASS_PER_OUT  ass_stCalOut;

ASS_PARA_CONFIGURE ass_stParaCong;
ASS_PARA_SET       ass_stParaSet;

ASS_CURLIM_COEF ass_stCurLimCoef = ASS_LIM_DEFAULT;
ASS_CURLIM_OUT  ass_stCurLimOut;
ASS_LIMIT_ACCORDING_VOL_COF ass_stCurLimCalBMSCoef;
ASS_LIMIT_ACCORDING_VOL_OUT ass_stCurLimitCalBMSOut;

ASR_SPDPI_IN  asr_stTorqSpdPIIn;
ASR_SPDPI_OUT asr_stTorqSpdPIOut;
ASR_SPDPI_COF asr_stTorqSpdPICoef;
ASR_SPDPI_COFIN asr_stTorqSpdPICoefIn;

ASS_TORQ_PI_IN ass_stTorqPIIn;
ASS_TORQ_PI_OUT ass_stTorqPIOut;

SWORD ass_swTorqMafBuf[64];
MAF_IN ass_stTorqMafValue = {0, 32, 0, 0, ass_swTorqMafBuf, 0, FALSE};
SWORD ass_swUqLimMafBuf[64];
MAF_IN ass_stUqLimMafValue = {0, 64, 0, 0, ass_swUqLimMafBuf, 0, FALSE};

static TOR2CURRENT_CAL_COEF ass_Tor2CurCalCoef;
static UWORD StartUpGainArray[5] = START_GAIN_DEFAULT;
static UWORD LinerAssist[5] = ASS_LINER_TORQUE_DEFAULT;
/******************************
 *
 *  Function
 *
 ******************************/
/**
 * @brief  Three order polynomial  Y = a*X^3 + b*X^2 + c*x +d
 *
 * @param coef polynomial coefficient a, b, c, d
 * @param Value polynomial input value X
 * @param Qnum  polynomial input Q type
 * @return UWORD  polynomial output Y
 */
static SLONG ass_slPolynomial(const POLY_COEF *coef, const SWORD *value, UWORD Qnum)
{
    SLONG out;
    SLONG temp_a, temp_b, temp_c;
    /* out = a * x ^ 3 + b * x ^ 2 + c * x + d */
    temp_a = (SLONG)((((((SQWORD)coef->a * *value >> 12) * *value) >> (SWORD)Qnum) * *value) >> (SWORD)Qnum); // Qx+Q12-Q12+Qx-Qx+Qx-Qx=Qx
    temp_b = (SLONG)((((SQWORD)coef->b * *value >> 12) * *value) >> (SWORD)Qnum);                      // Qx+Q12-Q12+Qx-Qx=Qx
    temp_c = (SLONG)((SQWORD)coef->c * *value >> 12);                                           // Qx+Q12-Q12=Qx

    out = temp_a + temp_b + temp_c + coef->d;
    out = (SLONG)out;
    return out;
}

/**
 * @brief   Y = z*(x-h)^2 + k to  Y = a*X^3 + b*X^2 + c*x +d
 *
 * @param coef original point coefficient z, h, k
 * @return POLY_COEF  a, b, c, d
 */
//static POLY_COEF ass_stPolynomialcenter(ORIG_COEF *coef)
//{
//    POLY_COEF out;
//    /* a = 0; b = z; c = -2ha; d= ah^2 +k*/
//    out.a = (SQWORD)0;                                                                // Q12
//    out.b = (SQWORD)coef->z;                                                          // Q12
//    out.c = -(((SQWORD)2 * coef->h * coef->z) >> 12);                                 // Q12
//    out.d = (((((SQWORD)coef->z * coef->h) >> 12) * coef->h) >> 12) + (SLONG)coef->k; // Q12
//    return out;
//}

/**
 * @brief  Torque to Current when Id = 0;
 *         Te = 1.5p*iq*fai -> iq = te/(1.5*p*fai)
 * @param coef polynomial coefficient a, b, c, d
 * @param Value polynomial input value X
 * @param Qnum  polynomial input Q type
 * @return UWORD  polynomial output Y
 */
static SWORD ass_swTorq2CurPu(SWORD Tor)
{
    SWORD swCurrentPu;
    SWORD swMotorTorqueNotPu;

    swMotorTorqueNotPu = (SWORD)(((SLONG)Tor * (SWORD)TORQUEBASE / (SWORD)ass_stParaCong.uwMechRationMotor) >> 7); // Q14-Q7 = Q7 0.1Nm   Not Pu
    swCurrentPu = (SWORD)(((SLONG)swMotorTorqueNotPu * ass_Tor2CurCalCoef.swCalCoefINV) * 10 / IBASE); // Q7+Q7 = Q14;  0.1Nm/0.01A

    return swCurrentPu;
}

/**
 * @brief  
 *
 * @param 
 * @return 
 */
static void ass_voAssistModeSelect(void) // 上电运行一次or助力参数更新后，AssistCoef需要重新计算
{
    UWORD TempAssit;
    UWORD TempGear, gear;
//    if (ass_stParaSet.uwAsssistSelectNum == 1) // OBC：更换成EE参数
//    {
//        TempAssit = ass_stParaCong.uwAssistSelect1;
//    }
//    else if (ass_stParaSet.uwAsssistSelectNum == 2)
//    {
//        TempAssit = ass_stParaCong.uwAssistSelect2;
//    }
//    else
//    {
//        TempAssit = ASSISTMOD_SELECT_DEFAULT;
//    }
    if (ass_stParaCong.uwStartMode == 1) // OBC：更换成EE参数
    {
        TempAssit = ASSISTMOD_SELECT_DEFAULT;
    }
    else if (ass_stParaCong.uwStartMode  == 2)
    {
        TempAssit = ass_stParaCong.uwAssistSelect1;
    }
    else if (ass_stParaCong.uwStartMode  == 3)
    {
        TempAssit = ass_stParaCong.uwAssistSelect2;
    }
    else
    {
        TempAssit = ASSISTMOD_SELECT_DEFAULT;
    }
    
    for (gear = 0; gear < 5; gear++)
    {
        TempGear = gear * 3 + ((TempAssit >> (UWORD)(gear << 1)) & 0x0003);
        memcpy(&ass_stCalCoef.uwTorqueAssGain[(gear + 1)], &flash_stPara.slTorqAssGain[TempGear], sizeof(POLY_COEF));
    }
    memcpy(&ass_stCalCoef.uwCadencAsseGain[1], &flash_stPara.slCadAssGain[0], sizeof(flash_stPara.slCadAssGain));
}


/**
 * @brief  Para from EE Init
 *
 * @param void
 * @return void
 */
void ass_voAssitEEInit(void)
{
    ass_stParaCong.uwWheelPerimeter = BIKE_WHEEL_PERIMETER; //   Q0   0.1CM
    ass_stParaCong.uwCadPulsePerCirc = CADENCE_PULSES_PER_CIRC;
    ass_stParaCong.uwMechRationMotor = 35; //   Q0

    ass_stParaCong.uwAssistMaxSpdKmH = BIKE_SPEED_IQLIMIT_THRESHOLD1;
    ass_stParaCong.uwThrottleMaxSpdKmH = BIKE_SPEED_THROTTLE_MAX;
    ass_stParaCong.uwNmFrontChainring = BIKE_FRONTTEETH_NMB;    // front gear
    ass_stParaCong.uwNmBackChainring = BIKE_BACKTEETH_NMB;     // min number of back gear
    ass_stParaCong.uwAssistSelect1 = BIKE_ASSIST_MODE1;
    ass_stParaCong.uwAssistSelect2 = BIKE_ASSIST_MODE2;
    ass_stParaCong.uwLightVoltage = BIKE_LIGHT_VOLTAGE;
    ass_stParaCong.swDeltPerimeter = BIKE_WHEEL_SIZE_ADJUST;
    ass_stParaCong.uwStartMode = BIKE_START_MODE;
    ass_stParaCong.uwAutoPowerOffTime = BIKE_AUTO_POWER_OFF_TIME;

    ass_stParaSet.uwStartupCoef = 8194;       // Q12 percentage Min 1-4096  1.5-6144
    ass_stParaSet.uwStartupCruiseCoef = 4096; // Q12 percentage Min 1-4096  1-6144

    ass_stParaSet.uwAssistStartNm = TORQUE_START_THRESHOLD;
    ass_stParaSet.uwAssistStopNm = TORQUE_STOP_THRESHOLD;
    ass_stParaSet.uwStartUpGainStep = 25;
    ass_stParaSet.uwStartUpCadNm = CADENCE_NUMBERS_PULSES >> 1; // 0.5 circle
    ass_stParaSet.uwTorLPFCadNm = CADENCE_NUMBERS_PULSES >> 1;  // 0.5 circle
    ass_stParaSet.uwSpeedAssistSpdRpm = BIKE_SPD_MOTOR_CONSTANT_COMMAND;
    ass_stParaSet.uwSpeedAssistIMaxA = BIKE_SPD_MOTOR_CURRENT_MAX;
    ass_stParaSet.uwAssistLimitBikeSpdStart = BIKE_SPEED_IQLIMIT_THRESHOLD1;
    ass_stParaSet.uwAssistLimitBikeSpdStop = BIKE_SPEED_IQLIMIT_THRESHOLD2;

    ass_stParaSet.uwCadenceWeight = 1229;                            // Q12 percentage
    ass_stParaSet.uwTorWeight = Q12_1 ; // Q12 percentage
    ass_stParaSet.uwTorAssAjstGain = 4096;                           // Q12 percentage
    ass_stParaSet.uwCadenceAssAjstGain = 4094;                       // Q12 percentage
    ass_stParaSet.uwAsssistSelectNum = 1;

    ass_stParaSet.uwSpdRegion[0] = 8192;  // Q15 1500rpm
    ass_stParaSet.uwSpdRegion[1] = 16384; // Q15 3000rpm
    ass_stParaSet.uwSpdRegion[2] = 21845; // Q15 4000rpm
    ass_stParaSet.uwSpdRegionGain[0] = 4094;
    ass_stParaSet.uwSpdRegionGain[1] = 4094;
    ass_stParaSet.uwSpdRegionGain[2] = 4094;
}

/**
 * @brief  Three order polynomial  Y = a*X^3 + b*X^2 + c*x +d
 *
 * @param coef polynomial coefficient a, b, c, d
 * @param Value polynomial input value X
 * @param Qnum  polynomial input Q type
 * @return UWORD  polynomial output Y
 */
LPF_OUT ass_pvt_stCurLpf;
void ass_voAssitCoef(void)
{
    /*状态机初始化*/
    Ass_FSM = StopAssit;
    /*电机限制初始化*/
    ass_stParaCong.uwCofCurMaxPu = ((ULONG)BIKE_ASS_MOTOR_CURRENT_MAX << 14) / IBASE; // Q14
    ass_stParaCong.uwMotorPoles = cp_stMotorPara.swMotrPolePairs;
    ass_stParaCong.uwCofTorMaxPu = (UWORD)(((ULONG)cp_stMotorPara.swTorMax << 14) / TORQUEBASE);           // Q14
    ass_stParaCong.uwBikeAssTorMaxPu = ass_stParaCong.uwCofTorMaxPu * ass_stParaCong.uwMechRationMotor; // Q14;
    /*速度环参数初始化*/
    asr_stTorqSpdPICoefIn.uwUbVt = VBASE;
    asr_stTorqSpdPICoefIn.uwIbAp = IBASE;
    asr_stTorqSpdPICoefIn.uwFbHz = FBASE;
    asr_stTorqSpdPICoefIn.uwFTbsHz = EVENT_1MS_HZ;
    asr_stTorqSpdPICoefIn.uwPairs = cp_stMotorPara.swMotrPolePairs;
    asr_stTorqSpdPICoefIn.uwMtJm = cp_stMotorPara.swJD;
    asr_stTorqSpdPICoefIn.uwMtFlxWb = cp_stMotorPara.swFluxWb;
    asr_stTorqSpdPICoefIn.uwMcoef = 5;//cp_stControlPara.swAsrPIM;
    asr_stTorqSpdPICoefIn.uwWvcHz = 10;//cp_stControlPara.swAsrPIBandwidth;
    asr_stTorqSpdPICoefIn.uwRatioJm = cp_stControlPara.swAsrSpdInerRate;
    asr_voSpdPICoef(&asr_stTorqSpdPICoefIn, &asr_stTorqSpdPICoef);
    /*电流限幅计算*/
    ass_stCurLimCalBMSCoef.uwIqLimitInitAbs = ass_stParaCong.uwCofCurMaxPu; // Q14
    ass_stCurLimCalBMSCoef.uwIqLimitStartSoc = 35;
    ass_stCurLimCalBMSCoef.uwIqLimitEndSoc = 0;
    ass_stCurLimCalBMSCoef.swIqLImitK =
    		(SWORD)ass_stCurLimCalBMSCoef.uwIqLimitInitAbs / ((SWORD)ass_stCurLimCalBMSCoef.uwIqLimitStartSoc - (SWORD)ass_stCurLimCalBMSCoef.uwIqLimitEndSoc);
    /*助力曲线初始化*/
    ass_voAssistModeSelect();

    /*助力启动阈值初始化*/
    ass_stCalCoef.uwAssThreshold = (UWORD)(((ULONG)ass_stParaSet.uwAssistStartNm << 14) / TORQUEBASE);    // Q14
    ass_stCalCoef.uwAssStopThreshold = (UWORD)(((ULONG)ass_stParaSet.uwAssistStopNm << 14) / TORQUEBASE); // Q14;

    /*助力系数初始化*/
    ass_stCalCoef.StartFlag = 0;
    ass_stCalCoef.swSmoothGain = 0;                                     // Q12
    ass_stCalCoef.uwStartUpTargetGain = 0;                              // Q12
    ass_stCalCoef.uwStartUpGainAddStep = ass_stParaSet.uwStartUpGainStep; // 25 Q12
    if (ass_stCalCoef.uwStartUpGainAddStep < 1)
    {
        ass_stCalCoef.uwStartUpGainAddStep = 1;
    }
    if (ass_stCalCoef.uwStartUpGainAddStep > 50)
    {
        ass_stCalCoef.uwStartUpGainAddStep = 50;
    }

    /*设置启动到正常助力最少踏频数*/
    ass_stCalCoef.uwStartUpTimeCadenceCnt = ass_stParaSet.uwStartUpCadNm;
    if (ass_stCalCoef.uwStartUpTimeCadenceCnt < (CADENCE_NUMBERS_PULSES >> 3))
    {
        ass_stCalCoef.uwStartUpTimeCadenceCnt = CADENCE_NUMBERS_PULSES >> 3;
    }
    if (ass_stCalCoef.uwStartUpTimeCadenceCnt > CADENCE_NUMBERS_PULSES)
    {
        ass_stCalCoef.uwStartUpTimeCadenceCnt = CADENCE_NUMBERS_PULSES;
    }
    /*设置滑动平均滤波踏频数*/
    ass_stTorqMafValue.uwLength = ass_stParaSet.uwTorLPFCadNm;

    ass_stCalCoef.swCadanceGain = 0;
    ass_stCalCoef.uwSwitch1TorqThreshold = ((ULONG)TORQUE_SWITCH1_THRESHOLD << 14) / TORQUEBASE;
    ass_stCalCoef.uwSwitch2TorqThreshold = ((ULONG)TORQUE_SWITCH2_THRESHOLD << 14) / TORQUEBASE;
    ass_stCalCoef.ulStartupDeltInv = ((ULONG)1 << 28) / (ass_stCalCoef.uwSwitch2TorqThreshold - ass_stCalCoef.uwSwitch1TorqThreshold); // Q14;

    /*初始化计数*/
    ass_stCalCoef.uwCadencePeriodCNT = 0;
    ass_stCalCoef.swCadanceCNT = 0;
    ass_stCalCoef.sw2StopCNT = 0;
    ass_stCalCoef.swAss2SpdCNT = 0;

    /*配置速度环参数*/
    ass_stCalCoef.uwSpeedConstantCommand = (UWORD)(((ULONG)ass_stParaSet.uwSpeedAssistSpdRpm << 15) / ((ULONG)FBASE * 60 / ass_stParaCong.uwMotorPoles));
    ass_stCalCoef.swSpdLoopAbsCurMax = ((SWORD)ass_stParaSet.uwSpeedAssistIMaxA << 14) / IBASE;

    ass_stCalCoef.swSpeedlimtrpm = -100;
    ass_stCalCoef.swBikeSpeedGain = 0;

    /*设置电流限幅*/
    ass_stCalCoef.uwCurrentMaxPu = ass_stParaCong.uwCofCurMaxPu;
    ass_stCalCoef.swCurrentmax_torAssPu = (SWORD)(((SLONG)ass_stCalCoef.uwCurrentMaxPu * (SWORD)ass_stParaSet.uwTorWeight) >> 12);          // Q14
    ass_stCalCoef.swCurrentmax_cadAssPu = (SWORD)(((SLONG)ass_stCalCoef.uwCurrentMaxPu * (SWORD)ass_stParaSet.uwCadenceWeight )>> 12); // Q14

    /*初始化标志*/
    ass_stCalCoef.blAssistflag = FALSE;

    ass_stCalOut.swTorAssistSum1 = 0;
    ass_stCalOut.swTorAssistSum2 = 0;
    ass_stCalOut.swTorAss2CurrentTemp = 0;
    ass_stCalOut.swCadAss2CurrentTemp = 0;
    ass_stCalOut.swTorAssistCurrentTemp = 0;
    ass_stCalOut.swTorSpdLoopCurrentTemp = 0;
    ass_stCalOut.swTorAssistCurrent = 0;
    ass_stCalOut.swSpeedRef = 0;
    ass_stCalOut.swCadSpd2MotSpd = 0;

    ass_stCurLimCoef.uwLimitGain[0] = 0; // Q10 percentage of max Current
    ass_stCurLimCoef.uwLimitGain[1] = 400;
    ass_stCurLimCoef.uwLimitGain[2] = 682;
    ass_stCurLimCoef.uwLimitGain[3] = 910;
    ass_stCurLimCoef.uwLimitGain[4] = 1024;
    ass_stCurLimCoef.uwLimitGain[5] = 1024;
    ass_stCurLimCoef.uwSpdThresHold = 21845;

    /*设置车速限幅*/
    ass_stCurLimCoef.uwBikeSpdThresHold1 = (UWORD)(((UQWORD)1000 << 20) * ass_stParaSet.uwAssistLimitBikeSpdStart /
                                         ((UQWORD)36 * (ass_stParaCong.uwWheelPerimeter + (UWORD)ass_stParaCong.swDeltPerimeter) * FBASE)); // Q20    3216 = Q10(3.1415926)
    ass_stCurLimCoef.uwBikeSpdThresHold2 = (UWORD)(((UQWORD)1000 << 20) * ass_stParaSet.uwAssistLimitBikeSpdStop /
                                         ((UQWORD)36 * (ass_stParaCong.uwWheelPerimeter + (UWORD)ass_stParaCong.swDeltPerimeter) * FBASE)); // Q20    3216 = Q10(3.1415926)

    ass_stCurLimCoef.ulBikeSpdDeltInv = (ULONG)(((UQWORD)1 << 20) / (ass_stCurLimCoef.uwBikeSpdThresHold2 - ass_stCurLimCoef.uwBikeSpdThresHold1)); // Q20;
    ass_stCurLimCoef.uwBikeSpdIqLimitK =
    		(UWORD)((((ULONG)ass_stCurLimCoef.uwBikeSpdThresHold2 - ass_stCurLimCoef.uwBikeSpdThresHold1) << 8) / ass_stParaCong.uwCofCurMaxPu); // Q28-q14 = Q14;

    /*设置转矩电流标定系数*/
    ass_Tor2CurCalCoef.uwMotorFluxWb = cp_stMotorPara.swFluxWb; // 0.001mWb
    ass_Tor2CurCalCoef.uwMotprPolePairs = ass_stParaCong.uwMotorPoles;
    ass_Tor2CurCalCoef.swCalCoefINV =
        (((SLONG)1 << 7) * 1000 * 1000) /
        (((SLONG)3 * ass_Tor2CurCalCoef.uwMotorFluxWb * ass_Tor2CurCalCoef.uwMotprPolePairs) >> 1); // Q7 Not Pu    // 1/(1.5p*fai);
    
    mth_voLPFilterCoef(1000000 / 25, EVENT_1MS_HZ, &ass_pvt_stCurLpf.uwKx); //100Hz
    ass_pvt_stCurLpf.slY.sl = 0;  
}

/**
 * @brief  Three order polynomial  Y = a*X^3 + b*X^2 + c*x +d
 *
 * @param coef polynomial coefficient a, b, c, d
 * @param Value polynomial input value X
 * @param Qnum  polynomial input Q type
 * @return UWORD  polynomial output Y
 */
//void ass_voAssitTorqPIInit(void)
//{
//    ass_stTorqPIOut.slIRefPu = 0;
//    ass_stTorqPIOut.swErrZ1Pu = 0;
//    ass_stTorqPIOut.swIRefPu = 0;
//}
//
//void ass_voAssitTorqPI(const ASS_TORQ_PI_IN *in, ASS_TORQ_PI_OUT *out)
//{
//    SLONG slErrPu, slDeltaErrPu; 
//    SLONG  slIpPu, slIiPu;     
//    SLONG  slImaxPu, slIminPu; 
//    SQWORD sqIRefPu, sqIpPu;
//    UWORD uwKpPu = 5000, uwKitPu = 0;   // uwKpPu(Q12), uwKitPu(Q15)
//    
////    uwKpPu = ass_stParaSet.uwSpeedAssistIMaxA;  
////    uwKitPu =  ass_stParaSet.uwStartUpCadNm;    
//    
//    slImaxPu = (SLONG)in->swImaxPu << 15; // Q14+Q15=Q29
//    slIminPu = (SLONG)in->swIminPu << 15; // Q14+Q15=Q29
//
//    slErrPu = in->swTorqRefPu - in->swTorqFdbPu; // Q14
//    
//    if (slErrPu > 32767)
//    {
//        slErrPu = 32767;
//    }
//    else if (slErrPu < -32768)
//    {
//        slErrPu = -32768;
//    }
//    else
//    {
//        /* Nothing */
//    }
//    //slDeltaErrPu = slErrPu - out->swErrZ1Pu; // Q14
//    slDeltaErrPu = slErrPu;
//    if (slDeltaErrPu > 32767)
//    {
//        slDeltaErrPu = 32767;
//    }
//    else if (slDeltaErrPu < -32768)
//    {
//        slDeltaErrPu = -32768;
//    }
//    else
//    {
//        /* Nothing */
//    }
//
//    slIpPu = slDeltaErrPu * uwKpPu; // Q14+Q12=Q26
//    sqIpPu = (SQWORD)slIpPu << 3;
//
//    slIiPu = slErrPu * uwKitPu; // Q14+Q15=Q29
//
//    //sqIRefPu = sqIpPu + (SQWORD)slIiPu + (SQWORD)out->slIRefPu; // Q29  
//    sqIRefPu = sqIpPu;
//      
//    if (sqIRefPu > slImaxPu)
//    {
//        out->slIRefPu = slImaxPu;
//    }
//    else if (sqIRefPu < slIminPu)
//    {
//        out->slIRefPu = slIminPu;
//    }
//    else
//    {
//        out->slIRefPu = sqIRefPu;
//    }
//    out->swIRefPu = out->slIRefPu >> 15; // Q29-Q15=Q14
//    out->swErrZ1Pu = (SWORD)slErrPu;
//}

static SWORD ass_pvt_swVoltCnt=0;
static UWORD ass_pvt_uwTorqAccCnt=0,ass_pvt_uwTorqDecCnt=0,ass_pvt_uwSpd2TorqCnt=0;
static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目圈复杂度无法更改，后续避免" */
{  
    SLONG slTeTorAssitTmpPu,slTeTorAssitLinerPu,slTeCadAssitTmpPu;
    SWORD swTeTorAssitPu1, swTeTorAssitPu2;
    SWORD swTeCadAssitPu1, swTeCadAssitPu2;
    SWORD swTmpSpdtoTorqCur;
    SLONG slTmpSmoothCur;
    SWORD swTorqCmd1, swTorqCmd, swCadCmd;
    UWORD uwTorqAccStep = 50,uwTorqDecStep = 80;
    SWORD swCurSwitch = 0;
    SWORD swTmpVoltPu,swTmpVoltPu2;
    SLONG slSpdErr,slTmpVoltLim;
    SWORD swSpdKpPu = 500; //Q10
    UWORD uwVoltAccStep = 1, uwVoltDecStep = 3;
    UWORD uwTmpStopCnt = 0;

//    SLONG slTmp_a1, slTmp_b1, slTmp_c1;

  
    /* Select Torq Growth Rate by Bike Gear */
    if (ass_stCalIn.uwGearSt == 1)
    {
        uwTorqAccStep = 50;
    }
    else if(ass_stCalIn.uwGearSt == 2)
    {
        uwTorqAccStep = 100;
    }
    else if(ass_stCalIn.uwGearSt == 3)
    {
        uwTorqAccStep = 120;
    }
    else if(ass_stCalIn.uwGearSt == 4)
    {
        uwTorqAccStep = 150;
    }   
    else if(ass_stCalIn.uwGearSt == 5)
    {
        uwTorqAccStep = 150;
    }
    else
    {
    	//do nothing
    }
    uwTorqDecStep = 80;
    
    /* Select TorqRef: LPFTorq or MAFTorq */
    swTorqCmd1 = (SWORD)(((SLONG)ass_stCalIn.uwtorque * ass_stCalCoef.swTorqFilterGain >> 14) +
               ((SLONG)ass_stCalIn.uwtorquelpf * (Q14_1 - ass_stCalCoef.swTorqFilterGain) >> 14)); //转矩指令滤波切换，由低通滤波到踏频相关的滑动平均滤波
    swTorqCmd = (SWORD)(((SLONG)swTorqCmd1 * ass_stCalCoef.swSmoothGain) >> 12);                             //转矩指令斜坡
    if (swTorqCmd > ass_stParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
    {
        swTorqCmd = (SWORD)ass_stParaCong.uwBikeAssTorMaxPu;
    }
   
    /*  Assist torque Cal using Assist Curve */
    slTeTorAssitTmpPu = (SLONG)(ass_slPolynomial(&ass_stCalCoef.uwTorqueAssGain[ass_stCalIn.uwGearSt], &swTorqCmd, 14)); // Q14  转矩助力曲线
    if(ass_stCalIn.uwGearSt == 5)
    {
        slTeTorAssitLinerPu = (((SLONG)swTorqCmd * LinerAssist[ass_stCalIn.uwGearSt-1] )>> 12) + 273;
    }
    else
    {
        slTeTorAssitLinerPu = (((SLONG)swTorqCmd * LinerAssist[ass_stCalIn.uwGearSt-1] )>> 12) + 273;
    }
   
    if (slTeTorAssitTmpPu < slTeTorAssitLinerPu)
    {
        slTeTorAssitTmpPu = slTeTorAssitLinerPu;
    }
    else
    {
        //do nothing;
    }
                                        
    swCadCmd = (SWORD)((((SLONG)ass_stCalIn.uwcadance * ass_stCalCoef.swSmoothGain) >> 12) * 10);  // 踏频指令斜坡
    slTeCadAssitTmpPu = ((SLONG)(ass_slPolynomial(&ass_stCalCoef.uwCadencAsseGain[ass_stCalIn.uwGearSt], &swCadCmd, 20))) >> 6; // Q20 - Q6 = Q14 //踏频助力曲线

    if (slTeTorAssitTmpPu > ass_stParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
    {
        slTeTorAssitTmpPu = ass_stParaCong.uwBikeAssTorMaxPu;
    }

    if (slTeCadAssitTmpPu > ass_stParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
    {
        slTeCadAssitTmpPu = ass_stParaCong.uwBikeAssTorMaxPu;
    }

    /* Select Assist Percent of Torq and Candence*/
    swTeTorAssitPu1 = (SWORD)(slTeTorAssitTmpPu * (SLONG)ass_stParaSet.uwTorAssAjstGain >> 12);     // Q14+Q12-Q12 = Q14; 用户可设置转矩比例
    swTeCadAssitPu1 = (SWORD)(slTeCadAssitTmpPu * (SLONG)ass_stParaSet.uwCadenceAssAjstGain >> 12); // Q14+Q12-Q12 = Q14; 用户可设置踏频比例
    ass_stCalOut.swTorAssistSum1 = swTeTorAssitPu1 + swTeCadAssitPu1;                          // Q14

    /* Candance Speed to Motor Speed*/
    ass_stCalOut.swCadSpd2MotSpd =
    		(SWORD)(((SLONG)ass_stCalIn.uwcadance * (SWORD)ass_stParaCong.uwMechRationMotor * (SWORD)ass_stParaCong.uwMotorPoles) >> 5); // Q20-Q5= Q15 出力时电机转速计算
    ass_stCalCoef.uwCadencePeriodCNT = (UWORD)(TIME_MS2CNT(((ULONG)1000 << 20) / ((ULONG)ass_stCalIn.uwcadance * FBASE)));        //一圈踏频时间计数
   
    /* Back EMF Cal */
    swTmpVoltPu = (SWORD)((SLONG)ass_stCalOut.swCadSpd2MotSpd * (SLONG)cof_uwFluxPu >> 13);//Q15+Q12-Q13=Q14;
    swTmpVoltPu2 = (SWORD)((SLONG)ass_stCalIn.uwSpdFbkAbsPu* (SLONG)cof_uwFluxPu >> 13);//Q15+Q12-Q13=Q14;
    if (swTmpVoltPu < swTmpVoltPu2)
    {
        swTmpVoltPu = swTmpVoltPu2;
    }
      
    ass_stCalCoef.uwStartupGain =  ass_stParaSet.uwStartupCoef ; //零速启动助力比计算
    ass_stCalCoef.uwStartupCruiseGain = ass_stParaSet.uwStartupCruiseCoef ; //带速启动助力比计算
    
    /*  Assist FSM Control */
    switch (Ass_FSM)
    {
    case Startup:
//        ass_stCalCoef.swSmoothGain = Q12_1;      
         ass_stCalCoef.swSmoothGain  += (SWORD)ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep;
         if(ass_stCalCoef.swSmoothGain >= Q12_1)
        {
            ass_stCalCoef.swSmoothGain = Q12_1;
        }
         
        swSpdKpPu = 2000;  //ass_stParaSet.uwStartUpCadNm;
        slSpdErr = (SLONG)ass_stCalOut.swCadSpd2MotSpd - (SLONG)ass_stCalIn.uwSpdFbkAbsPu;
        if(slSpdErr < 0)
        {
            slSpdErr = 0;
        } 
//        ass_stCalCoef.StartFlag = 1;        
        /* Open Voltage Limit according SpdErr*/
        if(ass_stCalCoef.StartFlag == 0)
        { 
            slTmpVoltLim= ((slSpdErr * swSpdKpPu )>> 11) + swTmpVoltPu; 
            if(slTmpVoltLim > scm_swVsDcpLimPu)
            {
                slTmpVoltLim = scm_swVsDcpLimPu;
            }
            else if(slTmpVoltLim <= swTmpVoltPu)
            {
                slTmpVoltLim = swTmpVoltPu;
            }
            else
            {
            	//do nothing
            }
            
            ass_stCalOut.swVoltLimitPu = (SWORD)slTmpVoltLim;
            
            if(slSpdErr <= 1000)
            {
                ass_stCalCoef.StartFlag = 1;
            }     
        }
        else if(ass_stCalCoef.StartFlag ==1 )
        {
            if(ass_stCalOut.swVoltLimitPu < (scm_swVsDcpLimPu - uwVoltAccStep))
            {
                ass_stCalOut.swVoltLimitPu += (SWORD)uwVoltAccStep;//ass_stCalCoef.uwStartUpGainAddStep;
            }
            else
            {
                ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
            }
 
            if(slSpdErr <= 100)
            {
                ass_pvt_swVoltCnt++;
            }
            else
            {
                ass_pvt_swVoltCnt--;
                if(ass_pvt_swVoltCnt < 0)
                {
                    ass_pvt_swVoltCnt = 0;
                }
            }
            /* Switch to TorqueAssit FSM */
            if(ass_pvt_swVoltCnt > 30)
            {
                Ass_FSM = TorqueAssit;
                ass_stCalCoef.StartFlag=0;
                ass_pvt_swVoltCnt=0;
            }
        } 
        else
        {
        	//do nothing
        }

        /* Switch to ReduceCurrent FSM */
        if((ass_stCalIn.uwcadancePer == 0) || (ass_stCalIn.uwGearSt == 0) || BikeBrake_blGetstate() == TRUE)
        {
            /* When CandanceFreq=0 or BikeGear=0*/
            ass_stCalCoef.swAss2SpdCNT = 0;
            Ass_FSM = ReduceCurrent;
        }
        else if(ass_stCalIn.uwtorquePer <= (ass_stCalCoef.uwAssStopThreshold))
        {            
            ass_stCalCoef.swAss2SpdCNT++;
            uwTmpStopCnt = ass_stCalIn.uwcadance;//((ULONG)1000<<20)/(ass_CalIn.uwcadance * FBASE) ;
            if(uwTmpStopCnt < 200)
            {
                uwTmpStopCnt = 200;
            }
            else if(uwTmpStopCnt > 500)
            {
                uwTmpStopCnt = 500;
            }
            else
            {
            	//do nothing
            }
            
            if(ass_stCalCoef.swAss2SpdCNT > uwTmpStopCnt)
            {
                ass_stCalCoef.swAss2SpdCNT = 0;
                Ass_FSM = ReduceCurrent;
            } 
        }
        else
        {           
             ass_stCalCoef.swAss2SpdCNT = 0;
        }

        break;
    case TorqueAssit:  
            
          /* 启动系数 */
        ass_stCalCoef.swSmoothGain  +=  (SWORD)ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep;
        if(ass_stCalCoef.swSmoothGain >= Q12_1)
        {
            ass_stCalCoef.swSmoothGain = Q12_1;
        }
        
        /* Reduce Voltage Limit When LPFTorq < Switch1TorqThreshold */     
//         if(ass_stCalIn.uwtorquelpf >= ass_stCalCoef.uwSwitch1TorqThreshold)
//         {             
             ass_stCalOut.swVoltLimitPu +=  (SWORD)uwVoltAccStep; //ass_stCalCoef.uwStartUpGainAddStep;
//         }
//         else if (ass_stCalIn.uwtorquelpf <= ass_stCalCoef.uwSwitch1TorqThreshold)
//         {
//             ass_stCalOut.swVoltLimitPu -=  uwVoltDecStep; //ass_stCalCoef.uwSpeedConstantCommand;
//         }
//         else
//         {
//         }
         if (ass_stCalOut.swVoltLimitPu > scm_swVsDcpLimPu)
         {
             ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
         }
//         else if (ass_stCalOut.swVoltLimitPu <= (swTmpVoltPu + ass_stParaSet.uwStartUpCadNm))
//         {
//             ass_stCalOut.swVoltLimitPu =  swTmpVoltPu + ass_stParaSet.uwStartUpCadNm;
//         }
       
        /* TorqueRef Select Coef */
        ass_stCalCoef.swTorqFilterGain += 4; // Q14 转矩滤波方式切换系数
        if (ass_stCalCoef.swTorqFilterGain > Q14_1)
        {
            ass_stCalCoef.swTorqFilterGain = Q14_1;
        }
                 
        /* Switch to ReduceCurrent FSM */
        if((ass_stCalIn.uwcadancePer == 0) || (ass_stCalIn.uwGearSt == 0) || BikeBrake_blGetstate() == TRUE)
        {
            /* When CandanceFreq=0 or BikeGear=0*/
            ass_stCalOut.blTorqPIFlg = FALSE;
            ass_stCalCoef.swAss2SpdCNT = 0;
            Ass_FSM = ReduceCurrent;
        }
        else if(ass_stCalIn.uwtorquePer <= (ass_stCalCoef.uwAssStopThreshold))
        {         
            ass_stCalCoef.swAss2SpdCNT++;
            uwTmpStopCnt = ass_stCalIn.uwcadance;//((ULONG)1000<<20)/(ass_CalIn.uwcadance * FBASE) ;
            if(uwTmpStopCnt < 200)
            {
                uwTmpStopCnt = 200;
            }
            else if(uwTmpStopCnt > 500)
            {
                uwTmpStopCnt = 500;
            }
            else
            {
            	//do nothing
            }

            if(ass_stCalCoef.swAss2SpdCNT > uwTmpStopCnt)
            {            
                ass_stCalCoef.swAss2SpdCNT = 0;
                ass_stCalOut.blTorqPIFlg = FALSE;
                Ass_FSM = ReduceCurrent;
            } 
        }
        else
        {
              ass_stCalCoef.swAss2SpdCNT = 0;
        }

        break;
        
    case  ReduceCurrent:
        /* Switch to StopAssit FSM */
        if(ass_stCalCoef.swSmoothGain <= 0)
        {
            ass_stCalCoef.swSmoothGain = 0;
            ass_stCalCoef.swTorqFilterGain = 0;
            ass_stCalCoef.swCadanceGain = 0;
            Ass_FSM = StopAssit;     
        }  
        else
        {
            /* Reduce Curret Coef to Zero*/
            ass_stCalCoef.swSmoothGain -=40;   
            ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
        }
        
        /* Switch to Startup FSM */
//        if (ass_stCalIn.uwtorquePer > ((ass_stCalCoef.uwAssThreshold * 3)>>3)  && ass_stCalIn.uwcadance > 0)
//        {
//            Ass_FSM = Startup;
//            ass_stCalOut.swSpeedRef = ass_stCalIn.uwSpdFbkAbsPu;
//            ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
//        }
        
        break;
        
    case StopAssit:
       ass_stCalOut.swTorSpdLoopCurrentTemp = 0;
       /* Switch to Startup FSM */
       if ((BikeBrake_blGetstate() == FALSE) && (ass_stCalIn.uwGearSt > 0))
       {
              if (ass_stCalIn.uwbikespeed < 449) // 0.3Hz,  (2.19m轮径下  2.36km/h )
              {
                  if (ass_stCalIn.uwtorquePer > ass_stCalCoef.uwAssThreshold && ass_stCalIn.uwcadance > 0)
                  {               
                      ass_stCalCoef.sw2StopCNT = 0;
                      ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
                      ass_pvt_stCurLpf.slY.sw.hi = 0;                
                      Ass_FSM = Startup;
                  }
              }
              else
              {
                  if (ass_stCalIn.uwtorquelpf > ((ass_stCalCoef.uwAssThreshold * 3)>>3) && ass_stCalIn.uwtorquePer > ass_stCalCoef.uwAssThreshold && ass_stCalIn.uwcadance > 0)
                  {            
                      ass_stCalCoef.sw2StopCNT = 0;
                      ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
                      ass_pvt_stCurLpf.slY.sw.hi = 0;               
                      Ass_FSM = Startup;
                  }
              }
       }
       
        /* Assit Exit */
        if (ass_stCalIn.uwcadance == 0 || ass_stCalIn.uwtorquelpf < ass_stCalCoef.uwAssStopThreshold)
        {
            ass_stCalCoef.sw2StopCNT++;
        }
        else
        {
            if (ass_stCalCoef.sw2StopCNT >= 1)
            {
                ass_stCalCoef.sw2StopCNT--;
            }
        }
       
        if ((ass_stCalCoef.sw2StopCNT > TIME_MS2CNT(3000)) || (ass_stCalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE))// 3s
        {
            ass_stCalCoef.sw2StopCNT = 0;
            ass_stCalCoef.blAssistflag = FALSE;            
        }
       
        break;
    default:
        break;
    }

    /* Bikespeed Limit */
    if (ass_stCalIn.uwbikespeed <= ass_stCurLimCoef.uwBikeSpdThresHold1)
    {
        ass_stCalCoef.swBikeSpeedGain = Q12_1; // Q12
        
    }
    else if (ass_stCalIn.uwbikespeed > ass_stCurLimCoef.uwBikeSpdThresHold1 && ass_stCalIn.uwbikespeed <= ass_stCurLimCoef.uwBikeSpdThresHold2)
    {
        ass_stCalCoef.swBikeSpeedGain =
            (SWORD)(Q12_1 - ((((SQWORD)ass_stCalIn.uwbikespeed - (SQWORD)ass_stCurLimCoef.uwBikeSpdThresHold1) * (SQWORD)ass_stCurLimCoef.ulBikeSpdDeltInv) >> 8)); // Q12
        uwTorqAccStep = 10;
        uwTorqDecStep = 10;
    }
    else
    {
        ass_stCalCoef.swBikeSpeedGain = 0;
        uwTorqAccStep = 10;
        uwTorqDecStep = 10;
    }

    /* Assist Current Output in each FSM */
    switch (Ass_FSM)
    {
    case Startup:
        swTeTorAssitPu2 = swTeTorAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
        swTeCadAssitPu2 = swTeCadAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_stCalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2);                // Q14  电流指令计算
        ass_stCalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2);                // Q14  电流指令计算
        if (ass_stCalOut.swTorAss2CurrentTemp > ass_stCalCoef.swCurrentmax_torAssPu)
        {
            ass_stCalOut.swTorAss2CurrentTemp = ass_stCalCoef.swCurrentmax_torAssPu;
        }
        if (ass_stCalOut.swCadAss2CurrentTemp > ass_stCalCoef.swCurrentmax_cadAssPu)
        {
            ass_stCalOut.swCadAss2CurrentTemp = ass_stCalCoef.swCurrentmax_cadAssPu;
        }
        ass_stCalOut.swTorRefTarget = ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp;
        ass_stCalOut.swTorRefEnd = ass_stCalOut.swTorRefTarget;
        ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *ass_stCalOut.swTorRefEnd;
           
    break;  
    
    case TorqueAssit:  
        swTeTorAssitPu2 = swTeTorAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
        swTeCadAssitPu2 = swTeCadAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_stCalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2);                // Q14  电流指令计算
        ass_stCalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2);                // Q14  电流指令计算
        if (ass_stCalOut.swTorAss2CurrentTemp > ass_stCalCoef.swCurrentmax_torAssPu)
        {
            ass_stCalOut.swTorAss2CurrentTemp = ass_stCalCoef.swCurrentmax_torAssPu;
        }
        if (ass_stCalOut.swCadAss2CurrentTemp > ass_stCalCoef.swCurrentmax_cadAssPu)
        {
            ass_stCalOut.swCadAss2CurrentTemp = ass_stCalCoef.swCurrentmax_cadAssPu;
        }
#if CURSWITCH
        /* Ajust CurrentRef growth and decline rate */
        ass_stCalOut.swTorRefTarget = ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp;        
        if((ass_stCalOut.swTorRefTarget - ass_stCalOut.swTorRefEnd) > 2)
        {
            ass_pvt_uwTorqAccCnt++;
            if(ass_pvt_uwTorqAccCnt >= 2)
            {
                ass_stCalOut.swTorRefEnd += (SWORD)uwTorqAccStep;
                ass_pvt_uwTorqAccCnt = 0;
            }          
        }  
        else if((ass_stCalOut.swTorRefTarget - ass_stCalOut.swTorRefEnd) < -1)
        {
           if (ass_stCalIn.uwcadance != ass_stCalIn.uwcadancelast)
           {
                ass_stCalOut.swTorRefEnd -= (SWORD)uwTorqDecStep;
           }
//            ass_pvt_uwTorqDecCnt++;
//            if(ass_pvt_uwTorqDecCnt >= 10)
//            {
//                ass_stCalOut.swTorRefEnd += uwTorqAccStep; 
//                ass_pvt_uwTorqDecCnt = 0;
//            }    
        }
        else
        {
           ass_stCalOut.swTorRefEnd = ass_stCalOut.swTorRefTarget;
        }
        
        ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection * ass_stCalOut.swTorRefEnd;
        
        /* Torq Clzloop Test */      
//        if(ass_stCalIn.uwtorquelpf <= ass_stCalCoef.uwSwitch1TorqThreshold)
//        { 
//            if(!ass_stCalOut.blTorqPIFlg)
//            {
//                 /* Initial value */
//                 ass_stTorqPIOut.slIRefPu = 0;
//                 swCurSwitch = abs(ass_stCalOut.swTorRefTarget); //abs(ass_stCalOut.swAssitCurRef);
//                 ass_stCalOut.blTorqPIFlg = TRUE;
//            }
// 
//            ass_stTorqPIIn.swTorqRefPu = ass_stCalIn.uwtorquelpf ;  //torsensor_test_Lpf.slY.sw.hi ; //ass_stCalIn.uwtorque;
//            ass_stTorqPIIn.swTorqFdbPu = ass_stCalCoef.uwSwitch1TorqThreshold;           
//            ass_stTorqPIIn.swImaxPu = 0; 
//            ass_stTorqPIIn.swIminPu = -swCurSwitch;
//            ass_voAssitTorqPI(&ass_stTorqPIIn,&ass_stTorqPIOut);
//            ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(swCurSwitch + ass_stUqLimMafValue.slAverValue);
//        }
//        else
//        {
//            ass_stCalOut.blTorqPIFlg = FALSE;
//            ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection * ass_stCalOut.swTorRefEnd;
//        }
#else         
        ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp);
#endif      
        break;        
    
    case  ReduceCurrent:
        swTeTorAssitPu2 = swTeTorAssitPu1; // Q14+Q12-Q12+Q12-Q12=Q14
        swTeCadAssitPu2 = swTeCadAssitPu1; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_stCalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2);                // Q14  电流指令计算
        ass_stCalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2);                // Q14  电流指令计算
        if (ass_stCalOut.swTorAss2CurrentTemp > ass_stCalCoef.swCurrentmax_torAssPu)
        {
            ass_stCalOut.swTorAss2CurrentTemp = ass_stCalCoef.swCurrentmax_torAssPu;
        }
        if (ass_stCalOut.swCadAss2CurrentTemp > ass_stCalCoef.swCurrentmax_cadAssPu)
        {
            ass_stCalOut.swCadAss2CurrentTemp = ass_stCalCoef.swCurrentmax_cadAssPu;
        }
        ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp);
        break; 
 
    case StopAssit:
         ass_stCalOut.swTorAssistCurrentTemp = 0;
         ass_stCalOut.swTorRefEnd = 0;
         break;
    default:
    	break;
    }

    /* Assist Iqref Output */
    ass_stCalOut.swTorAssistCurrent = ass_stCalOut.swTorAssistCurrentTemp;
    mth_voLPFilter(ass_stCalOut.swTorAssistCurrent, &ass_pvt_stCurLpf);   
    /* Bikespeed Limit Coef*/
    ass_stCalOut.swAssitCurRef = (SWORD)((SLONG)ass_pvt_stCurLpf.slY.sw.hi * ass_stCalCoef.swBikeSpeedGain >> 12);
    //ass_stCalOut.swAssitCurRef =ass_stCalOut.swTorAssistCurrent;
}

/**
 * @brief  Three order polynomial  Y = a*X^3 + b*X^2 + c*x +d
 *
 * @param coef polynomial coefficient a, b, c, d
 * @param Value polynomial input value X
 * @param Qnum  polynomial input Q type
 * @return UWORD  polynomial output Y
 */
static void ass_voAssitCurLim(UWORD gear, UWORD uwBikeSpeedHzPu, UWORD uwCurMaxPu)
{
    /* Limit the Output Current according to Bike Gear */
    UWORD uwIqLimitTemp1;

    if(gear > 5)
    {
      gear = 5;
    }

    uwIqLimitTemp1 = (UWORD)(((ULONG)ass_stCurLimCoef.uwLimitGain[gear] * uwCurMaxPu) >> 10);

    ass_stCurLimOut.uwIqlimit = uwIqLimitTemp1;
}

/**
 * @brief  
 *
 * @param 
 * @return 
 */
static void ass_voAssistCurLimBMS(UWORD uwSOCvalue)
{
    /* Limit the Output Current according to Bike SOC */
    if (uwSOCvalue < ass_stCurLimCalBMSCoef.uwIqLimitStartSoc && uwSOCvalue > ass_stCurLimCalBMSCoef.uwIqLimitEndSoc)
    {
        ass_stCurLimitCalBMSOut.uwIqLimitAbs =
            ass_stCurLimCalBMSCoef.uwIqLimitInitAbs - ((ass_stCurLimCalBMSCoef.uwIqLimitStartSoc - uwSOCvalue) * ass_stCurLimCalBMSCoef.swIqLImitK);
    }
    else if (uwSOCvalue <= ass_stCurLimCalBMSCoef.uwIqLimitEndSoc)
    {
        ass_stCurLimitCalBMSOut.uwIqLimitAbs = 0;
    }
    else
    {
        ass_stCurLimitCalBMSOut.uwIqLimitAbs = ass_stCurLimCalBMSCoef.uwIqLimitInitAbs;
    }
}

/**
 * @brief  Assist function
 *
 * @param coef polynomial coefficient a, b, c, d
 * @param Value polynomial input value X
 * @param Qnum  polynomial input Q type
 * @return UWORD  polynomial output Y
 */
void ass_voAssist(void)
{
    /* Start Assist Jduge */
    if (((ass_stCalIn.uwtorquePer > ass_stCalCoef.uwAssThreshold && ass_stCalIn.uwcadancePer > 0) || ass_stCalIn.uwtorquePer > 3000) && (ass_stCalIn.uwGearSt > 0 && ass_stCalIn.uwGearSt != 0x22))
    {
        ass_stCalCoef.blAssistflag = TRUE;
    }
    
    if (ass_stCalCoef.blAssistflag == TRUE)
    {
        /* Calculate Iqref Limit */
        UWORD uwIqLimitTemp;
        ass_voAssitCurLim(ass_stCalIn.uwGearSt, ass_stCalIn.uwbikespeed, ass_stParaCong.uwCofCurMaxPu);
        ass_voAssistCurLimBMS(ass_stCalIn.SOCValue);

        uwIqLimitTemp = (ass_stCurLimOut.uwIqlimit < ass_stCalIn.swFlxIqLimit)
                          ? (ass_stCurLimOut.uwIqlimit < ass_stCalIn.swPwrIqLimit ? ass_stCurLimOut.uwIqlimit : ass_stCalIn.swPwrIqLimit)
                          : (ass_stCalIn.swFlxIqLimit < ass_stCalIn.swPwrIqLimit ? ass_stCalIn.swFlxIqLimit : ass_stCalIn.swPwrIqLimit);

        ass_stCalCoef.uwCurrentMaxPu = (uwIqLimitTemp < ass_stCurLimitCalBMSOut.uwIqLimitAbs) ? uwIqLimitTemp : ass_stCurLimitCalBMSOut.uwIqLimitAbs;
        ass_stCalCoef.swCurrentmax_torAssPu = (SWORD)(((SLONG)ass_stCalCoef.uwCurrentMaxPu * (SWORD)ass_stParaSet.uwTorWeight) >> 12); // Q14
        ass_stCalCoef.swCurrentmax_cadAssPu = (SWORD)(((SLONG)ass_stCalCoef.uwCurrentMaxPu * (SWORD)ass_stParaSet.uwCadenceWeight) >> 12);
        
        /* Calculate Assist Current, Iqref*/
        AssitCuvApplPerVolt();
        
        /* Iqref Limit */
        if (ass_stCalOut.swAssitCurRef > (SWORD)ass_stCalCoef.uwCurrentMaxPu)
        {
            ass_stCalOut.swAssitCurRef = (SWORD)ass_stCalCoef.uwCurrentMaxPu;
        } 
        else if(ass_stCalOut.swAssitCurRef < -(SWORD)ass_stCalCoef.uwCurrentMaxPu)
        {
           ass_stCalOut.swAssitCurRef = -(SWORD)ass_stCalCoef.uwCurrentMaxPu;
        }
        else
        {
        	//do nothing
        }
    }
    else
    {
        ass_stCalOut.swAssitCurRef = 0;
    }    
}

/**
 * @brief  
 *
 * @param 
 * @return 
 */
void ass_voMoveAverageFilter(MAF_IN *in)
{
    in->slSum -= in->swBuffer[in->uwIndex];
    in->swBuffer[in->uwIndex] = in->swValue;
    in->slSum += (SLONG)in->swValue;
    if (!in->blSecFlag)
    {
        in->slAverValue = in->slSum / ((SLONG)in->uwIndex + (SWORD)1);
    }
    else
    {
        in->slAverValue = in->slSum / (SLONG)in->uwLength;
    }
    in->uwIndex++;

    if (in->uwIndex >= in->uwLength)
    {
        in->blSecFlag = TRUE;
        in->uwIndex = 0;
    }
}

void ass_voMoveAverageFilterClear(MAF_IN *in)
{
    UWORD i;
    in->uwIndex = 0;
    in->slSum = 0;
    in->blSecFlag = FALSE;

    for (i = 0; i < 64; i++)
    {
        in->swBuffer[i] = 0;
    }
}