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 "FSM_1st.h"
#include "Cadence.h"
#include "torquesensor.h"
#include "flash_master.h"

/******************************
 *
 *  Parameter
 *
 ******************************/
ASS_FSM_STATUS Ass_FSM;
ASS_PER_IN   ass_CalIn = TORQUE_CAL_IN_DEFAULT;
ASS_PER_COEF ass_CalCoef;
ASS_PER_OUT  ass_CalOut;

ASS_PARA_CONFIGURE ass_ParaCong;
ASS_PARA_SET       ass_ParaSet;

ASS_CURLIM_COEF ass_CurLimCoef = ASS_LIM_DEFAULT;
ASS_CURLIM_OUT  ass_CurLimOut;
ASS_LIMIT_ACCORDING_VOL_COF ass_CurLimCalBMSCoef;
ASS_LIMIT_ACCORDING_VOL_OUT ass_CurLimitCalBMSOut;

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

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(POLY_COEF *coef, 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 = (((((SQWORD)coef->a * *value >> 12) * *value) >> Qnum) * *value) >> Qnum; // Qx+Q12-Q12+Qx-Qx+Qx-Qx=Qx
    temp_b = (((SQWORD)coef->b * *value >> 12) * *value) >> Qnum;                      // Qx+Q12-Q12+Qx-Qx=Qx
    temp_c = (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 CurrentPu;
    SWORD MotorTorqueNotPu;

    MotorTorqueNotPu = ((SLONG)Tor * TORQUEBASE / ass_ParaCong.uwMechRationMotor) >> 7; // Q14-Q7 = Q7 0.1Nm   Not Pu
    CurrentPu = ((SLONG)MotorTorqueNotPu * ass_Tor2CurCalCoef.swCalCoefINV) * 10 / IBASE; // Q7+Q7 = Q14;  0.1Nm/0.01A

    return CurrentPu;
}

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


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

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

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

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

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

    ass_ParaSet.uwSpdRegion[0] = 8192;  // Q15 1500rpm
    ass_ParaSet.uwSpdRegion[1] = 16384; // Q15 3000rpm
    ass_ParaSet.uwSpdRegion[2] = 21845; // Q15 4000rpm
    ass_ParaSet.uwSpdRegionGain[0] = 4094;
    ass_ParaSet.uwSpdRegionGain[1] = 4094;
    ass_ParaSet.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_ParaCong.uwCofCurMaxPu = (((ULONG)BIKE_ASS_MOTOR_CURRENT_MAX << 14) / IBASE); // Q14
    ass_ParaCong.uwMotorPoles = cp_stMotorPara.swMotrPolePairs;
    ass_ParaCong.uwCofTorMaxPu = (((ULONG)cp_stMotorPara.swTorMax << 14) / TORQUEBASE);           // Q14
    ass_ParaCong.uwBikeAssTorMaxPu = ass_ParaCong.uwCofTorMaxPu * ass_ParaCong.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_CurLimCalBMSCoef.uwIqLimitInitAbs = ass_ParaCong.uwCofCurMaxPu; // Q14
    ass_CurLimCalBMSCoef.uwIqLimitStartSoc = 35;
    ass_CurLimCalBMSCoef.uwIqLimitEndSoc = 0;
    ass_CurLimCalBMSCoef.swIqLImitK =
        ass_CurLimCalBMSCoef.uwIqLimitInitAbs / ((SWORD)ass_CurLimCalBMSCoef.uwIqLimitStartSoc - ass_CurLimCalBMSCoef.uwIqLimitEndSoc);
    /*助力曲线初始化*/
    ass_voAssistModeSelect();

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

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

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

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

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

    /*配置速度环参数*/
    ass_CalCoef.uwSpeedConstantCommand = (((ULONG)ass_ParaSet.uwSpeedAssistSpdRpm << 15) / ((ULONG)FBASE * 60 / ass_ParaCong.uwMotorPoles));
    ass_CalCoef.swSpdLoopAbsCurMax = ((SWORD)ass_ParaSet.uwSpeedAssistIMaxA << 14) / IBASE;

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

    /*设置电流限幅*/
    ass_CalCoef.uwCurrentMaxPu = ass_ParaCong.uwCofCurMaxPu;
    ass_CalCoef.swCurrentmax_torAssPu =((SLONG)ass_CalCoef.uwCurrentMaxPu * ass_ParaSet.uwTorWeight) >> 12;          // Q14
    ass_CalCoef.swCurrentmax_cadAssPu = ((SLONG)ass_CalCoef.uwCurrentMaxPu * ass_ParaSet.uwCadenceWeight )>> 12; // Q14

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

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

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

    /*设置车速限幅*/
    //    ass_CurLimCoef.uwBikeSpdThresHold1 = ((SQWORD)10000 << 30) * ass_ParaSet.uwAssistLimitBikeSpdStart /
    //                                         ((SQWORD)36 * 3216 * ass_ParaCong.uwWheelDiameter * FBASE); // Q20    3216 = Q10(3.1415926)

    //    ass_CurLimCoef.uwBikeSpdThresHold2 =
    //        ((SQWORD)10000 << 30) * ass_ParaSet.uwAssistLimitBikeSpdStop / ((SQWORD)36 * 3216 * ass_ParaCong.uwWheelDiameter * FBASE);
    ass_CurLimCoef.uwBikeSpdThresHold1 = ((SQWORD)1000 << 20) * ass_ParaSet.uwAssistLimitBikeSpdStart /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelDiameter + ass_ParaCong.swDeltDiameter) * FBASE); // Q20    3216 = Q10(3.1415926)
    ass_CurLimCoef.uwBikeSpdThresHold2 = ((SQWORD)1000 << 20) * ass_ParaSet.uwAssistLimitBikeSpdStop /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelDiameter + ass_ParaCong.swDeltDiameter) * FBASE); // Q20    3216 = Q10(3.1415926)

    ass_CurLimCoef.ulBikeSpdDeltInv = ((SQWORD)1 << 40) / (ass_CurLimCoef.uwBikeSpdThresHold2 - ass_CurLimCoef.uwBikeSpdThresHold1); // Q20;
    ass_CurLimCoef.uwBikeSpdIqLimitK =
        (((ULONG)ass_CurLimCoef.uwBikeSpdThresHold2 - ass_CurLimCoef.uwBikeSpdThresHold1) << 8) / ass_ParaCong.uwCofCurMaxPu; // Q28-q14 = Q14;

    /*设置转矩电流标定系数*/
    ass_Tor2CurCalCoef.uwMotorFluxWb = cp_stMotorPara.swFluxWb; // 0.001mWb
    ass_Tor2CurCalCoef.uwMotprPolePairs = ass_ParaCong.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(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_ParaSet.uwSpeedAssistIMaxA;  
//    uwKitPu =  ass_ParaSet.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
    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   
      
    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;
}

