/**
 * @file AssistCurve.c
 * @author Zhang, Kai(zhangkai71@midea.com)
 * @brief
 * @version 0.1
 * @date 2021-11-15
 *
 * @copyright Copyright (c) 2021
 *
 */
#include "AssistCurve.h"
#include "Cadence.h"
#include "FSM_1st.h"
#include "Syspar.h"
#include "flash_master.h"
#include "string.h"
#include "typedefine.h"
#include "user.h"
#include "hwsetup.h"
/******************************
 *
 * constant Parameter
 *
 ******************************/
// SLONG CadenceAssGain[6][4] = {0, 0, 0,    0, // Q12
//                              0, 0, 4096, 0,
//                              0, 0, 8192, 0,
//                              0, 0, 12288, 0,
//                              0, 0, 16384, 0,
//                              0, 0, 20000, 0};
//
// SLONG TorqueAssGain[6][4] = {0, 0,    0,   0, //
//                             0, 1437, 409, 0, 0, 5160, 409, 0, 0, 14745, 0, 0, 33177, 22118, 0, 0, 232243, 22118, 0, 0};

/******************************
 *
 *  Parameter
 *
 ******************************/
ASS_FSM_STATUS Ass_FSM;


SWORD Assist_torqueper;

ASS_PARA_CONFIGURE ass_ParaCong;
ASS_PARA_SET       ass_ParaSet;

ASS_CURLIM_OUT  ass_CurLimOut;
ASS_CURLIM_COEF ass_CurLimCoef = ASS_LIM_DEFAULT;

ASS_PER_IN   ass_CalIn = TORQUE_CAL_IN_DEFAULT;
ASS_PER_COEF ass_CalCoef;
ASS_PER_OUT  ass_CalOut;
UWORD        StartUpGainArray[5] = START_GAIN_DEFAULT;
UWORD        LinerAssist[5] = ASS_LINER_TORQUE_DEFAULT;
SWORD  MAF_buffer[64];
MAF_IN maf_torque = {0, 32, 0, 0, MAF_buffer, 0, FALSE};

TOR2CURRENT_CAL_COEF ass_Tor2CurCalCoef;

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

ASS_LIMIT_ACCORDING_VOL_OUT ass_CurLimitCalBMSOut;
ASS_LIMIT_ACCORDING_VOL_COF ass_CurLimCalBMSCoef;

/******************************
 *
 *  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 Polynomial(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 Polynomial_center(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 swCurrentCal(SWORD Tor)
{
    SWORD Current;
    SWORD MotorTorqueNotPu;

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

    return Current;
}

/**
 * @brief  Para from EE Init
 *
 * @param void
 * @return void
 */
void AssitEEInit(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 AssitCoefInit(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);
    /*助力曲线初始化*/
    AssistMode_Select();

    /*助力启动阈值初始化*/
    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;
    }
    /*设置滑动平均滤波踏频数*/
    maf_torque.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
 */


UWORD uwspeed2torqCnt;
POLY_COEF Stop_Coef;
ORIG_COEF Stop_Orig_Coef = {-100, 0, 0};
    SLONG Te_Tor_Assit_tempPu,Te_Tor_Assit_LinerPu;
    SLONG Te_Cad_Assit_tempPu;
    SWORD Te_Tor_AssitPu1, Te_Tor_AssitPu2;
    SWORD Te_Cad_AssitPu1, Te_Cad_AssitPu2;
    SWORD TempSpeedtoTorque;
    SLONG TempSmooth;
    SWORD TorqCmd1, TorqCmd, CadCmd;
    SLONG temp_a1, temp_b1, temp_c1;
    UWORD  TorqueAccStep=0;
    UWORD  TorqueDecStep=80;
    UWORD TorquAccCnt=0,TorquDecCnt=0;
