motor_ctl-4/User project/3.BasicFunction/Include/AssistCurve.h

/**
 * @file AssistCurve.c
 * @author Zhang, Kai(zhangkai71@midea.com)
 * @brief
 * @version 0.1
 * @date 2021-11-15
 *
 * @copyright Copyright (c) 2021
 *
 */

#ifndef ASSISTCURVE_H
#define ASSISTCURVE_H

#include "asr.h"
#include "mathtool.h"

#ifdef __cplusplus
extern "C" {
#endif // __cplusplus
/****************************************
 *
 *          Definitions & Macros
 *
 ****************************************/
#define TIMEUNIT 1 // run time unit  1ms
#define CURSWITCH 1// 开关电流斜坡
   
#define MOTORSPEEDLIMIT_ENABLE 0 // 0-disable,1-enable

#define CURVE_NUM            10 // number of gear
#define GEAR_NUM                6 // number of gear
#define CADENCE_PULSES_PER_CIRC 64
   
#define TORQUE2PU                       1165    //  Q20 1<<20/TORQUEBASE  = (1<<20)/900

#define BIKE_WHEEL_PERIMETER           219 // CM
#define BIKE_MECH_RATION              35
#define BIKE_SPEED_IQLIMIT_THRESHOLD1 25                                // Km/h
#define BIKE_SPEED_IQLIMIT_THRESHOLD2 (BIKE_SPEED_IQLIMIT_THRESHOLD1 + 2) // Km/h
#define BIKE_SPEED_THROTTLE_MAX       25                                 // Km/h
#define BIKE_SPEED_WALK_MAX            6                                 // Km/h
#define BIKE_FRONTTEETH_NMB          38                                 // number
#define BIKE_BACKTEETH_NMB            11                                 // number
#define BIKE_BACKLIGHT_MODE           2      //mode 1-6
#define BIKE_BACKLIGHT_VOLTAGE        6     //6 or 12V
#define BIKE_FORWARDLEDLIGHT_VOLTAGE  6     //6 or 12V
#define BIKE_LIGHT_PARA               ((BIKE_BACKLIGHT_MODE<<12)|(BIKE_BACKLIGHT_VOLTAGE<<8)|BIKE_FORWARDLEDLIGHT_VOLTAGE)     
#define BIKE_WHEEL_SIZE_ADJUST        0     //cm
#define BIKE_START_MODE               2     //Normal
#define BIKE_POWERSTART_DELAY_TIME    0     //0.2s
#define BIKE_POWEROFF_DELAY_TIME      0     //0.2s
#define BIKE_AUTO_POWER_OFF_TIME      15     //min
#define BIKE_POWER_PARA               ((BIKE_POWERSTART_DELAY_TIME<<12)|(BIKE_POWEROFF_DELAY_TIME<<8)|BIKE_AUTO_POWER_OFF_TIME)
#define BIKE_THROTTLE_SMOOTH_INCREASE 15     //*10
#define BIKE_THROTTLE_SMOOTH_DECREASE 30     //*10
#define BIKE_THROTTLE_SMOOTH           ((BIKE_THROTTLE_SMOOTH_INCREASE<<8)|(BIKE_THROTTLE_SMOOTH_DECREASE))
#define BIKE_NONEOBCEBNABLE           0x00    //0xAA-支持，其他不支持
#define BIKE_REARLIGHTCYCLE           8         //低亮尾灯周期
#define BIKE_REARLIGHTDUTY            4         //低亮尾灯占空比
#define BIKE_DELTASPEEDLIMITION       0         //只能是负数
#define BIKE_ASSIST_MODE1              0x0155
#define BIKE_ASSIST_MODE2              0x02AA
#define TORQUE_START_THRESHOLD   70 // 0.1Nm
#define TORQUE_STOP_THRESHOLD    30 // 0.1Nm
#define TORQUE_SWITCH2_THRESHOLD 120 // 0.1Nm
#define TORQUE_SWITCH1_THRESHOLD 100 // 0.1Nm

#define BIKE_ASS_MOTOR_TORQUE_MAX       30   // 0.1Nm
#define BIKE_SPD_MOTOR_CURRENT_MAX      5  /* 电流系数初始值 */
#define BIKE_SPD_MOTOR_CONSTANT_COMMAND 4096  /* 速度波动补偿系数初始值 */

//#define CUR_LIM_DEFAULT                    \
//    {                                      \
//        1024, 512, 716, 870, 1024, 1024 \
//    } // 电流0-100% 1-50% 2-70% 3-85% 4-100% 5-100% Q10  
#define CUR_LIM_DEFAULT                    \
    {                                      \
        1024, 1024, 1024, 1024, 1024, 1024 \
    } // 电流0-100% 1-50% 2-70% 3-85% 4-100% 5-100% Q10  
#define ASS_LIM_DEFAULT                    \
    {                                      \
        {0, 716, 870, 1024, 1024, 1024}, 0 \
    } // Q10 percentage 0 70% 85% 100% 100% 100%
#define TORQUE_CAL_IN_DEFAULT     \
    {                             \
        0, 0, 0, 0, 0, 0, 0, 0, 0 \
    }
#define START_GAIN_DEFAULT             \
    {                                  \
        16384, 10240, 6144, 4778, 4096 \
    } // Q12

#define ASS_LINER_TORQUE_DEFAULT             \
    {                                  \
        2048, 2867, 4096, 10000, 2867 \
    } // Q12
//#define TORQUE_ASSIST_DEFAULT                                                                                                                      \
//    {                                                                                                                                              \
//      0, 1290, 1228, 91,\
//      0, 2212, 1024, 91,\
//      0, 3355, 819, 91, \
//      0, 4792, 614, 0,\
//      0, 6636, 409, 0,\
//      0, 8478, 409, 0,\
//      0, 11059, 0, 0,\
//      0, 13639, 0, 0, \
//      0, 16957, 0, 0,\
//      0, 22118, 0, 0,\
//      16589, 22118, 0, 0,\
//      33178, 25805, 0, 0, \
//      66355, 29491, 0, 0, \
//      132710, 29491, 0, 0,\
//      232243, 29491, 0, 0 \
//    }
#define TORQUE_ASSIST_DEFAULT                                                                                                                      \
    {                                                                                                                                              \
      0, 0, 2048, 137,\
      0, 0, 2458, 137,\
      0, 0, 2867, 137, \
      0, 7004, 410, 273,\
      0, 8663, 410, 273,\
      0, 10506, 410, 273,\
      9953, 16957, 0, 273,\
      16589, 21012, 0, 273, \
      36495, 22856, 0, 273,\
      82944, 29491, 0, 137,\
      149299, 29491, 0, 137,\
      232243, 29491, 0, 137, \
      82944, 29491, 0, -683,\
      149299, 29491, 0, -683,\
      232243, 29491, 0, -683, \
    }
#define CADENCE_ASSIST_DEFAULT                                                       \
    {                                                                                \
        0, 0, 10000, 0, 0, 0, 10000, 0, 0, 0, 10000, 0, 0, 0, 10000, 0, 0, 0, 10000, 0 \
    }

#define TORQUR_ASSIST_BASE       M_IS_PEAK_MAX_AP       //5500 // Maximum Current
#define TORQUR_CP(x)             ((ULONG)(x)*M_IS_PEAK_MAX_AP /TORQUR_ASSIST_BASE)
#define TORQUE_ASSIST_DEFAULT2            \
    {                                     \
      TORQUR_CP(10), TORQUR_CP(50), TORQUR_CP(200), 100,\
      TORQUR_CP(30), TORQUR_CP(80), TORQUR_CP(350), 100,\
      TORQUR_CP(50), TORQUR_CP(120), TORQUR_CP(600), 100, \
      TORQUR_CP(70), TORQUR_CP(260), TORQUR_CP(1200), 100,\
      TORQUR_CP(10), TORQUR_CP(90), TORQUR_CP(800), 100,\
      0, 0, 0, 0,\
      0, 0, 0, 0,\
      0, 0, 0, 0,\
      0, 0, 0, 0,\
      0, 0, 0, 0,\
      0, 0, 0, 0,\
      0, 0, 0, 0,\
      0, 0, 0, 0,\
      0, 0, 0, 0,\
      0, 0, 0, 0,\
    }// Y1, Y2, Y3, Z 单位：0.1 Nm 
#define   ASS_CURVE_X1    30 // 单位：0.1Nm
#define   ASS_CURVE_X2    100 // 单位：0.1Nm
#define   ASS_CURVE_X3    300 // 单位：0.1Nm
 
#define ASSISTMOD_SELECT_DEFAULT 0x0000

////////////////////////////////   just for Assist Module     It may has bug when u used in other Module//////////////
#define TIME_MS2CNT(X) ((ULONG)(X) / TIMEUNIT)  //   X unit:ms
#define PERCENT30(x)   (((ULONG)(x)*300) >> 10) //   X is variable needed to percentage
#define PERCENT50(x)   (((ULONG)(x)*500) >> 10) //   X is variable needed to percentage

#define Q12_1 4096  //   Q12(1)=4096
#define Q14_1 16384 //   Q14(1)=16384
/***************************************
 *
 *          Type  Definations
 *
 ***************************************/
 /**
 * @brief Assist Curve 
 *        Y = a*X^3 + b*X^2 + c*x +d
 */
typedef struct
{
    SLONG slY1; // 助力曲线点位坐标（X1,Y1）输入X1输出Y1 配置值 单位：0.1Nm
    SLONG slY2; // 助力曲线点位坐标（X2,Y2）输入X1输出Y2 配置值 单位：0.1Nm
    SLONG slY3; // 助力曲线点位坐标（X3,Y3）输入X1输出Y3 配置值 单位：0.1Nm
    SLONG slZ;  // 线性段与二次曲线切换点， 配置值 单位：0.1Nm
} ASS_CURVE_PREMETER;

/**
 * @brief Assist Curve coefficient
 *        Y = k1*x+b    x< Z
 *        Y =k1*x+k2*(x-Z)^2+b  x>= Z
 */
typedef struct
{
    SWORD swk1; // Q10   k1 =(y2-y1)/(x2-x1)
    SWORD swb;  // Q0    b = y2-k2*x1
    SWORD swk2; // Q20   k2 = (y3-k1*x3-b)/(x3-Z)^2
    SWORD swZ; //  Q14   
} ASS_CURVE_COEF;

/**
 * @brief Assist Curve compenssation coefficient 
 *       
 */
typedef struct
{
    SWORD swKLow;   // Q12   
    SWORD swKHigh;  // Q12   
} ASS_CURVE_COMP_COEF;
/**
 * @brief Polynomial coefficient
 *        Y = a*X^3 + b*X^2 + c*x +d
 *
 */
typedef struct
{
    SLONG a; // Q12
    SLONG b; // Q12
    SLONG c; // Q12
    SLONG d; // Q12
} POLY_COEF;

/**
 * @brief Polynomial coefficient
 *        Y = z*(x-h)^2 + k
 *
 */
typedef struct
{
    SLONG z; // Q12
    SLONG h; // Q12
    SLONG k; // Q12
} ORIG_COEF;

/**
 * @brief Type of Assist FSM
 *
 */
typedef enum
{
    Startup = 0,
    TorqueAssit = 1,
    SpeedAssit = 2,
    Spd2Torq = 3,
    StartupCruise = 4,
    StopAssit = 5,
    ReduceCurrent = 6,
    Prepare = 7
} ASS_FSM_STATUS;
/**
 * @brief AssistCurve Para Configure of Bike
 *
 */
typedef struct
{
    // Bike info
    UWORD uwWheelPerimeter;     // Q0  0.1cm    
    UWORD uwMechRationMotor;
    UWORD uwThrottleMaxSpdKmH;
    UWORD uwCartSpdKmH;
    UWORD uwNmFrontChainring;
    UWORD uwNmBackChainring;
    UWORD uwAssistSelect1;
    UWORD uwAssistSelect2;
    UWORD uwLightVoltage;
    SWORD swDeltPerimeter;
    UWORD uwStartMode;
    UWORD uwAutoPowerOffTime;
    UWORD uwThrottleSmooth;
    
    UWORD uwNoneOBCEnable;
    UWORD uwRearLightCycle;
    UWORD uwRearLightDuty;
    SWORD swDeltaBikeSpeedLimit;
    
    //    SWORD swAlmMotorOverHeatCeVal;
//    SWORD swAlmMotorOverHeatTM;
//    SWORD swAlmPwrLimitMotorStartTempVal;
    UWORD uwControlFunEN;
    UWORD uwThrottleGearEN;

    UWORD uwCadPulsePerCirc;
    UWORD uwBikeAssTorMaxPu;
    // Drive info
    UWORD uwMotorPoles;
    UWORD uwCofCurMaxPu; // Q14,allowed max Current
    UWORD uwCofTorMaxPu; // Q14,allowed max Torque
} ASS_PARA_CONFIGURE;

/**
 * @brief AssistCurve Para Set From extern
 *
 */
typedef struct
{
    UWORD uwStartupCoef; // Q12     assist gain during startup
    UWORD uwStartupCruiseCoef;
    UWORD uwAssistStartNm;
    UWORD uwAssistStopNm;
    UWORD uwStartUpGainStep;
    UWORD uwStartUpCadNm;
    UWORD uwTorLPFCadNm;
    UWORD uwSpeedAssistSpdRpm;
    UWORD uwSpeedAssistIMaxA;
    UWORD uwAssistLimitBikeSpdStart;
    UWORD uwAssistLimitBikeSpdStop;

    UWORD uwCadenceWeight;      // Q12 0-4094    Cadence sensor assist weight of the whole assist current��the torque weight = 4096 - uwTorWeight
    UWORD uwTorWeight;          // Q12 0-4094
    UWORD uwTorAssAjstGain;     // Q12 0-4094
    UWORD uwCadenceAssAjstGain; // Q12 0-4094
    UWORD uwAsssistSelectNum;
    UWORD uwSpdRegion[3];     // Q15 MOTOR SPEED
    UWORD uwSpdRegionGain[3]; // Q12 0-4094

} ASS_PARA_SET;

/**
 * @brief AssistCurve persentage input
 *
 */
typedef struct
{
    UWORD uwGearSt; // Assist Gear
    SWORD swDirection; // Motor direction 
    UWORD uwtorque;
    UWORD uwtorquelpf;
    UWORD uwtorquePer;    // Q15, Torque sensor output persentage  Tmax = 80Nm
    UWORD uwcadancePer;   // Q15, Cadance output persentage Fmax = 5Hz
    UWORD uwcadance;      // Q20, Cadance frequency
    UWORD uwcadancelast;  // Q20, last step Cadance frequency
    UWORD uwcadanceFWCnt; // Cadance pulse count number
    UWORD uwbikespeed;    // Q20, bikespeed hz
    UWORD uwSpdFbkAbsPu;  // Q15, bikespeed hz
    SWORD swSpdFbkPu;
    UWORD uwBaseSpdrpm;
    SWORD swFlxIqLimit; // Q14
    SWORD swPwrIqLimit; // Q14
    UWORD SOCValue;
    SWORD swCurFdbPu; //Q14
    SWORD swCurRefPu;
} ASS_PER_IN;

typedef struct
{
    POLY_COEF uwTorqueAssGain[GEAR_NUM];
    POLY_COEF uwCadencAsseGain[GEAR_NUM];
    
    ASS_CURVE_PREMETER slAssCurPre[CURVE_NUM];
    ASS_CURVE_COEF swAssCurCoef[CURVE_NUM];
    ASS_CURVE_COMP_COEF swAssCompCoef;
    
    UWORD uwAssCurvGain;
    UWORD uwAssistCurveGain;  //根据电机最大电流调整助力曲线
    
    UBYTE  ucAssistRatioGain[5];
    UBYTE  ucAssistAccelerationGain[5];
    UBYTE  ucMaxCurrentGain[5];
    UBYTE  ucMaxTorqueGain[5];
    UWORD     uwStartupGain;
    UWORD     uwStartupCruiseGain; // Q12     assist gain during startup cruise
    SWORD     swCoefStep;
    UWORD     uwSpeedAssistIMaxA;

    UWORD uwAssThreshold;
    UWORD uwAssStopThreshold;
    UWORD StartFlag;

    SWORD swSmoothGain;         // Q12
    SWORD swSmoothStopGain;         /* 停机电流系数 */
    UWORD uwStartUpGainAddStep; // Q12
    UWORD uwStartUpTargetGain;  // Q12
    UWORD uwStartUpTimeCadenceCnt;
    UWORD uwSwitch2TorqThreshold;
    UWORD uwSwitch1TorqThreshold;
    ULONG ulStartupDeltInv;
    SWORD swTorqFilterGain; // Q14  Torque filter change from lpf to move average filter
    SWORD swCadanceGain;

    UWORD uwCadencePeriodCNT;

    SWORD swCadanceCNT;
    SWORD sw2StopCNT;
    SWORD swAss2SpdCNT;

    UWORD uwSpeedConstantCommand;
    UWORD swSpdLoopAbsCurMax;

    SWORD swSpeedlimtrpm;
    SWORD swBikeSpeedGain;
    SWORD swMotorSpeedGain;

    UWORD uwCurrentMaxPu;
    SWORD swCurrentmax_torAssPu;
    SWORD swCurrentmax_cadAssPu;

    BOOL blAssistflag;
    BOOL blHCadLTorflag;
} ASS_PER_COEF;

typedef struct
{
    POLY_COEF uwTorqueAssGain[GEAR_NUM];
    POLY_COEF uwCadencAsseGain[GEAR_NUM];

} ASS_PER_COEFIN;

typedef struct
{
    UWORD swTorAssistSum1;
    UWORD swTorAssistSum2;

    SWORD swTorAss2CurrentTemp;
    SWORD swCadAss2CurrentTemp;
    SWORD swTorAssistCurrentTemp;
    SWORD swTorSpdLoopCurrentTemp;
    SWORD swTorAssistCurrent;
    SWORD swTorRefTarget;
    SWORD swTorRefEnd;
    SWORD swSpeedRef;
    SWORD swCadSpd2MotSpd;
    
    SWORD swVoltLimitPu;
    BOOL blAssHoldFlag;
    BOOL blTorqPIFlg;
    SWORD swAssitCurRef;
} ASS_PER_OUT;

/**
 * @brief AssistCurve current limit input
 *
 */
typedef struct
{
    UWORD uwIqlimit; // Q14, Iq limit
} ASS_CURLIM_OUT;

/**
 * @brief AssistCurve current limit input
 *
 */
typedef struct
{
    UWORD uwLimitGain[GEAR_NUM];
    UWORD uwSpdThresHold;      // Q15 motor speed to decided whether ass or not
    UWORD uwBikeSpdThresHold1; // Q15 the start bike speed to limit iqref
    UWORD uwBikeSpdThresHold2; // Q15 the end bike speed to limit iqref
    ULONG ulBikeSpdDeltInv;    // Q14
    UWORD uwBikeSpdIqLimitK;
    UWORD uwMotorSpdThresHold1;// Q15 the start motor speed to limit iqref
    UWORD uwMotorSpdThresHold2;// Q15 the start motor speed to limit iqref
    ULONG ulMotorSpdDeltInv;    // Q14
} ASS_CURLIM_COEF;

typedef struct
{
    SWORD  swValue;
    UWORD  uwLength; // 6: 2^6 value to average
    SLONG  slSum;
    SLONG  slAverValue;
    SWORD *swBuffer;
    UWORD  uwIndex;
    BOOL   blSecFlag;
} MAF_IN;

typedef struct
{
    UWORD uwMotprPolePairs;
    UWORD uwMotorFluxWb;
    SLONG swCalCoefINV;
} TOR2CURRENT_CAL_COEF;

typedef struct
{
    UWORD uwIqLimitInitAbs;
    UWORD uwIqLimitStartSoc; 
    UWORD uwIqLimitEndSoc;
    SWORD swIqLImitK;
} ASS_LIMIT_ACCORDING_VOL_COF;

typedef struct
{
    UWORD uwIqLimitAbs; 
} ASS_LIMIT_ACCORDING_VOL_OUT;

typedef struct
{
    SWORD swImaxPu;
    SWORD swIminPu;
    SWORD swTorqRefPu;
    SWORD swTorqFdbPu;  
} ASS_TORQ_PI_IN;

typedef struct
{
  SWORD swErrZ1Pu;
  SLONG slIRefPu;
  SWORD swIRefPu;    
} ASS_TORQ_PI_OUT;


/****************************************
 *
 *           Exported variable
 *
 ****************************************/
extern ASS_FSM_STATUS Ass_FSM;

extern ASS_PER_IN   ass_stCalIn;
extern ASS_PER_COEF ass_stCalCoef;
extern ASS_PER_OUT  ass_stCalOut;

extern ASS_PARA_CONFIGURE ass_stParaCong;
extern ASS_PARA_SET       ass_stParaSet;

extern ASS_LIMIT_ACCORDING_VOL_COF ass_stCurLimCalBMSCoef;
extern ASS_LIMIT_ACCORDING_VOL_OUT ass_stCurLimitCalBMSOut;
extern ASS_CURLIM_COEF ass_stCurLimCoef;
extern ASS_CURLIM_OUT  ass_stCurLimOut;

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

extern ASS_TORQ_PI_IN ass_stTorqPIIn;
extern ASS_TORQ_PI_OUT ass_stTorqPIOut;

extern SWORD  ass_swTorqMafBuf[64];
extern MAF_IN ass_stTorqMafValue;
extern SWORD  ass_swUqLimMafBuf[64];
extern MAF_IN ass_stUqLimMafValue;
extern LPF_OUT ass_pvt_stCurLpf;
extern UWORD   ass_uwPwrCurLim[6];
/************************************************************************
 Ram Allocation
*************************************************************************/

/***************************************
 *
 *          Function  Definations
 *
 ***************************************/
void ass_voAssitEEInit(void);
void ass_voAssitCoef(void);
void ass_voAssist(void);
void ass_voMoveAverageFilter(MAF_IN *in);
void ass_voMoveAverageFilterClear(MAF_IN *in);
void ass_voAssistCurveCoef(ASS_CURVE_COMP_COEF *comp);
SLONG ass_voAssistCurveCal(ASS_CURVE_COEF *coef, SWORD *value);

#ifdef __cplusplus
}
#endif // __cplusplus

#endif