motor_ctl-1/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 "AssistCurve.h"
#include "bikeinformation.h"
#include "Cadence.h"
#include "FSM_1st.h"
#include "flash_master.h"
#include "string.h"
#include "typedefine.h"
#include "user.h"
#include "hwsetup.h"
#include "UserGpio_Config.h"
#include "FuncLayerAPI.h"
/******************************
 *
 * constant Parameter
 *
 ******************************/

/******************************
 *
 *  Parameter
 *
 ******************************/
ASS_FSM_STATUS Ass_FSM = StopAssit;
ASS_PER_COEF ass_stCalCoef;
SWORD Assist_torqueper = 0;

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        LinerAssist[5] = ASS_LINER_TORQUE_DEFAULT;
SWORD  MAF_buffer[60];
MAF_IN maf_torque = {0, 30, 0, 0, MAF_buffer, 0, FALSE};

TOR2CURRENT_CAL_COEF ass_Tor2CurCalCoef;

ASS_LIMIT_ACCORDING_VOL_OUT ass_CurLimitCalBMSOut;
ASS_LIMIT_ACCORDING_VOL_COF ass_CurLimCalBMSCoef;

ASS_RESERVE_PARA ass_stReservePara;

#if(BIKE_TORQUESPDLIMIT_EN!=0)
ASS_SpedLinit_COEF Gear_Sped_Cof;
#endif
/******************************
 *
 *  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 = (SQWORD)Tor * TORQUEBASE * ass_Tor2CurCalCoef.uwInvMotorBikeRatio >> 22 ; // Q14+Q15-Q22 = 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.uwWheelPerimeter = ASS_BIKE_WHEEL_DIAMETER; //   Q0   0.1CM
    ass_ParaCong.swDeltPerimeter = BIKE_WHEEL_SIZE_ADJUST;
    ass_ParaCong.uwMechRationMotor = (UWORD)(((ULONG)ASS_BIKE_MECH_RATION*1024)/1000); //   Q10
    ass_ParaCong.uwThrottleMaxSpdKmH = (ASS_THROTTLE_SPD_LIMIT>>4);        //kM/H
    ass_ParaCong.uwCartSpdKmH = (ASS_CART_SPEED>>4);     //kM/H
    ass_ParaCong.uwNmFrontChainring = BIKE_FRONTTEETH_NMB;    // front gear
    ass_ParaCong.uwNmBackChainring = BIKE_BACKTEETH_NMB;     // min number of back gear
    ass_ParaCong.uwLightConfig = BIKE_LIGHT_CONFIG;
    ass_ParaCong.uwAutoPowerOffTime = BIKE_AUTO_POWER_OFF_TIME;

    ass_ParaCong.uwStartMode = BIKE_START_MODE;
    ass_ParaCong.uwAssistSelect1 = ASSISTMOD_SELECT_MODE1;
    ass_ParaCong.uwAssistSelect2 = ASSISTMOD_SELECT_MODE2;
    
    ass_ParaCong.swDeltaBikeSpeedLimit = BIKE_DELTASPEEDLIMITION;

    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 = 4;
    ass_ParaSet.uwStartUpCadNm = TORQUE_NUMBERS_PULSES >> 1; // 0.5 circle
    ass_ParaSet.uwTorLPFCadNm = TORQUE_NUMBERS_PULSES >> 1;  // 0.5 circle
    ass_ParaSet.uwSpeedAssistSpdRpm = VolLimCtrKp;
    ass_ParaSet.uwSpeedAssistIMaxA = VolLimCtrKi;
    ass_ParaSet.uwAssistLimitBikeSpdStart = BIKE_SPEED_IQLIMIT_THRESHOLD1;
    ass_ParaSet.uwAssistLimitBikeSpdStop = BIKE_SPEED_IQLIMIT_THRESHOLD2;
#if(BIKE_TORQUESPDLIMIT_EN!=0)
    Gear_Sped_Cof.uwAssistLimitSpdStart[0]=BIKE_SPEED_IQLIMIT_Gear5_S;
    Gear_Sped_Cof.uwAssistLimitSpdStop[0]=BIKE_SPEED_IQLIMIT_Gear5_E;
    Gear_Sped_Cof.uwAssistLimitSpdStart[1]=BIKE_SPEED_IQLIMIT_Gear1_S;
    Gear_Sped_Cof.uwAssistLimitSpdStop[1]=BIKE_SPEED_IQLIMIT_Gear1_E;
    Gear_Sped_Cof.uwAssistLimitSpdStart[2]=BIKE_SPEED_IQLIMIT_Gear2_S;
    Gear_Sped_Cof.uwAssistLimitSpdStop[2]=BIKE_SPEED_IQLIMIT_Gear2_E;
    Gear_Sped_Cof.uwAssistLimitSpdStart[3]=BIKE_SPEED_IQLIMIT_Gear3_S;
    Gear_Sped_Cof.uwAssistLimitSpdStop[3]=BIKE_SPEED_IQLIMIT_Gear3_E;
    Gear_Sped_Cof.uwAssistLimitSpdStart[4]=BIKE_SPEED_IQLIMIT_Gear4_S;
    Gear_Sped_Cof.uwAssistLimitSpdStop[4]=BIKE_SPEED_IQLIMIT_Gear4_E;
    Gear_Sped_Cof.uwAssistLimitSpdStart[5]=BIKE_SPEED_IQLIMIT_Gear5_S;
    Gear_Sped_Cof.uwAssistLimitSpdStop[5]=BIKE_SPEED_IQLIMIT_Gear5_E;
#endif
     
    ass_ParaSet.uwCadenceWeight = 1229;                            // Q12 percentage
    ass_ParaSet.uwTorWeight = Q12_1 ; // Q12 percentage
    ass_ParaSet.uwTorAssAjstGain = 4096;// 4096         // Q12 percentage
    ass_ParaSet.uwCadenceAssAjstGain = 0;                       // Q12 percentage
    ass_ParaSet.uwAsssistSelectNum = 1;

    ass_ParaSet.swUqLimitKi = 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
 */