SWORD   AssitCuvApplPer(void)
{
  ///////////////////////////  Assist torque Cal using Assist Curve ////////////////////////////////
    if (ass_CalIn.uwGearSt == 1)
    {
        TorqueAccStep = 50;
    }
    else if(ass_CalIn.uwGearSt == 2)
    {
        TorqueAccStep = 100;
    }
    else if(ass_CalIn.uwGearSt == 3)
    {
        TorqueAccStep = 120;
    }
    else if(ass_CalIn.uwGearSt == 4)
    {
        TorqueAccStep = 150;
    }   
    else if(ass_CalIn.uwGearSt == 5)
    {
        TorqueAccStep = 150;
    }
    else
    {

    }
     
    TorqCmd1 = ((ULONG)ass_CalIn.uwtorque * ass_CalCoef.swTorqFilterGain >> 14) +
               ((ULONG)ass_CalIn.uwtorquelpf * (Q14_1 - ass_CalCoef.swTorqFilterGain) >> 14); //转矩指令滤波切换，由低通滤波到踏频相关的滑动平均滤波
    TorqCmd = ((ULONG)TorqCmd1 * ass_CalCoef.swSmoothGain) >> 12;                             //转矩指令斜坡
    if (TorqCmd > ass_ParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
    {
        TorqCmd = ass_ParaCong.uwBikeAssTorMaxPu;
    }
   
    Te_Tor_Assit_tempPu = (SLONG)(Polynomial(&ass_CalCoef.uwTorqueAssGain[ass_CalIn.uwGearSt], &TorqCmd, 14)); // Q14  转矩助力曲线
    Te_Tor_Assit_LinerPu = (SLONG)(((TorqCmd * LinerAssist[ass_CalIn.uwGearSt] )>> 12) + 136);
    if (Te_Tor_Assit_tempPu < Te_Tor_Assit_LinerPu)
    {
      Te_Tor_Assit_tempPu = Te_Tor_Assit_LinerPu;
    }
    else
    {
        //do nothing;
    }
                                           
    CadCmd = (((SLONG)ass_CalIn.uwcadance * ass_CalCoef.swSmoothGain) >> 12)*10;   // 踏频指令斜坡
    Te_Cad_Assit_tempPu = ((SLONG)(Polynomial(&ass_CalCoef.uwCadencAsseGain[ass_CalIn.uwGearSt], &CadCmd, 20))) >> 6; // Q20 - Q6 = Q14 //踏频助力曲线

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

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

    // paraset  gain of user
    Te_Tor_AssitPu1 = (((SLONG)Te_Tor_Assit_tempPu) * ass_ParaSet.uwTorAssAjstGain) >> 12;     // Q14+Q12-Q12 = Q14; 用户设置转矩比例
    Te_Cad_AssitPu1 = (((SLONG)Te_Cad_Assit_tempPu) * ass_ParaSet.uwCadenceAssAjstGain) >> 12; // Q14+Q12-Q12 = Q14; 用户设置踏频比例
    ass_CalOut.swTorAssistSum1 = (Te_Tor_AssitPu1 + Te_Cad_AssitPu1);                          // Q14

    //////////////////////////////  Dadence para cal   /////////////////////////////////////////////
    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));        //一圈踏频时间计数
//    ass_CalCoef.uwStartupGain = ((ULONG)StartUpGainArray[ass_CalIn.uwGearSt - 1] * ass_ParaSet.uwStartupCoef) >> 12; //零速启动助力比计算
//    ass_CalCoef.uwStartupCruiseGain =
//        ((ULONG)StartUpGainArray[ass_CalIn.uwGearSt - 1] * ass_ParaSet.uwStartupCruiseCoef) >> 12; //带速启动助力比计算 //带速启动助力比计算
    ass_CalCoef.uwStartupGain =  ass_ParaSet.uwStartupCoef ; //零速启动助力比计算
    ass_CalCoef.uwStartupCruiseGain = ass_ParaSet.uwStartupCruiseCoef ; //带速启动助力比计算
    //////////////////////////////   Assist FSM Control ///////////////////////////////////////////
    switch (Ass_FSM)
    {
    case Startup:
        /*code*/
        if (ass_CalCoef.StartFlag == 0)
        {
            ass_CalCoef.swSmoothGain += ass_CalCoef.uwStartUpGainAddStep ;// / ass_CalIn.uwGearSt; //助力比斜坡，与用户设置以及档位相关
            if (ass_CalCoef.swSmoothGain > ass_CalCoef.uwStartupGain)
            {
                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) //正常助力时助力比等于1
            {
                ass_CalCoef.swSmoothGain = Q12_1;
                if (ass_CalCoef.swCadanceCNT > ass_CalCoef.uwStartUpTimeCadenceCnt)
                {
                    Ass_FSM = TorqueAssit;
                    ass_CalCoef.swCadanceCNT = 0;
                }
            }
        }
        /*启动停机判断，瞬时转矩小于停机值持续1/4圈*/
         if(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;
        }
        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_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.uwcadance != ass_CalIn.uwcadancelast)