SWORD ass_pvt_swVoltCnt=0;
UWORD ass_pvt_uwTorqAccCnt=0,ass_pvt_uwTorqDecCnt=0,ass_pvt_uwSpd2TorqCnt=0;
static SWORD AssitCuvApplPerVolt(void)
{
    SWORD swCurSwitch=0;
    SLONG slTeTorAssitTmpPu,slTeTorAssitLinerPu,slTeCadAssitTmpPu;
    SWORD swTeTorAssitPu1, swTeTorAssitPu2;
    SWORD swTeCadAssitPu1, swTeCadAssitPu2;
    SWORD swTmpSpdtoTorqCur;
    SLONG slTmpSmoothCur;
    SWORD swTorqCmd1, swTorqCmd, swCadCmd;
    UWORD uwTorqAccStep=0,uwTorqDecStep=80;
    SWORD swTmpVoltPu;
    SLONG slSpdErr,slTmpVoltLim;
    SWORD swSpdKpPu = 500; //Q10
    ORIG_COEF stStopOrigCoef = {-100, 0, 0};
    POLY_COEF stStopCoef;
//    SLONG slTmp_a1, slTmp_b1, slTmp_c1;

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

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

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

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

    /* Candance Speed to Motor Speed*/
    ass_CalOut.swCadSpd2MotSpd =
        ((SLONG)ass_CalIn.uwcadance * ass_ParaCong.uwMechRationMotor * ass_ParaCong.uwMotorPoles) >> 5; // Q20-Q5= Q15 出力时电机转速计算
    ass_CalCoef.uwCadencePeriodCNT = TIME_MS2CNT(((ULONG)1000 << 20) / ((ULONG)ass_CalIn.uwcadance * FBASE));        //一圈踏频时间计数   
   
    /* Back EMF Cal */
    swTmpVoltPu = (SLONG)ass_CalOut.swCadSpd2MotSpd *(SLONG)cof_uwFluxPu >> 13;//Q15+Q12-Q13=Q14;
      
    ass_CalCoef.uwStartupGain =  ass_ParaSet.uwStartupCoef ; //零速启动助力比计算
    ass_CalCoef.uwStartupCruiseGain = ass_ParaSet.uwStartupCruiseCoef ; //带速启动助力比计算
    
    /*  Assist FSM Control */
    switch (Ass_FSM)
    {
    case Startup:
        ass_CalCoef.swSmoothGain = Q12_1;        
        swSpdKpPu = 500;  //ass_ParaSet.uwStartUpCadNm;
        slSpdErr = (SLONG)ass_CalOut.swCadSpd2MotSpd - (SLONG)ass_CalIn.uwSpdFbkAbsPu;
       
        /* Open Voltage Limit according SpdErr*/
        if(ass_CalCoef.StartFlag == 0)
        { 
            slTmpVoltLim= ((slSpdErr * swSpdKpPu )>> 11) + swTmpVoltPu; 
            if(slTmpVoltLim > scm_swVsDcpLimPu)
            {
                slTmpVoltLim = scm_swVsDcpLimPu;
            }
            else if(slTmpVoltLim <= 0)
            {
                slTmpVoltLim =0;
            }
            ass_CalOut.swVoltLimitPu = slTmpVoltLim;
            
            if(slSpdErr <= 1500 )
            {
                ass_CalCoef.StartFlag=1;
            }     
        }
        else if(ass_CalCoef.StartFlag ==1 )
        {
            ass_CalOut.swVoltLimitPu += 3;//ass_CalCoef.uwStartUpGainAddStep; 
            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_CalCoef.StartFlag=0;
            }
        } 

        /* Switch to ReduceCurrent FSM */
        if((ass_CalIn.uwcadancePer == 0) || (ass_CalIn.uwGearSt == 0))
        {
            /* When CandanceFreq=0 or BikeGear=0*/
            ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15  Q10 (9.625)
            asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
            asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
            ass_CalCoef.swAss2SpdCNT = 0;
            ass_CalCoef.sw2StopCNT = 0;
            Ass_FSM = ReduceCurrent;
        }
        else if(ass_CalIn.uwtorquePer <= (ass_CalCoef.uwAssStopThreshold))
        {
             /* When InstantTorq < StopTorq and hold half circle */ 
            if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
            {
                ass_CalCoef.swAss2SpdCNT++;
            }
            if (ass_CalCoef.swAss2SpdCNT > (ass_ParaCong.uwCadPulsePerCirc >> 1))
            {
                ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15  Q10 (9.625)
                asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
                asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
                ass_CalCoef.swAss2SpdCNT = 0;
                ass_CalCoef.sw2StopCNT = 0;
                Ass_FSM = ReduceCurrent;
            }  
        }
        else
        {           
             ass_CalCoef.swAss2SpdCNT = 0;
        }

        break;
    case TorqueAssit:  
        /* Open Voltage Limit till MAX */      
        ass_pvt_swVoltCnt += 3;
        if(ass_pvt_swVoltCnt > Q12_1)
        {
           ass_pvt_swVoltCnt = Q12_1;
        }
        else
        {       
        }        
        /* Reduce Voltage Limit When MAFTorq < Switch1TorqThreshold */     
//          if(ass_CalIn.uwtorque >= ass_CalCoef.uwSwitch1TorqThreshold)
//          {             
//              ass_CalOut.swVoltLimitPu +=  ass_CalCoef.uwStartUpGainAddStep;
//          }
//          else if (ass_CalIn.uwtorque <= ass_CalCoef.uwSwitch1TorqThreshold)
//          {
//              ass_CalOut.swVoltLimitPu -=  ass_CalCoef.uwSpeedConstantCommand;
//          }
//          else
//          {}
//        
//          if (ass_CalOut.swVoltLimitPu > scm_swVsDcpLimPu)
//          {
//              ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu;
//          }
//          else if (ass_CalOut.swVoltLimitPu <= (swTmpVoltPu + ass_ParaSet.uwStartUpCadNm))
//          {
//              ass_CalOut.swVoltLimitPu =  swTmpVoltPu + ass_ParaSet.uwStartUpCadNm;
//          }

        /* Torque Clzloop Test */
        if (ass_CalOut.swVoltLimitPu < scm_swVsDcpLimPu)
        {
            ass_CalOut.swVoltLimitPu +=  ass_CalCoef.uwStartUpGainAddStep;
            if (ass_CalOut.swVoltLimitPu > scm_swVsDcpLimPu)
            {
                ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu;
            }
        }
       
        /* TorqueRef Select Coef */
        ass_CalCoef.swTorqFilterGain += 4; // Q14 转矩滤波方式切换系数
        if (ass_CalCoef.swTorqFilterGain > Q14_1)
        {
            ass_CalCoef.swTorqFilterGain = Q14_1;
        }
                 
        /* Switch to ReduceCurrent FSM */
        if((ass_CalIn.uwcadancePer == 0) || (ass_CalIn.uwGearSt == 0))
        {
            /* When CandanceFreq=0 or BikeGear=0*/
            ass_pvt_swVoltCnt=0;
            ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15  Q10 (9.625)
            asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
            asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
            ass_CalCoef.swAss2SpdCNT = 0;
            ass_CalCoef.sw2StopCNT = 0;
            stStopOrigCoef.k = (SLONG)ass_CalCoef.swSmoothGain;
            stStopCoef = ass_stPolynomialcenter(&stStopOrigCoef);
            Ass_FSM = ReduceCurrent;
            
            ass_CalOut.blTorqPIFlg = FALSE;
            ass_CalOut.blAssHoldFlag = FALSE;
        }
        else if(ass_CalIn.uwtorquePer <= (ass_CalCoef.uwAssStopThreshold))
        {
            /* When InstantTorq < StopTorq and hold half circle */ 
            if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
            {
                ass_CalCoef.swAss2SpdCNT++;
            }
            if (ass_CalCoef.swAss2SpdCNT > (ass_ParaCong.uwCadPulsePerCirc >> 1 ) || ass_CalIn.uwcadancePer == 0)
            {
                ass_pvt_uwTorqAccCnt=0;
                ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15  Q10 (9.625)
                asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
                asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
                ass_CalCoef.swAss2SpdCNT = 0;
                ass_CalCoef.sw2StopCNT = 0;
                Ass_FSM = ReduceCurrent;
                stStopOrigCoef.k = (SLONG)ass_CalCoef.swSmoothGain;
                stStopCoef = ass_stPolynomialcenter(&stStopOrigCoef);
                
                ass_CalOut.blTorqPIFlg = FALSE;
                ass_CalOut.blAssHoldFlag = FALSE;
            }  
        }
        else
        {
              ass_CalCoef.swAss2SpdCNT = 0;
        }

        break;
    case SpeedAssit:     
        ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu;
        /*电机速度指令斜坡，保证低速运行不停机*/
        if (ass_CalOut.swSpeedRef >= 0)  //ass_CalCoef.uwSpeedConstantCommand) // Q15
        {
            ass_CalOut.swSpeedRef -= 100;
        }
        else
        {
            ass_CalOut.swSpeedRef += 10;
        }
        if(ass_CalOut.swSpeedRef < 0)
        {
            ass_CalOut.swSpeedRef = 0;
        }
        
        asr_stTorqSpdPIIn.swSpdRefPu = ass_CalIn.swDirection*ass_CalOut.swSpeedRef;
        asr_stTorqSpdPIIn.swSpdFdbPu = ass_CalIn.swSpdFbkPu;           // Q15
        asr_stTorqSpdPIIn.swIqMaxPu = ass_CalCoef.swSpdLoopAbsCurMax;  // ass_CalCoef.uwCurrentMaxPu;
        asr_stTorqSpdPIIn.swIqMinPu = -ass_CalCoef.swSpdLoopAbsCurMax; // ass_CalCoef.uwCurrentMaxPu;
        asr_voSpdPI(&asr_stTorqSpdPIIn, &asr_stTorqSpdPICoef, &asr_stTorqSpdPIOut);
        ass_CalOut.swTorSpdLoopCurrentTemp = abs(asr_stTorqSpdPIOut.swIqRefPu);

        /* Switch to StopAssit FSM */
        if(abs(ass_CalIn.swSpdFbkPu) < SPD_RPM2PU(200))   
        {
            ass_CalCoef.sw2StopCNT = 0;
            ass_CalCoef.StartFlag = 0;
            ass_CalCoef.uwStartUpTargetGain = 0;
            Ass_FSM = StopAssit;
            ass_voMoveAverageFilterClear(&ass_stTorqMafValue);
            stStopOrigCoef.k = 0;
            stStopCoef = ass_stPolynomialcenter(&stStopOrigCoef);
            ass_CalCoef.swCoefStep = 0;
        }
        
        break;
    case Spd2Torq:
        /*加速啮合，速度指令斜坡快速追踪踏频*/
        if (ass_CalIn.uwSpdFbkAbsPu < ((ass_CalOut.swCadSpd2MotSpd*3) >> 1))
        {
            ass_CalOut.swSpeedRef += 100;
        }
        else
        {
//            ass_CalOut.swSpeedRef -= 4;
        }
        if (ass_CalOut.swSpeedRef > ((ass_CalOut.swCadSpd2MotSpd*3) >> 1))
        {
            ass_CalOut.swSpeedRef = ((ass_CalOut.swCadSpd2MotSpd*3) >> 1);
        }
        asr_stTorqSpdPIIn.swSpdRefPu = ass_CalIn.swDirection*ass_CalOut.swSpeedRef;
        asr_stTorqSpdPIIn.swSpdFdbPu = ass_CalIn.swSpdFbkPu;           // Q15
        asr_stTorqSpdPIIn.swIqMaxPu = ass_CalCoef.swSpdLoopAbsCurMax;  // Q14
        asr_stTorqSpdPIIn.swIqMinPu = -ass_CalCoef.swSpdLoopAbsCurMax; // Q14
        asr_voSpdPI(&asr_stTorqSpdPIIn, &asr_stTorqSpdPICoef, &asr_stTorqSpdPIOut);
        ass_CalOut.swTorSpdLoopCurrentTemp = abs(asr_stTorqSpdPIOut.swIqRefPu);
        /*啮合后切换至带速启动*/
        ass_pvt_uwSpd2TorqCnt++;
        if (ass_pvt_uwSpd2TorqCnt > 150)//ass_CalIn.uwSpdFbkAbsPu > ass_CalOut.swCadSpd2MotSpd )// && ass_CalIn.uwtorquePer > ass_CalCoef.uwAssStopThreshold) // Q15
        {
            ass_CalCoef.StartFlag = 0;
            ass_CalCoef.uwStartUpTargetGain = 0;
            swTmpSpdtoTorqCur = ass_swTorq2CurPu(swTeTorAssitPu1);
            slTmpSmoothCur = ((ULONG)abs(asr_stTorqSpdPIOut.swIqRefPu) << 12) /
                         swTmpSpdtoTorqCur; // abs(asr_stTorqSpdPIOut.swIqRefPu)/ass_swTorq2CurPu(swTeTorAssitPu1)
            if (slTmpSmoothCur > Q12_1)
            {
                slTmpSmoothCur = Q12_1;
            }
            else
            {}
            ass_CalCoef.swSmoothGain = 0;//Q12_1 >> 1;
            ass_pvt_uwSpd2TorqCnt = 0;

            Ass_FSM = StartupCruise;
        }

        /* Switch to ReduceCurrent FSM */
        if((ass_CalIn.uwcadancePer == 0) || (ass_CalIn.uwGearSt == 0))
        {
            ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15  Q10 (9.625)
            asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
            asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
            ass_CalCoef.swAss2SpdCNT = 0;
            Ass_FSM = ReduceCurrent;
            stStopOrigCoef.k = (SLONG)ass_CalCoef.swSmoothGain;
            stStopCoef = ass_stPolynomialcenter(&stStopOrigCoef);
        }
        else if ((ass_CalIn.uwtorquePer <= ass_CalCoef.uwAssStopThreshold)) // Q14
        {
            if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
            {
                ass_CalCoef.swAss2SpdCNT++;
            }
            if (ass_CalCoef.swAss2SpdCNT > (ass_ParaCong.uwCadPulsePerCirc >> 1)|| ass_CalIn.uwcadance == 0)
            {
                ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15  Q10 (9.625)
                asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
                asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
                ass_CalCoef.swAss2SpdCNT = 0;
                Ass_FSM = ReduceCurrent;
                stStopOrigCoef.k = (SLONG)ass_CalCoef.swSmoothGain;
                stStopCoef = ass_stPolynomialcenter(&stStopOrigCoef);
            }
        }
        else
        {
            ass_CalCoef.swAss2SpdCNT = 0;
        }
        break;

    case StartupCruise:
        if (ass_CalCoef.StartFlag == 0)
        {
            ass_CalCoef.swSmoothGain += ass_CalCoef.uwStartUpGainAddStep;// / ass_CalIn.uwGearSt; //助力比斜坡，与用户设置以及档位相关
            if (ass_CalCoef.swSmoothGain >= ass_CalCoef.uwStartupCruiseGain)
            {
                ass_CalCoef.StartFlag = 1;
            }
        }
        else if (ass_CalCoef.StartFlag == 1)
        {
            ass_CalCoef.swSmoothGain -= ass_CalCoef.uwStartUpGainAddStep;// / ass_CalIn.uwGearSt; //助力比斜坡，与用户设置以及档位相关
            if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
            {
                ass_CalCoef.swCadanceCNT++;
            }
            if (ass_CalCoef.swSmoothGain < Q12_1)
            {
                ass_CalCoef.swSmoothGain = Q12_1;
                if (ass_CalCoef.swCadanceCNT > ass_CalCoef.uwStartUpTimeCadenceCnt)
                {
                    Ass_FSM = TorqueAssit;
                    ass_CalCoef.swAss2SpdCNT = 0;
                    ass_CalCoef.swCadanceCNT = 0;
                    ass_CalCoef.sw2StopCNT = 0;
                }
            }
        }
        /* Switch to ReduceCurrent FSM */
        if((ass_CalIn.uwcadancePer == 0) || (ass_CalIn.uwGearSt == 0))
        {
            ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15  Q10 (9.625)
            asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
            asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
            ass_CalCoef.swAss2SpdCNT = 0;
            ass_CalCoef.sw2StopCNT = 0;
            Ass_FSM = ReduceCurrent;
            stStopOrigCoef.k = (SLONG)ass_CalCoef.swSmoothGain;
            stStopCoef = ass_stPolynomialcenter(&stStopOrigCoef);
        }
        else if(ass_CalIn.uwtorquePer <= (ass_CalCoef.uwAssStopThreshold))
        {
            if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
            {
                ass_CalCoef.swAss2SpdCNT++;
            }
            if (ass_CalCoef.swAss2SpdCNT > (ass_ParaCong.uwCadPulsePerCirc >> 1) || ass_CalIn.uwcadancePer == 0)
            {
                ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15  Q10 (9.625)
                asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
                asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
                ass_CalCoef.swAss2SpdCNT = 0;
                ass_CalCoef.sw2StopCNT = 0;
                Ass_FSM = ReduceCurrent;
                stStopOrigCoef.k = (SLONG)ass_CalCoef.swSmoothGain;
                stStopCoef = ass_stPolynomialcenter(&stStopOrigCoef);
            }  
        }
        else
        {
            ass_CalCoef.swAss2SpdCNT = 0;
        }

        break;
    case  ReduceCurrent:
        /* Switch to StopAssit FSM */
        if ((ass_CalCoef.swSmoothGain <= 0) && (ass_CalIn.uwSpdFbkAbsPu< SPD_RPM2PU(100)))
        {
            ass_CalCoef.swSmoothGain = 0;
            ass_CalCoef.swTorqFilterGain = 0;
            ass_voMoveAverageFilterClear(&ass_stTorqMafValue);
            ass_CalCoef.swCadanceGain = 0;
            ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15  Q10 (9.625)
            asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
            asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
            Ass_FSM = StopAssit;     
        }  
        else
        {
            /* Reduce Curret Coef to Zero*/
            if(ass_CalCoef.swSmoothGain >=40)
            {
                ass_CalCoef.swSmoothGain -=40;
            }
            else
            {
                ass_CalCoef.swSmoothGain=0;
            }
            
            /* Reduce Uq Limit to Zero*/
            if(ass_CalOut.swVoltLimitPu >= 20)
            {
                ass_CalOut.swVoltLimitPu -= 20;
            }
            else
            {
                ass_CalOut.swVoltLimitPu = 0;
            }
        }
        break;
    case StopAssit:
       ass_CalOut.swTorSpdLoopCurrentTemp = 0;
       /* Switch to Startup FSM */
       if (ass_CalIn.uwbikespeed < 449) // 0.3Hz,  (2.19m轮径下  2.36km/h )
        {
            if (ass_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadance > 0)
            {
//               hw_voPWMOn();
                Ass_FSM = Startup;
                ass_CalCoef.swTorqFilterGain = 0;
                ass_CalCoef.sw2StopCNT = 0;
                ass_pvt_swVoltCnt=0;
                ass_CalOut.swVoltLimitPu=0;
            }
        }
        else
        {
          if (ass_CalIn.uwtorquelpf > ((ass_CalCoef.uwAssThreshold * 3)>>3) && ass_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadance > 0)
            {
//               hw_voPWMOn();
                ass_CalCoef.swTorqFilterGain = 0;
                ass_pvt_uwSpd2TorqCnt = 0;
                Ass_FSM = Startup;
                ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu;
                ass_CalCoef.sw2StopCNT = 0;
                ass_pvt_swVoltCnt=0;
                ass_CalOut.swVoltLimitPu=0;
            }
        }
       
        /* Assit Exit */
        if (ass_CalIn.uwcadance == 0 || ass_CalIn.uwtorquelpf < ass_CalCoef.uwAssStopThreshold)
        {
            ass_CalCoef.sw2StopCNT++;
        }
        else
        {
            if (ass_CalCoef.sw2StopCNT >= 1)
            {
                ass_CalCoef.sw2StopCNT--;
            }
        }
       
        if ((ass_CalCoef.sw2StopCNT > TIME_MS2CNT(3000)) || (ass_CalIn.uwGearSt == 0))// 3s
        {
            ass_CalCoef.sw2StopCNT = 0;
            ass_CalCoef.blAssistflag = FALSE;            
        }
       
        break;
    default:
        break;
    }

    /* Bikespeed Limit */
    if (ass_CalIn.uwbikespeed <= ass_CurLimCoef.uwBikeSpdThresHold1)
    {
        ass_CalCoef.swBikeSpeedGain = Q12_1; // Q12
        
    }
    else if (ass_CalIn.uwbikespeed > ass_CurLimCoef.uwBikeSpdThresHold1 && ass_CalIn.uwbikespeed <= ass_CurLimCoef.uwBikeSpdThresHold2)
    {
        ass_CalCoef.swBikeSpeedGain =
            Q12_1 -
            ((((SQWORD)ass_CalIn.uwbikespeed - (SQWORD)ass_CurLimCoef.uwBikeSpdThresHold1) * (SQWORD)ass_CurLimCoef.ulBikeSpdDeltInv) >> 28); // Q12
        uwTorqAccStep = 10;
        uwTorqDecStep = 10;
    }
    else
    {
        ass_CalCoef.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_CalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2);                // Q14  电流指令计算
        ass_CalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2);                // Q14  电流指令计算
        if (ass_CalOut.swTorAss2CurrentTemp > ass_CalCoef.swCurrentmax_torAssPu)
        {
            ass_CalOut.swTorAss2CurrentTemp = ass_CalCoef.swCurrentmax_torAssPu;
        }
        if (ass_CalOut.swCadAss2CurrentTemp > ass_CalCoef.swCurrentmax_cadAssPu)
        {
            ass_CalOut.swCadAss2CurrentTemp = ass_CalCoef.swCurrentmax_cadAssPu;
        }
        ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
        ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefTarget;
        if (ass_CalCoef.StartFlag==0)
        {
             ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * 13000;
        }
        else
        {
             ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection *ass_CalOut.swTorRefEnd;
        }
           
    break;  
    
    case TorqueAssit:  
        swTeTorAssitPu2 = swTeTorAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
        swTeCadAssitPu2 = swTeCadAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_CalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2);                // Q14  电流指令计算
        ass_CalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2);                // Q14  电流指令计算
        if (ass_CalOut.swTorAss2CurrentTemp > ass_CalCoef.swCurrentmax_torAssPu)
        {
            ass_CalOut.swTorAss2CurrentTemp = ass_CalCoef.swCurrentmax_torAssPu;
        }
        if (ass_CalOut.swCadAss2CurrentTemp > ass_CalCoef.swCurrentmax_cadAssPu)
        {
            ass_CalOut.swCadAss2CurrentTemp = ass_CalCoef.swCurrentmax_cadAssPu;
        }