LPF_OUT ass_pvt_stCurLpf;
void AssitCoefInit(void)
{
    /*状态机初始化*/
    Ass_FSM = StopAssit;
    /*电机限制初始化*/
    ass_ParaCong.uwCofCurMaxPu = (UWORD)(((ULONG)(cp_stMotorPara.swIpeakMaxA) << 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>>10; // Q14+Q10-Q10;

    /*电流限幅计算*/
    ass_CurLimCalBMSCoef.uwIqLimitInitAbs = ass_ParaCong.uwCofCurMaxPu; // Q14
    ass_CurLimCalBMSCoef.uwIqLimitStartSoc = 30;
    ass_CurLimCalBMSCoef.uwIqLimitEndSoc = 5;
    ass_CurLimCalBMSCoef.swIqLImitK =
      ((ass_CurLimCalBMSCoef.uwIqLimitInitAbs * 3) >> 2) / ((SWORD)ass_CurLimCalBMSCoef.uwIqLimitStartSoc - ass_CurLimCalBMSCoef.uwIqLimitEndSoc);  
    ass_CurLimCalBMSCoef.uwIqLimitStartIdc = IDCLIMIQSTART; 
    ass_CurLimCalBMSCoef.uwIqLimitEndIdc =  IDCLIMIQEND;    
    ass_CurLimCalBMSCoef.swIqLImitIdcK = ass_CurLimCalBMSCoef.uwIqLimitInitAbs / ((SWORD)ass_CurLimCalBMSCoef.uwIqLimitEndIdc - ass_CurLimCalBMSCoef.uwIqLimitStartIdc);
      
    /*助力曲线初始化*/
    /*根据仪表选型配置整体曲线补偿系数*/
    AssistCurveRatio();
    /*助力曲线系数计算*/
    AssistCurveCoef(&ass_CalCoef.swAssCompCoef);

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

    /*助力系数初始化*/
    ass_CalCoef.swSmoothGain = 0;                                     // Q12
    ass_CalCoef.swSmoothStopGain=4096;          // 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.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_CalOut.blAssistflag = FALSE;

    ass_CalOut.swTorAssistSum1 = 0;
    ass_CalOut.swTorAss2CurrentTemp = 0;
    ass_CalOut.swCadAss2CurrentTemp = 0;
    ass_CalOut.swTorAssistCurrentTemp = 0;
    ass_CalOut.swTorAssistCurrent = 0;

    ass_CurLimCoef.uwLimitGain[0] = 0; // Q10 percentage of max Current
#if(BIKE_TORGEARMAX!=0)
    ass_CurLimCoef.uwLimitGain[1] = 358; //35%
    ass_CurLimCoef.uwLimitGain[2] = 512; //50%
    ass_CurLimCoef.uwLimitGain[3] = 716;//70%
    ass_CurLimCoef.uwLimitGain[4] = 870;//85%
    ass_CurLimCoef.uwLimitGain[5] = 1024;//100%
#else
    ass_CurLimCoef.uwLimitGain[1] = 225;//22%
    ass_CurLimCoef.uwLimitGain[2] = 450;//44%
    ass_CurLimCoef.uwLimitGain[3] = 796;//78%
    ass_CurLimCoef.uwLimitGain[4] = 1024;//100%
    ass_CurLimCoef.uwLimitGain[5] = 1024;//100%
#endif

#if(BIKE_TORQUESPDLIMIT_EN!=0)
    /*设置档位限制车速速度*/
    Gear_Sped_Cof.uwAssistLimitSpdStart[0]=ass_ParaSet.uwAssistLimitBikeSpdStart+ ass_ParaCong.swDeltPerimeter;
     Gear_Sped_Cof.uwAssistLimitSpdStop[0]=Gear_Sped_Cof.uwAssistLimitSpdStart[0]+2;

     Gear_Sped_Cof.uwAssistLimitSpdStart[1]=(ass_ParaSet.uwAssistLimitBikeSpdStart+ass_ParaCong.swDeltPerimeter)*12/25;//48%
     Gear_Sped_Cof.uwAssistLimitSpdStop[1]=Gear_Sped_Cof.uwAssistLimitSpdStart[1]+2;

     Gear_Sped_Cof.uwAssistLimitSpdStart[2]=(ass_ParaSet.uwAssistLimitBikeSpdStart+ ass_ParaCong.swDeltPerimeter)*15/25;//60%
     Gear_Sped_Cof.uwAssistLimitSpdStop[2]=Gear_Sped_Cof.uwAssistLimitSpdStart[2]+2;

     Gear_Sped_Cof.uwAssistLimitSpdStart[3]=(ass_ParaSet.uwAssistLimitBikeSpdStart+ ass_ParaCong.swDeltPerimeter)*18/25;//72%
     Gear_Sped_Cof.uwAssistLimitSpdStop[3]=Gear_Sped_Cof.uwAssistLimitSpdStart[3]+2;

     Gear_Sped_Cof.uwAssistLimitSpdStart[4]=(ass_ParaSet.uwAssistLimitBikeSpdStart+ ass_ParaCong.swDeltPerimeter)*21/25;//84%
     Gear_Sped_Cof.uwAssistLimitSpdStop[4]=Gear_Sped_Cof.uwAssistLimitSpdStart[4]+2;

     Gear_Sped_Cof.uwAssistLimitSpdStart[5]=ass_ParaSet.uwAssistLimitBikeSpdStart + ass_ParaCong.swDeltPerimeter;//100%
     Gear_Sped_Cof.uwAssistLimitSpdStop[5]=Gear_Sped_Cof.uwAssistLimitSpdStart[5]+2;

    Gear_Sped_Cof.uwLimitdSpeed_S[0] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStart[5] /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    Gear_Sped_Cof.uwLimitdSpeed_E[0] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStop[5] /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    Gear_Sped_Cof.ulBikeSpdGearDeltInv[0] = (1 << 20) / (Gear_Sped_Cof.uwLimitdSpeed_E[0] - Gear_Sped_Cof.uwLimitdSpeed_S[0]); 
    
    Gear_Sped_Cof.uwLimitdSpeed_S[1] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStart[1] /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    Gear_Sped_Cof.uwLimitdSpeed_E[1] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStop[1] /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    Gear_Sped_Cof.ulBikeSpdGearDeltInv[1] = (1 << 20) / (Gear_Sped_Cof.uwLimitdSpeed_E[1] - Gear_Sped_Cof.uwLimitdSpeed_S[1]); 
    
    Gear_Sped_Cof.uwLimitdSpeed_S[2] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStart[2] /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    Gear_Sped_Cof.uwLimitdSpeed_E[2] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStop[2] /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    Gear_Sped_Cof.ulBikeSpdGearDeltInv[2] = (1 << 20) / (Gear_Sped_Cof.uwLimitdSpeed_E[2] - Gear_Sped_Cof.uwLimitdSpeed_S[2]); 

    Gear_Sped_Cof.uwLimitdSpeed_S[3] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStart[3] /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    Gear_Sped_Cof.uwLimitdSpeed_E[3] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStop[3] /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    Gear_Sped_Cof.ulBikeSpdGearDeltInv[3] = (1 << 20) / (Gear_Sped_Cof.uwLimitdSpeed_E[3] - Gear_Sped_Cof.uwLimitdSpeed_S[3]); 

    Gear_Sped_Cof.uwLimitdSpeed_S[4] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStart[4] /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    Gear_Sped_Cof.uwLimitdSpeed_E[4] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStop[4] /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    Gear_Sped_Cof.ulBikeSpdGearDeltInv[4] = (1 << 20) / (Gear_Sped_Cof.uwLimitdSpeed_E[4] - Gear_Sped_Cof.uwLimitdSpeed_S[4]); 

     Gear_Sped_Cof.uwLimitdSpeed_S[5] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStart[5] /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    Gear_Sped_Cof.uwLimitdSpeed_E[5] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStop[5] /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    Gear_Sped_Cof.ulBikeSpdGearDeltInv[5] = (1 << 20) / (Gear_Sped_Cof.uwLimitdSpeed_E[5] - Gear_Sped_Cof.uwLimitdSpeed_S[5]); 
#endif
    /*设置车速限幅*/
    //    ass_CurLimCoef.uwBikeSpdThresHold1 = ((SQWORD)10000 << 30) * ass_ParaSet.uwAssistLimitBikeSpdStart /
    //                                         ((SQWORD)36 * 3216 * ass_ParaCong.uwWheelPerimeter * FBASE); // Q20    3216 = Q10(3.1415926)

    //    ass_CurLimCoef.uwBikeSpdThresHold2 =
    //        ((SQWORD)10000 << 30) * ass_ParaSet.uwAssistLimitBikeSpdStop / ((SQWORD)36 * 3216 * ass_ParaCong.uwWheelPerimeter * FBASE);
    ass_CurLimCoef.uwBikeSpdThresHold1 = ((SQWORD)1000 << 20) * (ass_ParaSet.uwAssistLimitBikeSpdStart + ass_ParaCong.swDeltaBikeSpeedLimit) /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    ass_CurLimCoef.uwBikeSpdThresHold2 = ((SQWORD)1000 << 20) * (ass_ParaSet.uwAssistLimitBikeSpdStop + ass_ParaCong.swDeltaBikeSpeedLimit) /
                                         ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    ass_CurLimCoef.ulBikeSpdDeltInv = (1 << 20) / (ass_CurLimCoef.uwBikeSpdThresHold2 - ass_CurLimCoef.uwBikeSpdThresHold1); 
    
    /*设置转矩电流标定系数*/
    ass_Tor2CurCalCoef.uwMotorFluxWb = cp_stMotorPara.swFluxWb; // 0.001mWb
    ass_Tor2CurCalCoef.uwMotprPolePairs = ass_ParaCong.uwMotorPoles;
    ass_Tor2CurCalCoef.uwInvMotorBikeRatio = (UWORD)(32768L*1024L/ass_ParaCong.uwMechRationMotor);
    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;  
}
#if(BIKE_TORQUESPDLIMIT_EN!=0)
void  Gear_LimitSpd(UWORD Gear)
{
    if(Gear>5)
        Gear=5;
    ass_CurLimCoef.uwBikeSpdThresHold1= Gear_Sped_Cof.uwLimitdSpeed_S[Gear];
    ass_CurLimCoef.uwBikeSpdThresHold2=Gear_Sped_Cof.uwLimitdSpeed_E[Gear];
    ass_CurLimCoef.ulBikeSpdDeltInv=Gear_Sped_Cof.ulBikeSpdGearDeltInv[Gear];
}
#endif
/*!
 * @brief ebike assist function implement
 *
 * @param void  No input parameter
 *
 * @return None
 */
SLONG Te_Tor_Assit_tempPu;					/* assist torque curve calculate value */
SLONG Te_Tor_Assit_LinerPu;					/* assist torque line curve calculate value */
SLONG Te_Cad_Assit_tempPu;					/* cadence curve calculate value */
SWORD Te_Tor_AssitPu1;						/* assist torque command*/
SWORD Te_Cad_AssitPu1;						/* assist cadence command */
SWORD TorqCmd1=0;						    /* sampled torque after filtering */
SWORD TorqCmd=0;							/* torque command */
SWORD CadCmd;								/* cadence command */
UWORD StartUpGainAddStep;
SLONG tst_Te_Tor_Assit_tempPu;
UWORD test_startflag;
UWORD test_stopflag;
UWORD test_startflag;
UWORD test_AssState;
void AssitCuvApplPerVolt(void)
{
    /* 局部变量定义 */
    static UWORD CadGpioSta=0;
    static UWORD exit_pulse_cnt = 200;
    UWORD uwTmpTorqLpf;
    SLONG tmp_slBikeSpd2MotSpd = 0;						/* bike speed correspond to motor's speed */
  //  SLONG tmp_Te_Tor_Assit_tempPu = 0;
    SLONG tmp_slUqLimInit = 0;
    SLONG tmp_slCurrErr = 0;
    SLONG tmp_slVoltLimitPu = 0;
  //  SWORD tmp_swUqLimStep = 0;
    
    /* 启动标志判断 */
    if(ass_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold)
    {
        if(CadGpioSta != Get_CAD_FRE_PORT())
        {
 			CadGpioSta = Get_CAD_FRE_PORT();
            if(ass_CalIn.uwStartRunPulse < 100)
            {
                ass_CalIn.uwStartRunPulse++;
            }
            else
            {
                ass_CalIn.uwStartRunPulse = 100;
            }
           
        }
    }
    else if (ass_CalIn.uwtorquePer < ass_CalCoef.uwAssStopThreshold)
    {
        ass_CalIn.uwStartRunPulse = 0;
    }
    else
    {
        //滞环，不动作
    }
    //脚踏不动强制关闭
    if((ass_CalIn.uwcadLowStopCnt>=200) || (ass_CalIn.uwcadHighStopCnt>=200))
    {
        ass_CalIn.uwStartRunPulse = 0;
    }

    if((ass_CalIn.StartAssistEnble == FALSE) && (ass_CalIn.uwStartRunPulse > 1)) 
    {
        ass_CalIn.StartAssistEnble = TRUE;
    }
    else if((ass_CalIn.StartAssistEnble == TRUE) && (ass_CalIn.uwStartRunPulse == 0))
    {
        ass_CalIn.StartAssistEnble = FALSE;
    }
    else
    {
        //滞环，不动作
    }
    test_startflag = ass_CalIn.StartAssistEnble;

    /* 停止脚踏标志判断 */
    if(cp_stBikeRunInfoPara.BikeSpeedKmH < 30)
    {
        exit_pulse_cnt = 250;
    }
    else
    {
        exit_pulse_cnt = 200;
    }
    if (ass_CalIn.uwtorquePer < ass_CalCoef.uwAssStopThreshold)
    {
        exit_pulse_cnt = 200;
    }
    
    if( Get_CAD_FRE_PORT() == 0)
    {
        if(ass_CalIn.uwcadLowStopCnt<=4000)
        {
            ass_CalIn.uwcadLowStopCnt++;
        }
        
        if((ass_CalIn.uwcadLowStopCnt >= exit_pulse_cnt) || (ass_CalIn.uwcadHighStopCnt >= exit_pulse_cnt))
        {
            ass_CalIn.blStopAssistEnble=TRUE;
        }
        
        if((ass_CalIn.uwcadHighStopCnt != 0) && (ass_CalIn.uwcadHighStopCnt < exit_pulse_cnt))
        {
            ass_CalIn.blStopAssistEnble=FALSE;
        }
        
        ass_CalIn.uwcadHighStopCnt=0;
    }
    else
    {
        if(ass_CalIn.uwcadHighStopCnt<4000)
        {
            ass_CalIn.uwcadHighStopCnt++;
        }
        
        if((ass_CalIn.uwcadLowStopCnt >= exit_pulse_cnt) || (ass_CalIn.uwcadHighStopCnt >= exit_pulse_cnt))
        {
            ass_CalIn.blStopAssistEnble=TRUE;
        }
        
        if((ass_CalIn.uwcadLowStopCnt != 0) && (ass_CalIn.uwcadLowStopCnt < exit_pulse_cnt))
        {
            ass_CalIn.blStopAssistEnble=FALSE;
        }
        
        ass_CalIn.uwcadLowStopCnt=0;
    }

    test_stopflag = ass_CalIn.blStopAssistEnble;

    /* 脚踏力矩输入处理 - 滑动平均与低通滤波切换 */
    TorqCmd1 = ((ULONG)ass_CalIn.uwtorque * ass_CalCoef.swTorqFilterGain >> 14) +
                            ((ULONG)ass_CalIn.uwtorquelpf * (Q14_1 - ass_CalCoef.swTorqFilterGain) >> 14); 

    /* 脚踏力矩输入处理 - 起动过程平滑处理 */
    uwTmpTorqLpf = ((ULONG)TorqCmd1 * ass_CalCoef.swSmoothGain) >> 12;                            
    
    /* 脚踏力矩输入最大值限制 */
    if (uwTmpTorqLpf > ass_ParaCong.uwBikeAssTorMaxPu) 	
    {
        TorqCmd = ass_ParaCong.uwBikeAssTorMaxPu;
    }
    else
    {
        TorqCmd = uwTmpTorqLpf;
    }
    /***************** assist curve calculate ******************/
    if(ass_CalIn.uwGearSt == 0)
    {
        Te_Tor_Assit_tempPu = 0;
    }
    else
    {
        Te_Tor_Assit_tempPu = AssistCurveCal(&ass_CalCoef.swAssCurCoef[ass_CalIn.uwGearSt-1], &TorqCmd);
    }
    
    if (Te_Tor_Assit_tempPu > ass_ParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
    {
        Te_Tor_Assit_tempPu = ass_ParaCong.uwBikeAssTorMaxPu;
    }
     
    CadCmd = (((SLONG)ass_CalIn.uwcadance * ass_CalCoef.swSmoothGain) >> 12);  // 踏频指令斜坡

    Te_Cad_Assit_tempPu = ((SLONG)(Polynomial(&ass_CalCoef.uwCadencAsseGain[ass_CalIn.uwGearSt], &CadCmd, 20))) >> 6; // Q20 - Q6 = Q14 //踏频助力曲线
    if (Te_Cad_Assit_tempPu > ass_ParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
    {
        Te_Cad_Assit_tempPu = ass_ParaCong.uwBikeAssTorMaxPu;
    }

    /* 力矩助力根据用户设定比例调整 */
    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

    /* 计算力矩助力电流 */
    ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu1);				 // Q14  电流指令计算

    /* 计算踏频助力电流 */
    ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu1);				 // 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_CalCoef.uwCurrentMaxPu)		/* max assist current limiter */
    {
        ass_CalOut.swTorRefTarget = ass_CalCoef.uwCurrentMaxPu;
    }
    
    /* 根据助力方向，对助力电流的正负进行处理 */
    ass_CalOut.swTorRefEnd = ass_CalIn.swDirection * ass_CalOut.swTorRefTarget;

    /* 计算当前时刻车速对应电机转速 */
    if(ass_CalIn.uwbikespeed < 95L)   //95 - 1Km/h
    {
         tmp_slBikeSpd2MotSpd = (((SLONG)95L* ass_ParaCong.uwMechRationMotor >>10) * ass_ParaCong.uwMotorPoles) >> 5; // Q20-Q5= Q15 出力时电机转速计算
    }
    else
    {
         tmp_slBikeSpd2MotSpd = (((SLONG)ass_CalIn.uwbikespeed * ass_ParaCong.uwMechRationMotor >>10) * ass_ParaCong.uwMotorPoles) >> 5; // Q20-Q5= Q15 出力时电机转速计算
    }
   
    if(tmp_slBikeSpd2MotSpd > _IQ15(0.99))
    {
        ass_CalOut.swBikeSpd2MotSpd = _IQ15(0.99);
    }
    else if(tmp_slBikeSpd2MotSpd < _IQ15(0.0))
    {
        ass_CalOut.swBikeSpd2MotSpd = _IQ15(0.0);
    }
    else
    {
        ass_CalOut.swBikeSpd2MotSpd = (SWORD)tmp_slBikeSpd2MotSpd;
    }

    /* 计算啮合电机转速对应的电机反电动势，计算起动时刻电压限幅初值 */
    if(ass_CalIn.swSpdFbkPu * ass_CalIn.swDirection <  ass_CalOut.swBikeSpd2MotSpd)
    {
        tmp_slUqLimInit = (SLONG)ass_CalOut.swBikeSpd2MotSpd * (SLONG)ass_CalIn.swDirection *(SLONG)cof_uwFluxPu >> 13;//Q15+Q12-Q13=Q14;
    }
    else
    {
        tmp_slUqLimInit = (SLONG)ass_CalIn.swSpdFbkPu *(SLONG)cof_uwFluxPu >> 13;//Q15+Q12-Q13=Q14;
    }
//    tmp_slUqLimInit = (SLONG)ass_CalIn.swSpdFbkPu *(SLONG)cof_uwFluxPu >> 13;//Q15+Q12-Q13=Q14;
    tmp_slUqLimInit = tmp_slUqLimInit * 4096  >> 12;  //放大1.25倍
    if (tmp_slUqLimInit > ((SLONG)scm_swVsDcpLimPu_Assist))
    {
        ass_CalOut.swUqLimInit = scm_swVsDcpLimPu_Assist;
    }
    else if(tmp_slUqLimInit < -((SLONG)scm_swVsDcpLimPu_Assist))
    {
        ass_CalOut.swUqLimInit = -scm_swVsDcpLimPu_Assist;
    }
    else
    {
        ass_CalOut.swUqLimInit = tmp_slUqLimInit;
    }
    
    /* 限速系数计算 */
    ass_CalIn.uwbikespeedCal = ass_CalIn.uwbikespeed;
    /* Bike Speed Limit: use ass_CalCoef.swBikeSpeedGain(0-1) */
    #if(BIKE_TORQUESPDLIMIT_EN!=0)
    Gear_LimitSpd(ass_CalIn.uwGearSt);
    #endif
    if (ass_CalIn.uwbikespeedCal <= ass_CurLimCoef.uwBikeSpdThresHold1)
    {
        ass_CalCoef.swBikeSpeedGain = Q12_1; // Q12
    }
    else if (ass_CalIn.uwbikespeedCal > ass_CurLimCoef.uwBikeSpdThresHold1 && ass_CalIn.uwbikespeedCal <= ass_CurLimCoef.uwBikeSpdThresHold2)
    {
        ass_CalCoef.swBikeSpeedGain = Q12_1 -((((SLONG)ass_CalIn.uwbikespeedCal - (SLONG)ass_CurLimCoef.uwBikeSpdThresHold1) * ass_CurLimCoef.ulBikeSpdDeltInv) >> 8); // Q12
    }
    else
    {
        ass_CalCoef.swBikeSpeedGain = 0;
    }

    /* 起动平滑系数步进量计算，根据车速自适应变化 */
    StartUpGainAddStep = (cp_stBikeRunInfoPara.BikeSpeedKmH >> 4) + ass_CalCoef.uwStartUpGainAddStep;

    /* 电压限幅步进量计算Q22 */
    if(ass_CalIn.swCurRefPu * ass_CalIn.swDirection > VolLimCurrLoopErr)
    {
        tmp_slCurrErr = ((SLONG)ass_CalIn.swCurRefPu - (SLONG)ass_CalIn.swCurFdbPu)  - VolLimCurrLoopErr * ass_CalIn.swDirection;
    }
    else
    {
        tmp_slCurrErr = - (SLONG)ass_CalIn.swCurFdbPu;
    }
    tmp_slVoltLimitPu = tmp_slCurrErr * (SLONG)ass_ParaSet.uwSpeedAssistSpdRpm >> 6;
    if(ass_ParaSet.swUqLimitKi < ass_ParaSet.uwSpeedAssistIMaxA)
    {
        ass_ParaSet.swUqLimitKi += ass_ParaSet.uwStartUpCadNm;
    }
    else
    {
        ass_ParaSet.swUqLimitKi = (SWORD)ass_ParaSet.uwSpeedAssistIMaxA;
    }
    ass_CalOut.swUqLimStep = (SWORD)(tmp_slCurrErr * (SLONG)ass_ParaSet.swUqLimitKi >> 12);
    
    test_AssState = Ass_FSM;
    /* 力矩助力过程状态机控制 */
    switch (Ass_FSM)
    {
        case StopAssit:    
            ass_CalOut.swTorAssistCurrentTemp = 0;		//停止助力直接将助力电流赋为0
            ass_CalOut.blAssistflag = FALSE;

            /* 启动判断*/
            if((ass_CalIn.uwGearSt > 0) && (BikeBrake_blGetstate() == FALSE) &&(BMS_RunInfo.Status.Status_Bit.Charge==FALSE))
            {
                if(ass_CalIn.StartAssistEnble == TRUE)//5启动根据脉冲数
                {
                    ass_CalCoef.swTorqFilterGain = 0;
                    ass_CalCoef.swSmoothGain = 0;
                    ass_CalCoef.swSmoothStopGain = Q12_1;
                    ass_CalOut.swVoltLimitPu = ass_CalOut.swUqLimInit;
                    ass_CalOut.slUqLimSum = ((SLONG)ass_CalOut.swUqLimInit)<<8;
                    ass_CalOut.uwPreStartCnt = 0;
                    ass_CalOut.uwPreStartCntMax = (cp_stBikeRunInfoPara.BikeSpeedKmH<<1) + 200;
                    
                    Ass_FSM = PreStart;
                }
            }
        break;

        case  PreStart:
            /* 力矩助力标志置位 */
            ass_CalOut.blAssistflag = TRUE;

            /* 预启动阶段，助力电流最小值限制 */
//            if((ass_CalOut.swTorRefEnd * ass_CalIn.swDirection) > PreStartCurr)
//            {
//                ass_CalOut.swTorAssistCurrentTemp = PreStartCurr * ass_CalIn.swDirection;
//            }
//            else
//            {
//                ass_CalOut.swTorAssistCurrentTemp = ass_CalOut.swTorRefEnd;
//            }
//            ass_CalOut.swTorAssistCurrentTemp = PreStartCurr * ass_CalIn.swDirection;
            ass_CalOut.swTorAssistCurrentTemp = ass_CalOut.swTorRefEnd;

            /* 平滑系数值计算和限幅 */
            ass_CalCoef.swSmoothGain  += StartUpGainAddStep;//(SWORD)ass_CalCoef.uwStartUpGainAddStep; ////ass_stCalCoef.uwStartUpGainAddStep;
            if(ass_CalCoef.swSmoothGain >= Q12_1)
            {
                ass_CalCoef.swSmoothGain = Q12_1;
            }
            ass_CalCoef.swSmoothStopGain += 64;
            if(ass_CalCoef.swSmoothStopGain >= Q12_1)
            {
                ass_CalCoef.swSmoothStopGain = Q12_1;
            }

            /* 力矩切换系数值计算和限幅 */
            ass_CalCoef.swTorqFilterGain += 4; // Q14 转矩滤波方式切换系数
            if (ass_CalCoef.swTorqFilterGain > Q14_1)
            {
                ass_CalCoef.swTorqFilterGain = Q14_1;
            }

            /* 电压限幅值计算和限幅 */
//            ass_CalOut.slUqLimSum += ass_CalOut.swUqLimStep;	
            ass_ParaSet.swUqLimitKi = 0;
            ass_CalOut.slUqLimSum = ((SLONG)ass_CalOut.swUqLimInit)<<8;
            
            if (ass_CalOut.slUqLimSum > ((SLONG)scm_swVsDcpLimPu_Assist << 8))
            {
                ass_CalOut.slUqLimSum = (SLONG)scm_swVsDcpLimPu_Assist << 8;
            }
            else if(ass_CalOut.slUqLimSum < -((SLONG)scm_swVsDcpLimPu_Assist << 8))
            {
                ass_CalOut.slUqLimSum = -((SLONG)scm_swVsDcpLimPu_Assist << 8);
            }
            else
            {
                //中间不操作
            }
            tmp_slVoltLimitPu = ass_CalOut.slUqLimSum + tmp_slVoltLimitPu;
            if (tmp_slVoltLimitPu > ((SLONG)scm_swVsDcpLimPu_Assist << 8))
            {
                ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu_Assist;
            }
            else if(tmp_slVoltLimitPu < -((SLONG)scm_swVsDcpLimPu_Assist << 8))
            {
                ass_CalOut.swVoltLimitPu = -scm_swVsDcpLimPu_Assist;
            }
            else
            {
                ass_CalOut.swVoltLimitPu = (SWORD)(tmp_slVoltLimitPu >> 8);
            }

            /* 预启动阶段计时，超时进入助力状态 */
            if(ass_CalOut.uwPreStartCnt < ass_CalOut.uwPreStartCntMax)
            {
                ass_CalOut.uwPreStartCnt ++;
            }
            else
            {
                ass_CalOut.uwPreStartCnt = 0;
                Ass_FSM = TorqueAssit; 
            }

            /* 预启动阶段，停机判断 */
            if((ass_CalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE) || (BMS_RunInfo.Status.Status_Bit.Charge==TRUE) || \
               ((ass_CalIn.blStopAssistEnble == TRUE) || (ass_CalIn.StartAssistEnble == FALSE)))
            {
                Ass_FSM = PreStop;
            }
        break;

        case TorqueAssit:
            /* 力矩助力标志置位 */
            ass_CalOut.blAssistflag = TRUE;

            /* 平滑系数值计算和限幅 */
            ass_CalCoef.swSmoothGain  += StartUpGainAddStep;//(SWORD)ass_CalCoef.uwStartUpGainAddStep; ////ass_stCalCoef.uwStartUpGainAddStep;
            if(ass_CalCoef.swSmoothGain >= Q12_1)
            {
                ass_CalCoef.swSmoothGain = Q12_1;
            }
            ass_CalCoef.swSmoothStopGain += 64;
            if(ass_CalCoef.swSmoothStopGain >= Q12_1)
            {
                ass_CalCoef.swSmoothStopGain = Q12_1;
            }

            /* 力矩切换系数值计算和限幅 */
            ass_CalCoef.swTorqFilterGain += 4; // Q14 转矩滤波方式切换系数
            if (ass_CalCoef.swTorqFilterGain > Q14_1)
            {
                ass_CalCoef.swTorqFilterGain = Q14_1;
            }

            /* 电压限幅值计算和限幅 */
            ass_CalOut.slUqLimSum += ass_CalOut.swUqLimStep;		
            
            if (ass_CalOut.slUqLimSum > ((SLONG)scm_swVsDcpLimPu_Assist << 8))
            {
                ass_CalOut.slUqLimSum = (SLONG)scm_swVsDcpLimPu_Assist << 8;
            }
            else if(ass_CalOut.slUqLimSum < -((SLONG)scm_swVsDcpLimPu_Assist << 8))
            {
                ass_CalOut.slUqLimSum = -((SLONG)scm_swVsDcpLimPu_Assist << 8);
            }
            else
            {
                //中间不操作
            }
            tmp_slVoltLimitPu = ass_CalOut.slUqLimSum + tmp_slVoltLimitPu;
            if (tmp_slVoltLimitPu > ((SLONG)scm_swVsDcpLimPu_Assist << 8))
            {
                ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu_Assist;
            }
            else if(tmp_slVoltLimitPu < -((SLONG)scm_swVsDcpLimPu_Assist << 8))
            {
                ass_CalOut.swVoltLimitPu = -scm_swVsDcpLimPu_Assist;
            }
            else
            {
                ass_CalOut.swVoltLimitPu = (SWORD)(tmp_slVoltLimitPu >> 8);
            }

            /* 助力电流读取 */
            ass_CalOut.swTorAssistCurrentTemp = ass_CalOut.swTorRefEnd;

            /* 预启动阶段，停机判断 */
            if((ass_CalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE) || (BMS_RunInfo.Status.Status_Bit.Charge==TRUE) ||
               ((ass_CalIn.blStopAssistEnble == TRUE) || (ass_CalIn.StartAssistEnble == FALSE)))
            {
                Ass_FSM = PreStop;
            }
        break;

        case  PreStop:
            /* 力矩助力标志置位 */
            ass_CalOut.blAssistflag = TRUE;

            /* 助力电流读取 */
            ass_CalOut.swTorAssistCurrentTemp = ass_CalOut.swTorRefEnd;

            ass_CalCoef.swSmoothGain -= 64;		//降速步进值，修改此值实现降速快慢
            if(ass_CalCoef.swSmoothGain <= 0)
            {
                ass_CalCoef.swSmoothGain = 0;
            }
            ass_CalCoef.swSmoothStopGain -= 64;

            /* 电压限幅值计算和限幅 */
            ass_CalOut.slUqLimSum += ass_CalOut.swUqLimStep;		
            
            if (ass_CalOut.slUqLimSum > ((SLONG)scm_swVsDcpLimPu_Assist << 8))
            {
                ass_CalOut.slUqLimSum = (SLONG)scm_swVsDcpLimPu_Assist << 8;
            }
            else if(ass_CalOut.slUqLimSum < -((SLONG)scm_swVsDcpLimPu_Assist << 8))
            {
                ass_CalOut.slUqLimSum = -((SLONG)scm_swVsDcpLimPu_Assist << 8);
            }
            else
            {
                //中间不操作
            }
            tmp_slVoltLimitPu = ass_CalOut.slUqLimSum + tmp_slVoltLimitPu;
            if (tmp_slVoltLimitPu > ((SLONG)scm_swVsDcpLimPu_Assist << 8))
            {
                ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu_Assist;
            }
            else if(tmp_slVoltLimitPu < -((SLONG)scm_swVsDcpLimPu_Assist << 8))
            {
                ass_CalOut.swVoltLimitPu = -scm_swVsDcpLimPu_Assist;
            }
            else
            {
                ass_CalOut.swVoltLimitPu = (SWORD)(tmp_slVoltLimitPu >> 8);
            }		

            /* 重新启动判断*/
            if((ass_CalIn.uwGearSt > 0) && (BikeBrake_blGetstate() == FALSE) &&(BMS_RunInfo.Status.Status_Bit.Charge==FALSE))
            {
                if(ass_CalIn.StartAssistEnble == TRUE)
                {
                    Ass_FSM = TorqueAssit;
                }
            }

            /* 预停机完成判断*/
            if(ass_CalCoef.swSmoothStopGain <= 0)
            {
                ass_CalCoef.swSmoothStopGain = 0;      
                ass_CalCoef.swSmoothGain = 0;
                ass_CalCoef.swTorqFilterGain = 0;
                MoveAverageFilterClear(&maf_torque);
                Ass_FSM = StopAssit;     
            }
        break;

        default:
        break;
    }

    /* 指令电流限误差处理 */
    if((ass_CalOut.swTorAssistCurrentTemp - ass_CalIn.swCurFdbPu)  * ass_CalIn.swDirection > RefCurrErrLim)
    {
        ass_CalOut.swTorAssistCurrent = ass_CalIn.swCurFdbPu + RefCurrErrLim * ass_CalIn.swDirection;
    }
    else
    {
        ass_CalOut.swTorAssistCurrent = ass_CalOut.swTorAssistCurrentTemp;
    }
    
    /* 指令电流滤波处理 */
     mth_voLPFilter(ass_CalOut.swTorAssistCurrent, &ass_pvt_stCurLpf);

    /* 指令电流增量限幅处理 */
//    if((ass_CalOut.swTorAssistCurrent - ass_pvt_stCurLpf.slY.sw.hi) * ass_CalIn.swDirection >  ass_stCadAssParaPro.swTargetAssCurAcc)
//    {
//        ass_pvt_stCurLpf.slY.sw.hi += ass_stCadAssParaPro.swTargetAssCurAcc * ass_CalIn.swDirection;
//    }
//    else if((ass_pvt_stCurLpf.slY.sw.hi - ass_CalOut.swTorAssistCurrent) * ass_CalIn.swDirection >  (ass_stCadAssParaPro.swTargetAssCurAcc<<1))
//    {
//        ass_pvt_stCurLpf.slY.sw.hi -= (ass_stCadAssParaPro.swTargetAssCurAcc<<1) * ass_CalIn.swDirection;
//    }
//    else
//    {
//        ass_pvt_stCurLpf.slY.sw.hi = ass_CalOut.swTorAssistCurrent;
//    }
    
    /* 指令电流限速处理 */
    Assist_torqueper = ((SLONG)ass_pvt_stCurLpf.slY.sw.hi * ass_CalCoef.swBikeSpeedGain) >> 12;

    /* 指令电流停机平滑处理 */
    Assist_torqueper  = (SLONG)Assist_torqueper * ass_CalCoef.swSmoothStopGain >> 12; 
}

/**
 * @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;
    if(gear > 5)
    {
      gear = 5;
    }
    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 @1ms
 */
void Assist(void)
{ 
    ////////////  Calculate the Iq limit ///////////////////
    UWORD IqLimitTemp;
    AssitCuvLim(ass_CalIn.uwGearSt, ass_CalIn.uwbikespeed, ass_ParaCong.uwCofCurMaxPu);

    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);
//    IqLimitTemp = (IqLimitTemp < ass_CurLimitCalBMSOut.uwIdcLimIqAbs) ? IqLimitTemp : ass_CurLimitCalBMSOut.uwIdcLimIqAbs;
    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 ////////////////////////
    AssitCuvApplPerVolt(); //助力处理

    /////////////// Limit ///////////////////////////
    if (Assist_torqueper > ass_CalCoef.uwCurrentMaxPu)
    {
        Assist_torqueper = ass_CalCoef.uwCurrentMaxPu;
    } 
    else if(Assist_torqueper < -(SWORD)ass_CalCoef.uwCurrentMaxPu)
    {
        Assist_torqueper = -(SWORD)ass_CalCoef.uwCurrentMaxPu;
    }
    else
    {

    } 
}

void MoveAverageFilter(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 MoveAverageFilterClear(MAF_IN *in)
{
    UWORD i;
    in->index = 0;
    in->sum = 0;
    in->blSecFlag = FALSE;

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

void AssistCurveRatio(void) // 上电运行一次or助力参数更新后，AssistCoef需要重新计算
{
    /* 根据OBC调整补偿系数*/
    if (ass_ParaCong.uwStartMode == 1) //
    {
        ass_CalCoef.uwAssCurvGain = _IQ12(0.9);
    }
    else if (ass_ParaCong.uwStartMode  == 2)
    {
        ass_CalCoef.uwAssCurvGain = _IQ12(1.0);
    }
    else if (ass_ParaCong.uwStartMode  == 3)
    {
        ass_CalCoef.uwAssCurvGain = _IQ12(1.1);
    }
    else
    {
        ass_CalCoef.uwAssCurvGain = _IQ12(1.0);
    }
    /*根据电机力矩大小配置补偿系数*/
    // ass_CalCoef.uwAssistCurveGain = ((SQWORD)cp_stMotorPara.swIpeakMaxA * TORQUR_ASSIST_RATIO * TORQUR_ASSIST_BASE_INV >> 24) + 4096 - TORQUR_ASSIST_RATIO; //Q12+Q16 -Q16 =Q12
    // ass_CalCoef.uwAssistCurveGain = (SLONG)ass_CalCoef.uwAssCurvGain * ass_CalCoef.uwAssistCurveGain >> 12; // Q12
    ass_CalCoef.uwAssistCurveGain = ass_CalCoef.uwAssCurvGain;
    /*线性段补偿系数*/
    ass_CalCoef.swAssCompCoef.swKHigh = ass_CalCoef.uwAssistCurveGain;
    /*曲线段补偿系数*/
    ass_CalCoef.swAssCompCoef.swKLow = ass_CalCoef.uwAssistCurveGain;

     memcpy(&ass_CalCoef.uwCadencAsseGain[1], &Syspara2.flash_stPara.slCadAssGain[0], sizeof(Syspara2.flash_stPara.slCadAssGain));
}

void AssistCurveCoef(ASS_CURVE_COMP_COEF *comp)
{
    /*传感器输入力矩参考点*/
    SWORD swX1 = ASS_CURVE_X1 * TORQUE2PU >> 6; //Q14
    SWORD swX2 = ASS_CURVE_X2 * TORQUE2PU >> 6; //Q14
    SWORD swX3 = ASS_CURVE_X3 * TORQUE2PU >> 6; //Q14
    SWORD swY1,swY2,swY3,swZ;
    SLONG sltmpk2;
    
    /*电机输出力矩参考点*/
     memcpy(&ass_CalCoef.slAssCurPre[0], &Syspara2.flash_stPara.slTorqAssGain[0], sizeof(ass_CalCoef.slAssCurPre));

    for(UWORD i = 0; i < 5; i++)
    {
        swY1 = (ass_CalCoef.slAssCurPre[i].slY1 * TORQUE2PU >> 6) * comp->swKLow >> 12 ; //Q20 - Q6 + Q12 - Q12 = Q14
        swY2 = (ass_CalCoef.slAssCurPre[i].slY2 * TORQUE2PU >> 6) * comp->swKLow >> 12;  //Q20 - Q6 + Q12 - Q12 = Q14
        swY3 = (ass_CalCoef.slAssCurPre[i].slY3 * TORQUE2PU >> 6) * comp->swKHigh >> 12; //Q20 - Q6 + Q12 - Q12 = Q14
        swZ = ass_CalCoef.slAssCurPre[i].slZ * TORQUE2PU >> 6;  //Q20 -Q16 = Q14
        /* k1 =(y2-y1)/(x2-x1) */
        ass_CalCoef.swAssCurCoef[i].swk1 = (((SLONG)swY2-(SLONG)swY1)<<10)/(swX2-swX1); // Q10  
        /* b = y2-k2*x1 */
        ass_CalCoef.swAssCurCoef[i].swb = ((SLONG)swY2 - ((SLONG)ass_CalCoef.swAssCurCoef[i].swk1 * swX2 >> 10));// Q14
        /* k2 = (y3-k1*x3-b)/(x3-Z)^2 */
        sltmpk2 = (SLONG)swY3 - ((SLONG)ass_CalCoef.swAssCurCoef[i].swk1 * swX3 >> 10)- (SLONG)ass_CalCoef.swAssCurCoef[i].swb; //Q14
        ass_CalCoef.swAssCurCoef[i].swk2 = ((SQWORD)sltmpk2 << 20 )/((SLONG)(swX3-swZ)*(swX3-swZ)); //Q20
        ass_CalCoef.swAssCurCoef[i].swZ = swZ;
    }
}

SLONG AssistCurveCal(ASS_CURVE_COEF *coef, SWORD *value)
{
    SLONG out;
    if(*value < coef->swZ)
    {
        /* Y = k1*x+b    x< Z */
        out = ((SLONG)*value * coef->swk1 >> 10) + coef->swb;
    }
    else
    {
        /* Y = k1*x+k2*(x-Z)^2+b  x>= Z */
        out = ((SLONG)*value * coef->swk1 >> 10) + coef->swb + ((SQWORD)coef->swk2 * (*value - coef->swZ) * (*value - coef->swZ) >> 20); 
    }
    return out;
}


void ass_voTorqAssistInput(void)
{
    /** Flux current limit input */
    if(cp_stFlg.RotateDirectionSelect == ForwardRotate)
    {
        ass_CalIn.swDirection = 1;
    }
    else 
    {
        ass_CalIn.swDirection = -1;
    }

    /** BMS SOCValue */
    ass_CalIn.SOCValue = MC_RunInfo.SOC;

    /** Flux current limit input */
    ass_CalIn.swFlxIqLimit = abs(flx_stCtrlOut.swIqLimPu);

    /** Power limit current limit input */
    ass_CalIn.swPwrIqLimit = abs(pwr_stPwrLimOut2.swIqLimPu);
   
    /** Assist gear input */
    ass_CalIn.uwGearSt = cp_stBikeRunInfoPara.uwBikeGear;

    /** Cadence last frequency  input */
    ass_CalIn.uwcadancelast =  ass_CalIn.uwcadance;

    /** Cadence frequency  input */
    ass_CalIn.uwcadance = cadence_stFreGetOut.uwFrequencyPu;

    /** Cadence frequency percentage input */
    ass_CalIn.uwcadancePer = cadence_stFreGetOut.uwFreqPercent;

    /** Cadence forword pulse counter input */
    ass_CalIn.uwcadanceFWCnt = cadence_stFreGetOut.uwForwardCnt;
    
    /** Bike speed forword pulse counter input */
    ass_CalIn.uwbikespeed = bikespeed_stFreGetOut.uwLPFFrequencyPu;
    
    /** Motor abs speed input */
    ass_CalIn.uwSpdFbkAbsPu = scm_uwSpdFbkLpfAbsPu;

    /** Motor speed input */
    ass_CalIn.swSpdFbkPu = scm_stSpdFbkLpf.slY.sw.hi;
    
    /** Motor speed base rpm input */
    ass_CalIn.uwBaseSpdrpm = cof_uwVbRpm;

    /** Torque move average filter input */
    ass_CalIn.uwtorque = maf_torque.AverValue; 

    /** Torque low pass filter input */
    ass_CalIn.uwtorquelpf = torsensor_stTorSensorOut.uwTorqueLPFPu;

    /** Instant torque input */
    ass_CalIn.uwtorquePer = torsensor_stTorSensorOut.uwTorquePu;

    /** Current feedback input */
    ass_CalIn.swCurFdbPu = scm_stIqFbkforDesat.slY.sw.hi;
//    ass_CalIn.swCurFdbPu = scm_swIqFdbLpfPu;

    /** Current reference input */
    ass_CalIn.swCurRefPu = scm_swIqRefPu;
}

void AssistCurrentLimitAccordingBMS(UWORD uwSOCvalue)
{
    if (uwSOCvalue < ass_CurLimCalBMSCoef.uwIqLimitStartSoc && uwSOCvalue > ass_CurLimCalBMSCoef.uwIqLimitEndSoc)
    {
        ass_CurLimitCalBMSOut.uwIqLimitAbs =
            ass_ParaCong.uwCofCurMaxPu - (((SLONG)ass_CurLimCalBMSCoef.uwIqLimitStartSoc - uwSOCvalue) * ass_CurLimCalBMSCoef.swIqLImitK);
    }
    else if (uwSOCvalue <= ass_CurLimCalBMSCoef.uwIqLimitEndSoc)
    {
        ass_CurLimitCalBMSOut.uwIqLimitAbs = 
            ass_ParaCong.uwCofCurMaxPu - (((SLONG)ass_CurLimCalBMSCoef.uwIqLimitStartSoc - ass_CurLimCalBMSCoef.uwIqLimitEndSoc) * ass_CurLimCalBMSCoef.swIqLImitK);
    }
    else
    {
        ass_CurLimitCalBMSOut.uwIqLimitAbs = ass_CurLimCalBMSCoef.uwIqLimitInitAbs;
    }
}

void AssCurLimAccordIdc(UWORD uwIdcPu)
{
    if (uwIdcPu > ass_CurLimCalBMSCoef.uwIqLimitStartIdc && uwIdcPu < ass_CurLimCalBMSCoef.uwIqLimitEndIdc)
    {
        ass_CurLimitCalBMSOut.uwIdcLimIqAbs =
        ass_CurLimCalBMSCoef.uwIqLimitInitAbs - (((SLONG)uwIdcPu - ass_CurLimCalBMSCoef.uwIqLimitStartIdc) * ass_CurLimCalBMSCoef.swIqLImitIdcK);
    }
    else if (uwIdcPu >= ass_CurLimCalBMSCoef.uwIqLimitEndIdc)
    {
        ass_CurLimitCalBMSOut.uwIdcLimIqAbs = 0;
    }
    else
    {
        ass_CurLimitCalBMSOut.uwIdcLimIqAbs = ass_CurLimCalBMSCoef.uwIqLimitInitAbs;
    }
}