//            {
//                ass_CalCoef.swAss2SpdCNT--;
//            }
//            if (ass_CalCoef.swAss2SpdCNT < 0)
//            {
//                ass_CalCoef.swAss2SpdCNT = 0;
//            }
             ass_CalCoef.swAss2SpdCNT = 0;

        }

        break;

    case TorqueAssit:
        /* Torque stop*/
        ass_CalCoef.swTorqFilterGain += 4; // Q14 转矩滤波方式切换系数
        if (ass_CalCoef.swTorqFilterGain > Q14_1)
        {
            ass_CalCoef.swTorqFilterGain = Q14_1;
        }
        /*踏频助力比系数，随转矩大小浮动，优化骑行踏空感*/
        if (ass_CalIn.uwtorque > ((ass_CalCoef.uwAssThreshold*5)>>3))//ass_CalCoef.uwAssStopThreshold)
        {
            ass_CalCoef.swCadanceGain +=2;
        }
        else
        {
            ass_CalCoef.swCadanceGain -=2;
        }
        if (ass_CalCoef.swCadanceGain > Q12_1)
        {
            ass_CalCoef.swCadanceGain = Q12_1;
        }

        else if (ass_CalCoef.swCadanceGain < 0)
        {
            ass_CalCoef.swCadanceGain = 0;
        }
        else
        {}
        /*停机判断，瞬时转矩小于停机值持续1/4圈*/
        
        if(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;
            Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
            Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
            Ass_FSM = ReduceCurrent;
        }
        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;
                Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
            Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
            }  
        }
         else
        {
          ass_CalCoef.swAss2SpdCNT = 0;
//            if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
//            {
//                ass_CalCoef.swAss2SpdCNT--;
//            }
//            if (ass_CalCoef.swAss2SpdCNT < 0)
//            {
//                ass_CalCoef.swAss2SpdCNT = 0;
//            }
        }

//        /*启动停机判断，瞬时转矩小于停机值持续1/4圈*/
//        if(ass_CalIn.uwcadancePer == 0) //防止启动时急刹车电机仍有动力
//        {
//            Ass_FSM = StopAssit;
//            MoveAverageFilterClear(&maf_torque);
//            ass_CalCoef.sw2StopCNT = 0;
//            ass_CalCoef.swCadanceCNT = 0;
//            ass_CalCoef.StartFlag = 0;
//            ass_CalCoef.uwStartUpTargetGain = 0;
//            Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
//            Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
//            ass_CalCoef.swCoefStep = 0;
//        }
//        else if ((ass_CalIn.uwtorquePer <= ass_CalCoef.uwAssStopThreshold)) // Q14
//        {
//            if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
//            {
//                ass_CalCoef.sw2StopCNT++;
//            }
//            if (ass_CalCoef.sw2StopCNT > (ass_ParaCong.uwCadPulsePerCirc >> 1) || ass_CalIn.uwcadancePer == 0)
//            {
//                Ass_FSM = StopAssit;
//                MoveAverageFilterClear(&maf_torque);
//                ass_CalCoef.sw2StopCNT = 0;
//                ass_CalCoef.swCadanceCNT = 0;
//                ass_CalCoef.StartFlag = 0;
//                ass_CalCoef.uwStartUpTargetGain = 0;
//                Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
//                Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
//                ass_CalCoef.swCoefStep = 0;
//            }
//        }
//        else
//        {
//            ass_CalCoef.sw2StopCNT = 0;
//        }

        break;

    case SpeedAssit:
      
        /*电机速度指令斜坡，保证低速运行不停机*/
        if (ass_CalOut.swSpeedRef >= 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);

//        /* 速度环切换至转矩环*/
//        if (ass_CalIn.uwtorquelpf > ass_CalCoef.uwAssThreshold)
//        {
//            Ass_FSM = Spd2Torq;
//            ass_CalCoef.swAss2SpdCNT = 0;
//            ass_CalCoef.sw2StopCNT = 0;
//        }

        /* 停机判断*/
//        if (ass_CalIn.uwtorque < ass_CalCoef.uwAssStopThreshold)
//        {
//            ass_CalCoef.sw2StopCNT++;
//            if (ass_CalIn.uwcadance == 0)
//            {
//                ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu;
//            }
//        }
//        else
//        {
//            if (ass_CalCoef.sw2StopCNT >= 1)
//            {
//                ass_CalCoef.sw2StopCNT--;
//            }
//        }