#if CURSWITCH
        /* Ajust CurrentRef growth and decline rate */
        ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;        
        if((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) > 2)
        {
            ass_pvt_uwTorqAccCnt++;
            if(ass_pvt_uwTorqAccCnt >= 2)
            {
                ass_CalOut.swTorRefEnd += uwTorqAccStep; 
                ass_pvt_uwTorqAccCnt = 0;
            }          
        }  
        else if(((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) < (-1)))
        {
           if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
           {
                ass_CalOut.swTorRefEnd -= uwTorqDecStep;            
           }
//            ass_pvt_uwTorqDecCnt++;
//            if(ass_pvt_uwTorqDecCnt >= 10)
//            {
//                ass_CalOut.swTorRefEnd += uwTorqAccStep; 
//                ass_pvt_uwTorqDecCnt = 0;
//            }    
        }
        else
        {
           ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefTarget;
        }
        
       ////ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * ass_CalOut.swTorRefEnd;
#else         
        ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection *(ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp);
#endif
        
        /* Torq Clzloop Test */      
        if(ass_CalIn.uwtorquelpf <= ass_CalCoef.uwSwitch1TorqThreshold)
        //if(torsensor_test_Lpf.slY.sw.hi <= ass_CalCoef.uwSwitch1TorqThreshold)
        { 
            if(!ass_CalOut.blTorqPIFlg)
            {
                 /* Initial value */
                 ////ass_stTorqPIOut.slIRefPu = (SLONG)ass_CalOut.swTorRefTarget << 15;//(SLONG)ass_CalOut.swAssitCurRef << 15;
                 ass_stTorqPIOut.slIRefPu = 0;
                 swCurSwitch = abs(ass_CalOut.swTorRefTarget);//abs(ass_CalOut.swAssitCurRef);
                 ass_CalOut.blTorqPIFlg = TRUE;
            }
 
            ass_stTorqPIIn.swTorqRefPu = ass_CalIn.uwtorquelpf ;  //torsensor_test_Lpf.slY.sw.hi ; //ass_CalIn.uwtorque;
            ass_stTorqPIIn.swTorqFdbPu = ass_CalCoef.uwSwitch1TorqThreshold;           
            ass_stTorqPIIn.swImaxPu = 0; 
            ass_stTorqPIIn.swIminPu = -swCurSwitch;
            ass_voAssitTorqPI(&ass_stTorqPIIn,&ass_stTorqPIOut);
            //ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection *(swCurSwitch + ass_stTorqPIOut.swIRefPu);
            ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection *(swCurSwitch + ass_stUqLimMafValue.AverValue);
        }
        else
        {
            ass_CalOut.blTorqPIFlg = FALSE;
            ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * ass_CalOut.swTorRefEnd;
        }        
        
        break;        
    case StartupCruise:
        swTeTorAssitPu2 = swTeTorAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
        swTeCadAssitPu2 = swTeCadAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_CalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2);                // Q14  电流指令计算
        ass_CalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2);                // Q14  电流指令计算
        if (ass_CalOut.swTorAss2CurrentTemp > ass_CalCoef.swCurrentmax_torAssPu)
        {
            ass_CalOut.swTorAss2CurrentTemp = ass_CalCoef.swCurrentmax_torAssPu;
        }
        if (ass_CalOut.swCadAss2CurrentTemp > ass_CalCoef.swCurrentmax_cadAssPu)
        {
            ass_CalOut.swCadAss2CurrentTemp = ass_CalCoef.swCurrentmax_cadAssPu;
        }              