//        if (ass_CalCoef.sw2StopCNT > TIME_MS2CNT(1500) || ass_CalIn.uwcadance == 0) // 1.5s
        if(abs(ass_CalIn.swSpdFbkPu)< 400)
        {
            ass_CalCoef.sw2StopCNT = 0;
            ass_CalCoef.StartFlag = 0;
            ass_CalCoef.uwStartUpTargetGain = 0;
            Ass_FSM = StopAssit;
            MoveAverageFilterClear(&maf_torque);
            Stop_Orig_Coef.k = 0;
            Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
            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);
        /*啮合后切换至带速启动*/
        uwspeed2torqCnt++;
        if (uwspeed2torqCnt > 150)//ass_CalIn.uwSpdFbkAbsPu > ass_CalOut.swCadSpd2MotSpd )// && ass_CalIn.uwtorquePer > ass_CalCoef.uwAssStopThreshold) // Q15
        {
            ass_CalCoef.StartFlag = 0;
            ass_CalCoef.uwStartUpTargetGain = 0;
            TempSpeedtoTorque = swCurrentCal(Te_Tor_AssitPu1);
            TempSmooth = ((ULONG)abs(asr_stTorqSpdPIOut.swIqRefPu) << 12) /
                         TempSpeedtoTorque; // abs(asr_stTorqSpdPIOut.swIqRefPu)/swCurrentCal(Te_Tor_AssitPu1)
            if (TempSmooth > Q12_1)
            {
                TempSmooth = Q12_1;
            }
            else
            {}
            ass_CalCoef.swSmoothGain = 0;//Q12_1 >> 1;
            uwspeed2torqCnt = 0;

            Ass_FSM = StartupCruise;
        }

        /* 停机判断*/
        if( 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;
            Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
            Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
        }
        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;
                Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
                Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
            }
        }
        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;
                }
            }
        }
//        /* 停机判断*/
//        if(ass_CalIn.uwcadance == 0)
//        {
//            Ass_FSM = StopAssit;
//            MoveAverageFilterClear(&maf_torque);
//            ass_CalCoef.sw2StopCNT = 0;
//            ass_CalCoef.swCadanceCNT = 0;
//            ass_CalCoef.StartFlag = 0;
//            ass_CalCoef.uwStartUpTargetGain = 0;
//            Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
//            Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
//            ass_CalCoef.swCoefStep = 0;
//        }
//        else if ((ass_CalIn.uwtorquePer <= ass_CalCoef.uwAssStopThreshold)) // Q14
//        {
//            if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
//            {
//                ass_CalCoef.sw2StopCNT++;
//            }
//            if (ass_CalCoef.sw2StopCNT > (ass_ParaCong.uwCadPulsePerCirc >> 1) || ass_CalIn.uwcadancePer == 0)
//            {
//                Ass_FSM = StopAssit;
//                MoveAverageFilterClear(&maf_torque);
//                ass_CalCoef.sw2StopCNT = 0;
//                ass_CalCoef.swCadanceCNT = 0;
//                ass_CalCoef.StartFlag = 0;
//                ass_CalCoef.uwStartUpTargetGain = 0;
//                Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
//                Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
//                ass_CalCoef.swCoefStep = 0;
//            }
//        }
//        else
//        {
//            ass_CalCoef.sw2StopCNT = 0;
//        }
         if(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;
            Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
            Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
        }
        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;
                Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
                Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
            }  
        }
         else
        {
//            if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
//            {
//                ass_CalCoef.swAss2SpdCNT--;
//            }
//            if (ass_CalCoef.swAss2SpdCNT < 0)
//            {
//                ass_CalCoef.swAss2SpdCNT = 0;
//            }
            ass_CalCoef.swAss2SpdCNT = 0;
        }


        break;

    case  ReduceCurrent:
        if (ass_CalCoef.swSmoothGain <= 0)
        {
            ass_CalCoef.swSmoothGain = 0;
            ass_CalCoef.swTorqFilterGain = 0;
            MoveAverageFilterClear(&maf_torque);
            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 = SpeedAssit;
            
        }  
        else
        {
//            ass_CalCoef.swCoefStep += 40;
//            ass_CalCoef.swSmoothGain = Polynomial(&Stop_Coef, &ass_CalCoef.swCoefStep, 8);
            ass_CalCoef.swSmoothGain -=40;
        }
        break;
    case StopAssit:
        /* 斜坡停机*/
//        if (ass_CalCoef.swSmoothGain <= 0)
//        {
//            ass_CalCoef.swSmoothGain = 0;
//            ass_CalCoef.swTorqFilterGain = 0;
//            MoveAverageFilterClear(&maf_torque);
//            ass_CalCoef.swCadanceGain = 0;
////            hw_voPWMOff();         
//        }
//        else
//        {
//            ass_CalCoef.swCoefStep += 40;
//            ass_CalCoef.swSmoothGain = Polynomial(&Stop_Coef, &ass_CalCoef.swCoefStep, 8);
//        }
        ass_CalOut.swTorSpdLoopCurrentTemp = 0;
        /* 启动判断*/
       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;
                
            }
        }
        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;
                uwspeed2torqCnt = 0;
                Ass_FSM = Spd2Torq;
                ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu;
                ass_CalCoef.sw2StopCNT = 0;
            }
        }