#if CURSWITCH    
        /* Ajust CurrentRef growth and decline rate */
        ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
        if((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) > 2)
        {
            ass_pvt_uwTorqAccCnt++;
            if(ass_pvt_uwTorqAccCnt >= 1)
            {
                ass_CalOut.swTorRefEnd += uwTorqAccStep; 
                ass_pvt_uwTorqAccCnt = 0;
            }    
        }  
        else if(((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) < (-1)))
        {
          if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
          {
               ass_CalOut.swTorRefEnd -= uwTorqDecStep;
          }
        }
        else
        {
           ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefTarget;
        }
        
        if(ass_CalOut.swTorRefEnd  < ass_CalOut.swTorSpdLoopCurrentTemp)
        {
            ass_CalOut.swTorRefEnd  = ass_CalOut.swTorSpdLoopCurrentTemp;
            //ass_CalOut.swTorSpdLoopCurrentTemp = 0; // 启动前电流最小为速度环电流，启动后最小电流为0
        }
        else
        {
            ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefEnd;
        }
        ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * ass_CalOut.swTorRefEnd;
       
#else
        ass_CalOut.swTorAssistCurrentTemp = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
        if(ass_CalOut.swTorAssistCurrentTemp < ass_CalOut.swTorSpdLoopCurrentTemp)
        {
            ass_CalOut.swTorAssistCurrentTemp  = ass_CalIn.swDirection*ass_CalOut.swTorSpdLoopCurrentTemp;
            //ass_CalOut.swTorSpdLoopCurrentTemp = 0; // 启动前电流最小为速度环电流，启动后最小电流为0
        }
        else
        {
            ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * ass_CalOut.swTorAssistCurrentTemp;
        }