//        if (ass_CalIn.uwtorquelpf > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadance > 0)
//        {
////            hw_voPWMOn();
//            if (ass_CalIn.uwbikespeed == 0)
//            {
//                Ass_FSM = Startup;
//                ass_CalCoef.swTorqFilterGain = 0;
//            }
//            else
//            {
//                ass_CalCoef.swTorqFilterGain = 0;
//                Ass_FSM = Spd2Torq;
//                ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu;
//            }
//            ass_CalCoef.sw2StopCNT = 0;
//        }
        /* 退出助力判断*/
        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)) // 3s
        {
            ass_CalCoef.sw2StopCNT = 0;
            ass_CalCoef.blAssistflag = FALSE;
            
        }
        break;

    default:
        break;
    }

    //////////////////////////////   Bikespeed limt of Assist //////////////////////////////////
    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
        TorqueAccStep = 10;
        TorqueDecStep = 10;
    }
    else
    {
        ass_CalCoef.swBikeSpeedGain = 0;
    }

    switch (Ass_FSM)
    {
    case Startup:
       Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2);                // Q14  电流指令计算
        ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2);                // 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;
       ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection *ass_CalOut.swTorRefEnd;
      
    break;  
      
    case TorqueAssit:
   
        Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2);                // Q14  电流指令计算
        ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2);                // 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
        ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
        
        if((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) > 2)
        {
            TorquAccCnt++;
            if(TorquAccCnt >= 20)
            {
                ass_CalOut.swTorRefEnd += TorqueAccStep; 
                TorquAccCnt = 0;
            }          
        }  
        else if(((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) < (-1)))
        {
          if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
          {

                ass_CalOut.swTorRefEnd -= TorqueDecStep;
               
          }
           
        }
        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
        break;
        
    case StartupCruise:
        Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2);                // Q14  电流指令计算
        ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2);                // 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
         
        ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
        if((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) > 2)
        {
            TorquAccCnt++;
            if(TorquAccCnt >= 20)
            {
                ass_CalOut.swTorRefEnd += TorqueAccStep; 
                TorquAccCnt = 0;
            }    
        }  
        else if(((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) < (-1)))
        {
          if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
          {
               ass_CalOut.swTorRefEnd -= TorqueDecStep;
          }
        }
        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:
        Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2);                // Q14  电流指令计算
        ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2);                // 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;
    }

    ////////////////////////////    output Iqref ///////////////////////////////////////
    ass_CalOut.swTorAssistCurrent = ass_CalOut.swTorAssistCurrentTemp;
//    mth_voLPFilter(ass_CalOut.swTorAssistCurrent, &ass_pvt_stCurLpf);   
//    Assist_torqueper = ass_pvt_stCurLpf.slY.sw.hi;
      Assist_torqueper =ass_CalOut.swTorAssistCurrent;
    return Assist_torqueper;
}


SWORD tmpVoltargetPu,VoltCnt;
UWORD tempVoltage;

        SLONG slSpderror,sltmpvoltagelimit;
        SWORD SpdKp = 500; //Q10;