#endif
      
        break; 
    case  ReduceCurrent:
        swTeTorAssitPu2 = swTeTorAssitPu1; // Q14+Q12-Q12+Q12-Q12=Q14
        swTeCadAssitPu2 = swTeCadAssitPu1; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_CalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2);                // Q14  电流指令计算
        ass_CalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2);                // Q14  电流指令计算
        if (ass_CalOut.swTorAss2CurrentTemp > ass_CalCoef.swCurrentmax_torAssPu)
        {
            ass_CalOut.swTorAss2CurrentTemp = ass_CalCoef.swCurrentmax_torAssPu;
        }
        if (ass_CalOut.swCadAss2CurrentTemp > ass_CalCoef.swCurrentmax_cadAssPu)
        {
            ass_CalOut.swCadAss2CurrentTemp = ass_CalCoef.swCurrentmax_cadAssPu;
        }
        ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection *(ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp);
        break; 
    case SpeedAssit:
        ass_CalOut.swTorAssistCurrentTemp = asr_stTorqSpdPIOut.swIqRefPu; // ass_CalOut.swTorSpdLoopCurrentTemp;
        break;
    case Spd2Torq:
        ass_CalOut.swTorAssistCurrentTemp = asr_stTorqSpdPIOut.swIqRefPu; // ass_CalOut.swTorSpdLoopCurrentTemp;
        ass_CalOut.swTorRefEnd = ass_CalOut.swTorAssistCurrentTemp;
        break;
    case StopAssit:
         ass_CalOut.swTorAssistCurrentTemp = 0;
         ass_CalOut.swTorRefEnd = 0;
         break;
    default:
    	break;
    }

    /* Assist Iqref Output */
    ass_CalOut.swTorAssistCurrent = ass_CalOut.swTorAssistCurrentTemp;
    mth_voLPFilter(ass_CalOut.swTorAssistCurrent, &ass_pvt_stCurLpf);   
    /* Bikespeed Limit Coef*/
    ass_CalOut.swAssitCurRef = ((SLONG)ass_pvt_stCurLpf.slY.sw.hi * ass_CalCoef.swBikeSpeedGain) >> 12;
    //ass_CalOut.swAssitCurRef =ass_CalOut.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;

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

    ass_CurLimOut.uwIqlimit = uwIqLimitTemp1;
}