SWORD   AssitCuvApplPerVolt(void)
{
  ///////////////////////////  Assist torque Cal using Assist Curve ////////////////////////////////
    if (ass_CalIn.uwGearSt == 1)
    {
        TorqueAccStep = 50;
    }
    else if(ass_CalIn.uwGearSt == 2)
    {
        TorqueAccStep = 100;
    }
    else if(ass_CalIn.uwGearSt == 3)
    {
        TorqueAccStep = 120;
    }
    else if(ass_CalIn.uwGearSt == 4)
    {
        TorqueAccStep = 150;
    }   
    else if(ass_CalIn.uwGearSt == 5)
    {
        TorqueAccStep = 150;
    }
    else
    {

    }
     TorqueDecStep = 80;
    TorqCmd1 = ((ULONG)ass_CalIn.uwtorque * ass_CalCoef.swTorqFilterGain >> 14) +
               ((ULONG)ass_CalIn.uwtorquelpf * (Q14_1 - ass_CalCoef.swTorqFilterGain) >> 14); //转矩指令滤波切换，由低通滤波到踏频相关的滑动平均滤波
    TorqCmd = ((ULONG)TorqCmd1 * ass_CalCoef.swSmoothGain) >> 12;                             //转矩指令斜坡
    if (TorqCmd > ass_ParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
    {
        TorqCmd = ass_ParaCong.uwBikeAssTorMaxPu;
    }
   
    Te_Tor_Assit_tempPu = (SLONG)(Polynomial(&ass_CalCoef.uwTorqueAssGain[ass_CalIn.uwGearSt], &TorqCmd, 14)); // Q14  转矩助力曲线
    Te_Tor_Assit_LinerPu = (SLONG)(((TorqCmd * LinerAssist[ass_CalIn.uwGearSt -1] )>> 12) + 136);
    if (Te_Tor_Assit_tempPu < Te_Tor_Assit_LinerPu)
    {
      Te_Tor_Assit_tempPu = Te_Tor_Assit_LinerPu;
    }
    else
    {
        //do nothing;
    }
                                        
    CadCmd = (((SLONG)ass_CalIn.uwcadance * ass_CalCoef.swSmoothGain) >> 12)*10;  // 踏频指令斜坡 
    Te_Cad_Assit_tempPu = ((SLONG)(Polynomial(&ass_CalCoef.uwCadencAsseGain[ass_CalIn.uwGearSt], &CadCmd, 20))) >> 6; // Q20 - Q6 = Q14 //踏频助力曲线

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

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

    // paraset  gain of user
    Te_Tor_AssitPu1 = (((SLONG)Te_Tor_Assit_tempPu) * ass_ParaSet.uwTorAssAjstGain) >> 12;     // Q14+Q12-Q12 = Q14; 用户设置转矩比例
    Te_Cad_AssitPu1 = (((SLONG)Te_Cad_Assit_tempPu) * ass_ParaSet.uwCadenceAssAjstGain) >> 12; // Q14+Q12-Q12 = Q14; 用户设置踏频比例
    ass_CalOut.swTorAssistSum1 = (Te_Tor_AssitPu1 + Te_Cad_AssitPu1);                          // Q14

    //////////////////////////////  Dadence para cal   /////////////////////////////////////////////
    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));        //一圈踏频时间计数   
   
    
    tmpVoltargetPu = (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:
        /*code*/
        ass_CalCoef.swSmoothGain = Q12_1;
        
        SpdKp = 500;//ass_ParaSet.uwStartUpCadNm;
        slSpderror = (SLONG)ass_CalOut.swCadSpd2MotSpd - (SLONG)ass_CalIn.uwSpdFbkAbsPu;
       
        if(ass_CalCoef.StartFlag == 0)
        { 
            sltmpvoltagelimit= ((slSpderror * SpdKp )>> 11) + tmpVoltargetPu; 
            if(sltmpvoltagelimit > scm_swVsDcpLimPu)
            {
                sltmpvoltagelimit = scm_swVsDcpLimPu;
            }
            else if(sltmpvoltagelimit <= 0)
            {
                sltmpvoltagelimit =0;
            }
            ass_CalOut.swVoltLimitPu = sltmpvoltagelimit;
            
            if(slSpderror <= 1500 )
            {
                ass_CalCoef.StartFlag=1;
            }     
        }
        else if(ass_CalCoef.StartFlag ==1 )
        {
            ass_CalOut.swVoltLimitPu += 3;//ass_CalCoef.uwStartUpGainAddStep; 
            if(slSpderror <= 100)
            {
              VoltCnt++;
            }
            else
            {
                VoltCnt--;
                if (VoltCnt<0)
                {
                  VoltCnt=0;
                }
             }
             if(VoltCnt > 30)
             {
                Ass_FSM = TorqueAssit;
                ass_CalCoef.StartFlag=0;
            }
        }
        
        
        /*启动停机判断，瞬时转矩小于停机值持续1/4圈*/
        if(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;
        }
        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_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:
        
        VoltCnt += 3;
        if(VoltCnt > Q12_1)
        {
           VoltCnt = Q12_1;
        }
        else
        {
        
        }
        
//        if(ass_CalIn.uwtorquelpf >= ass_CalCoef.uwSwitch2TorqThreshold)
//        {  
//            ass_CalCoef.swCadanceGain = Q12_1; 
//        }
//        else if (ass_CalIn.uwtorquelpf > ass_CalCoef.uwSwitch1TorqThreshold && ass_CalIn.uwtorquelpf <= ass_CalCoef.uwSwitch2TorqThreshold)
//        {
//            ass_CalCoef.swCadanceGain = ((SLONG)ass_CalIn.uwtorquelpf - (SLONG)ass_CalCoef.uwSwitch1TorqThreshold) * ass_CalCoef.ulStartupDeltInv >> 16;//Q14+Q14-Q16=Q12;
//        }
//        else
//        {
//            ass_CalCoef.swCadanceGain = 0;
//        }
//        ass_CalCoef.swCadanceGain=Q12_1;
//        tempVoltage =(SLONG)scm_swVsDcpLimPu * ass_CalCoef.swCadanceGain >> 12;
//        ass_CalOut.swVoltLimitPu = (SLONG)tmpVoltargetPu + ((SLONG)tempVoltage * VoltCnt >> 12);
        
          ///////////test/////////
          
          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 <= (tmpVoltargetPu + ass_ParaSet.uwStartUpCadNm))
          {
              ass_CalOut.swVoltLimitPu =  tmpVoltargetPu + ass_ParaSet.uwStartUpCadNm;
          }

        
        /* Torque stop*/
        ass_CalCoef.swTorqFilterGain += 4; // Q14 转矩滤波方式切换系数
        if (ass_CalCoef.swTorqFilterGain > Q14_1)
        {
            ass_CalCoef.swTorqFilterGain = Q14_1;
        }