/**
 * @brief  
 *
 * @param 
 * @return 
 */
static void ass_voAssistCurLimBMS(UWORD uwSOCvalue)
{
    /* Limit the Output Current according to Bike SOC */
    if (uwSOCvalue < ass_CurLimCalBMSCoef.uwIqLimitStartSoc && uwSOCvalue > ass_CurLimCalBMSCoef.uwIqLimitEndSoc)
    {
        ass_CurLimitCalBMSOut.uwIqLimitAbs =
            ass_CurLimCalBMSCoef.uwIqLimitInitAbs - (((SLONG)ass_CurLimCalBMSCoef.uwIqLimitStartSoc - uwSOCvalue) * ass_CurLimCalBMSCoef.swIqLImitK);
    }
    else if (uwSOCvalue <= ass_CurLimCalBMSCoef.uwIqLimitEndSoc)
    {
        ass_CurLimitCalBMSOut.uwIqLimitAbs = 0;
    }
    else
    {
        ass_CurLimitCalBMSOut.uwIqLimitAbs = ass_CurLimCalBMSCoef.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_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadancePer > 0) || ass_CalIn.uwtorquePer > 3000) && ass_CalIn.uwGearSt > 0)
    {
        ass_CalCoef.blAssistflag = TRUE;
    }
    
    if (ass_CalCoef.blAssistflag == TRUE)
    {
        /* Calculate Iqref Limit */
        UWORD IqLimitTemp;
        ass_voAssitCurLim(ass_CalIn.uwGearSt, ass_CalIn.uwbikespeed, ass_ParaCong.uwCofCurMaxPu);
        ass_voAssistCurLimBMS(ass_CalIn.SOCValue);

        IqLimitTemp = (ass_CurLimOut.uwIqlimit < ass_CalIn.swFlxIqLimit)
                          ? (ass_CurLimOut.uwIqlimit < ass_CalIn.swPwrIqLimit ? ass_CurLimOut.uwIqlimit : ass_CalIn.swPwrIqLimit)
                          : (ass_CalIn.swFlxIqLimit < ass_CalIn.swPwrIqLimit ? ass_CalIn.swFlxIqLimit : ass_CalIn.swPwrIqLimit);

        ass_CalCoef.uwCurrentMaxPu = (IqLimitTemp < ass_CurLimitCalBMSOut.uwIqLimitAbs) ? IqLimitTemp : ass_CurLimitCalBMSOut.uwIqLimitAbs;
        ass_CalCoef.swCurrentmax_torAssPu = ((SLONG)ass_CalCoef.uwCurrentMaxPu * ass_ParaSet.uwTorWeight) >> 12; // Q14
        ass_CalCoef.swCurrentmax_cadAssPu = ((SLONG)ass_CalCoef.uwCurrentMaxPu * ass_ParaSet.uwCadenceWeight) >> 12;
        
        /* Calculate Assist Current, Iqref*/
        AssitCuvApplPerVolt();
        
        /* Iqref Limit */
        if (ass_CalOut.swAssitCurRef > ass_CalCoef.uwCurrentMaxPu)
        {
            ass_CalOut.swAssitCurRef = ass_CalCoef.uwCurrentMaxPu;
        } 
        else if(ass_CalOut.swAssitCurRef < -(SWORD)ass_CalCoef.uwCurrentMaxPu)
        {
           ass_CalOut.swAssitCurRef = -(SWORD)ass_CalCoef.uwCurrentMaxPu;
        }
        else
        {}
    }
    else
    {
        ass_CalOut.swAssitCurRef = 0;
    }    
}

/**
 * @brief  
 *
 * @param 
 * @return 
 */
void ass_voMoveAverageFilter(MAF_IN *in)
{
    in->sum -= in->buffer[in->index];
    in->buffer[in->index] = in->value;
    in->sum += (SQWORD)in->value;
    if (!in->blSecFlag)
    {
        in->AverValue = (SLONG)(in->sum / (in->index + 1));
    }
    else
    {
        in->AverValue = (SLONG)(in->sum / in->length);
    }
    in->index++;

    if (in->index >= in->length)
    {
        in->blSecFlag = TRUE;
        in->index = 0;
    }
}

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

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