//ass_CalCoef.swTorqFilterGain = 0;
                 
        /*停机判断，瞬时转矩小于停机值持续1/4圈*/
        
        if(ass_CalIn.uwcadancePer == 0)
        {
            VoltCnt=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;
            Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
            Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
            Ass_FSM = ReduceCurrent;
            
            ass_CalOut.blAssHoldFlag = 0;
        }
        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)
            {
               TorquAccCnt=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;
                Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
                Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
                
                ass_CalOut.blAssHoldFlag = 0;
            }  
        }
         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);


        if(abs(ass_CalIn.swSpdFbkPu)< 400)
        {
            ass_CalCoef.sw2StopCNT = 0;
            ass_CalCoef.StartFlag = 0;
            ass_CalCoef.uwStartUpTargetGain = 0;
            Ass_FSM = StopAssit;
            MoveAverageFilterClear(&maf_torque);
            Stop_Orig_Coef.k = 0;
            Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
            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);
        /*啮合后切换至带速启动*/
        uwspeed2torqCnt++;
        if (uwspeed2torqCnt > 150)//ass_CalIn.uwSpdFbkAbsPu > ass_CalOut.swCadSpd2MotSpd )// && ass_CalIn.uwtorquePer > ass_CalCoef.uwAssStopThreshold) // Q15
        {
            ass_CalCoef.StartFlag = 0;
            ass_CalCoef.uwStartUpTargetGain = 0;
            TempSpeedtoTorque = swCurrentCal(Te_Tor_AssitPu1);
            TempSmooth = ((ULONG)abs(asr_stTorqSpdPIOut.swIqRefPu) << 12) /
                         TempSpeedtoTorque; // abs(asr_stTorqSpdPIOut.swIqRefPu)/swCurrentCal(Te_Tor_AssitPu1)
            if (TempSmooth > Q12_1)
            {
                TempSmooth = Q12_1;
            }
            else
            {}
            ass_CalCoef.swSmoothGain = 0;//Q12_1 >> 1;
            uwspeed2torqCnt = 0;

            Ass_FSM = StartupCruise;
        }

        /* 停机判断*/
        if( 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;
            Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
            Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
        }
        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;
                Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
                Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
            }
        }
        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;
                }
            }
        }

         if(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;
            Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
            Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
        }
        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;
                Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
                Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
            }  
        }
         else
        {

            ass_CalCoef.swAss2SpdCNT = 0;
        }


        break;

    case  ReduceCurrent:
       if (abs(ass_CalIn.swSpdFbkPu)< 500)
        {
            ass_CalCoef.swSmoothGain = 0;
            ass_CalCoef.swTorqFilterGain = 0;
            MoveAverageFilterClear(&maf_torque);
            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
        {
            if(ass_CalCoef.swSmoothGain >=40)
            {
                ass_CalCoef.swSmoothGain -=40;
            }
            else
            {
                ass_CalCoef.swSmoothGain=0;
            }

            if(ass_CalOut.swVoltLimitPu >=20)
            {
                ass_CalOut.swVoltLimitPu -= 20;
            }
            else
            {
                ass_CalOut.swVoltLimitPu = 0;
            }
        }
        break;
       
    case StopAssit:
        ass_CalOut.swTorSpdLoopCurrentTemp = 0;
        /* 启动判断*/
       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;
                VoltCnt=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;
                uwspeed2torqCnt = 0;
                Ass_FSM = Startup;
                ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu;
                ass_CalCoef.sw2StopCNT = 0;
                VoltCnt=0;
                ass_CalOut.swVoltLimitPu=0;
            }
        }
       
        /* 退出助力判断*/
        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)) // 3s
        {
            ass_CalCoef.sw2StopCNT = 0;
            ass_CalCoef.blAssistflag = FALSE;
            
        }
        break;

    default:
        break;
    }

    //////////////////////////////   Bikespeed limt of Assist //////////////////////////////////
    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
        TorqueAccStep = 10;
        TorqueDecStep = 10;
    }
    else
    {
        ass_CalCoef.swBikeSpeedGain = 0;
    }

    switch (Ass_FSM)
    {
    case Startup:
       Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2);                // Q14  电流指令计算
        ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2);                // 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;      
        if((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) > 2)
        {
            TorquAccCnt++;
            if(TorquAccCnt >= 2)
            {
                ass_CalOut.swTorRefEnd += TorqueAccStep; 
                TorquAccCnt = 0;
            }          
        }  
        else if(((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) < (-1)))
        {
            if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
            {
                ass_CalOut.swTorRefEnd -= TorqueDecStep;               
            }
//            TorquDecCnt++;
//            if(TorquDecCnt >= 10)
//            {
//                ass_CalOut.swTorRefEnd += TorqueAccStep; 
//                TorquDecCnt = 0;
//            }    
        }
        else
        {
           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:
   
        Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2);                // Q14  电流指令计算
        ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2);                // 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
        ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
        
        if((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) > 2)
        {
            TorquAccCnt++;
            if(TorquAccCnt >= 2)
            {
                ass_CalOut.swTorRefEnd += TorqueAccStep; 
                TorquAccCnt = 0;
            }          
        }  
        else if(((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) < (-1)))
        {
          if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
          {

                ass_CalOut.swTorRefEnd -= TorqueDecStep;
               
          }
//            TorquDecCnt++;
//            if(TorquDecCnt >= 10)
//            {
//                ass_CalOut.swTorRefEnd += TorqueAccStep; 
//                TorquDecCnt = 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
        break;
        
    case StartupCruise:
        Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2);                // Q14  电流指令计算
        ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2);                // 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
         
        ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
        if((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) > 2)
        {
            TorquAccCnt++;
            if(TorquAccCnt >= 1)
            {
                ass_CalOut.swTorRefEnd += TorqueAccStep; 
                TorquAccCnt = 0;
            }    
        }  
        else if(((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) < (-1)))
        {
          if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
          {
               ass_CalOut.swTorRefEnd -= TorqueDecStep;
          }
        }
        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:
        Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
        ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2);                // Q14  电流指令计算
        ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2);                // 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;
    }

    ////////////////////////////    output Iqref ///////////////////////////////////////
    ass_CalOut.swTorAssistCurrent = ass_CalOut.swTorAssistCurrentTemp;
    mth_voLPFilter(ass_CalOut.swTorAssistCurrent, &ass_pvt_stCurLpf);   
    Assist_torqueper = ass_pvt_stCurLpf.slY.sw.hi;
//      Assist_torqueper =ass_CalOut.swTorAssistCurrent;
    return Assist_torqueper;
}

/**
 * @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 AssitCuvLim(UWORD gear, UWORD uwBikeSpeedHzPu, UWORD uwCurMaxPu)
{
    UWORD uwIqLimitTemp1;

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

    ass_CurLimOut.uwIqlimit = uwIqLimitTemp1;
}

/**
 * @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 Assist(void)
{

    if ((ass_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadancePer > 0) && ass_CalIn.uwGearSt > 0)
    {
        ass_CalCoef.blAssistflag = TRUE;
    }
    if (ass_CalCoef.blAssistflag == TRUE)
    {
        ////////////  Calculate the Iq limit ///////////////////
        UWORD IqLimitTemp;
        AssitCuvLim(ass_CalIn.uwGearSt, ass_CalIn.uwbikespeed, ass_ParaCong.uwCofCurMaxPu);
        AssistCurrentLimitAccordingBMS(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;
        //////////////// Assist ////////////////////////

//        AssitCuvApplPer();
        AssitCuvApplPerVolt();
        /////////////// Limit ///////////////////////////
        if (Assist_torqueper > ass_CalCoef.uwCurrentMaxPu)
        {
            Assist_torqueper = ass_CalCoef.uwCurrentMaxPu;
        }            
    }
    else
    {
        Assist_torqueper = 0;
    }    
}

void MoveAverageFilter(MAF_IN *in)
{
    in->sum -= in->buffer[in->index];
    in->buffer[in->index] = in->value;
    in->sum += (SQWORD)in->value;
//    if (in->buffer[in->length - 1] == 0)
//    {
//        in->AverValue = (SLONG)(in->sum / (in->index + 1));
//    }
//    else
//    {
        in->AverValue = (SLONG)(in->sum / in->length);
//    }
    in->index++;

    if (in->index >= in->length)
    {
        in->index = 0;
    }
}
void MoveAverageFilterClear(MAF_IN *in)
{
    UWORD i;
    in->index = 0;
    in->sum = 0;
    //    memset((UBYTE*)in->buffer, 0, sizeof(in->buffer));

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

void AssistMode_Select(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));
}

void AssistCurrentLimitAccordingBMS(UWORD uwSOCvalue)
{
    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;
    }
}