motor_ctl-1/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 "typedefine.h"
#include "macroequ.h"
#include "asr.h"
#include "mathtool.h"
#include "user.h"
#include "syspar.h"
//#include "api.h"
#include "board_config.h"
/****************************************
 *
 *          Definitions & Macros
 *
 ****************************************/
///////////////////////////////////////////////////////////////////
#define VolLimCurrLoopErr    _IQ14((2.0*M_IS_PEAK_MAX_AP/7500.0)/60.0)        /* 电压与电限幅流环指令偏差 */
#define VolLimCtrKp          (2250L*M_FLUX_WB/4462L)                   /* Q14电压限幅控制器比例系数 */
#define VolLimCtrKi          (1920L*M_FLUX_WB/4462L)                   /* Q20电压限幅控制器积分系数 */
#define VolLimCtrKiStep      (VolLimCtrKi/120)        /* Q20电压限幅控制器积分系数 */
#define PreStartCurr         _IQ14(5.0/60.0)       /* 预启动电流 */
#define RefCurrErrLim        _IQ14(60.0/60.0)      /* 电流误差限幅 */

//#define GpioCadVolt_InputSta()          GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_10)
#define TIMEUNIT                        1 // run time unit  1ms
#define TIME_MS2CNT(X)                  ((ULONG)(X) / TIMEUNIT)  //   X unit:ms

#define Q12_1                           4096  //   Q12(1)=4096
#define Q14_1                           16384 //   Q14(1)=16384

#define TORQUE2PU                       1165    //  Q20 1<<20/TORQUEBASE  = (1<<20)/900
#define BIKE_SPEED_IQLIMIT_THRESHOLD1   (ASS_SPD_LIMIT >> 4)                  // 0.1Km/h
#define BIKE_SPEED_IQLIMIT_THRESHOLD2   (BIKE_SPEED_IQLIMIT_THRESHOLD1 + 20)  // 0.1Km/h
//档位限速  BIKE_SPDLIMIT_EN
#if(BIKE_TORQUESPDLIMIT_EN!=0)
#define BIKE_SPEED_IQLIMIT_Gear1_S  (BIKE_SPEED_IQLIMIT_THRESHOLD1*12/25)    // Km/h
#define BIKE_SPEED_IQLIMIT_Gear1_E   (BIKE_SPEED_IQLIMIT_Gear1_S + 20) // Km/h
#define BIKE_SPEED_IQLIMIT_Gear2_S  (BIKE_SPEED_IQLIMIT_THRESHOLD1*15/25)                  // Km/h
#define BIKE_SPEED_IQLIMIT_Gear2_E   (BIKE_SPEED_IQLIMIT_Gear2_S + 20) // Km/h
#define BIKE_SPEED_IQLIMIT_Gear3_S  (BIKE_SPEED_IQLIMIT_THRESHOLD1*18/25)                  // Km/h
#define BIKE_SPEED_IQLIMIT_Gear3_E   (BIKE_SPEED_IQLIMIT_Gear3_S + 20) // Km/h
#define BIKE_SPEED_IQLIMIT_Gear4_S  (BIKE_SPEED_IQLIMIT_THRESHOLD1*21/25)                  // Km/h
#define BIKE_SPEED_IQLIMIT_Gear4_E   (BIKE_SPEED_IQLIMIT_Gear4_S + 20) // Km/h
#define BIKE_SPEED_IQLIMIT_Gear5_S  (BIKE_SPEED_IQLIMIT_THRESHOLD1 )                  // Km/h
#define BIKE_SPEED_IQLIMIT_Gear5_E   (BIKE_SPEED_IQLIMIT_Gear5_S + 20) // Km/h
#endif
////////////////////////////////   just for Assist Module     It may has bug when u used in other Module//////////////
#define BIKE_NONEOBCEBNABLE           0x00     //0xAAAA-支持，其他不支持
#define BIKE_REARLIGHTCYCLE           10         //低亮尾灯周期
#define BIKE_REARLIGHTDUTY            5         //低亮尾灯占空比
#define BIKE_DELTASPEEDLIMITION       0         //只能是负数

#define GEAR_NUM                        6 // number of gear
#define BIKE_START_MODE                 2     // 1 Soft 2 Normal 3 Strong
#define ASSISTMOD_SELECT_DEFAULT        0x0000
#define ASSISTMOD_SELECT_MODE1          0x0155
#define ASSISTMOD_SELECT_MODE2          0x02AA

#define TORQUE_START_THRESHOLD          80 // 0.1Nm
#define TORQUE_STOP_THRESHOLD           30 // 0.1Nm

#define BIKE_SPD_MOTOR_CURRENT_MAX      500  // 0.01A
#define BIKE_SPD_MOTOR_CONSTANT_COMMAND 0  // RPM

#define ASS_LIM_DEFAULT                             \
    {                                               \
        {0, 716, 870, 1024, 1024, 1024}, 0, 0, 0    \
    } // Q10 percentage 0 70% 85% 100% 100% 100%

#define ASS_LINER_TORQUE_DEFAULT        \
    {                                   \
        1638, 1843, 2457, 3276, 1843    \
    } // Q12

#if 1
#define TORQUR_ASSIST_BASE       M_IS_PEAK_MAX_AP
#define TORQUR_CP(x)             ((ULONG)(x)*M_IS_PEAK_MAX_AP / TORQUR_ASSIST_BASE)

#define TORQUE_ASSIST_DEFAULT             \
    {                                     \
      TORQUR_CP(7), TORQUR_CP(23), TORQUR_CP(69), 100,\
      TORQUR_CP(15), TORQUR_CP(50), TORQUR_CP(150), 100,\
      TORQUR_CP(32), TORQUR_CP(106), TORQUR_CP(318), 100, \
      TORQUR_CP(44), TORQUR_CP(146), TORQUR_CP(438), 100,\
      TORQUR_CP(15), TORQUR_CP(50), TORQUR_CP(438), 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
#endif


// #define TORQUE_ASSIST_DEFAULT        \
// {                                    \
//     0, 0, 1228, 45,\
//     0, 0, 1433, 45,\
//     0, 0, 1638, 45, \
//     0, 3686, 409, 136,\
//     0, 4976, 409, 136,\
//     0, 6635, 409, 136,\
//     3317, 5529, 819, 137,\
//     1990, 7372, 819, 137, \
//     2654, 8847, 819, 137,\
//     9953, 7372, 1228, 137,\
//     13271, 8847, 1228, 137,\
//     16588, 9584, 1228, 137, \
//     9953, 7372, 1228, -227,\
//     13271, 8847, 1228, -227,\
//     16588, 9584, 1228, -227, \
// }
#define CADENCE_ASSIST_DEFAULT    		\
    {                                   \
        0, 0, 45000, 0, \
        0, 0, 45000, 0, \
        0, 0, 45000, 0, \
        0, 0, 45000, 0, \
        0, 0, 45000, 0 \
    }		//35000改为45000

/***************************************
 *
 *          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
{
    StopAssit = 0,
    PreStart = 1,
    TorqueAssit = 2,
    PreStop = 3
} 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 uwLightConfig;
    SWORD swDeltPerimeter;
    UWORD uwStartMode;
    UWORD uwAutoPowerOffTime;
    UWORD uwThrottleSmooth;

    UWORD uwNoneOBCEnable;
    UWORD uwRearLightCycle;
    UWORD uwRearLightDuty;
    SWORD swDeltaBikeSpeedLimit;

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

/**
 * @brief AssistCurve Para Set From extern
 *
 */
typedef struct
{
    UWORD uwStartupCoef; // Q12     (do not use)
    UWORD uwStartupCruiseCoef;     // (do not use)
    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

    SWORD swUqLimitKi;      //Q20 Uq Limit Calcute Coefficient
} ASS_PARA_SET;

/**
 * @brief AssistCurve persentage input
 *
 */
typedef struct
{
    BOOL  StartAssistEnble; //允许助力启动标志
    BOOL  blStopAssistEnble; //允许助力启动标志
    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 uwbikespeedCal;    // Q20, bikespeed hz
    UWORD uwSpdFbkAbsPu;  // Q15, bikespeed hz
    SWORD swSpdFbkPu;   // Q15, motor speed base 1400hz
    UWORD uwBaseSpdrpm;
    SWORD swFlxIqLimit; // Q14
    SWORD swPwrIqLimit; // Q14
    UWORD SOCValue;
    SWORD swCurFdbPu; //Q14
    SWORD swCurRefPu;
    UWORD uwStartRunPulse;//启动脉冲个数计数
    UWORD uwcadHighStopCnt;//踏频为0时，开始计数值
    UWORD uwcadLowStopCnt;//踏频为0时，开始计数值
} ASS_PER_IN;

#define TORQUE_CAL_IN_DEFAULT     \
    {                             \
        FALSE,FALSE,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 \
    }

typedef struct
{
    POLY_COEF uwTorqueAssGain[GEAR_NUM];
    POLY_COEF uwCadencAsseGain[GEAR_NUM];
    UBYTE  ucAssistRatioGain[5];
    UBYTE  ucAssistAccelerationGain[5];
    UBYTE  ucMaxCurrentGain[5];
    UBYTE  ucMaxTorqueGain[5];
    ASS_CURVE_PREMETER slAssCurPre[GEAR_NUM-1];
    ASS_CURVE_COEF swAssCurCoef[GEAR_NUM-1];
    ASS_CURVE_COMP_COEF swAssCompCoef; 
    UWORD uwAssCurvGain;

    UWORD uwAssThreshold;
    UWORD uwAssStopThreshold;

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

    SWORD swBikeSpeedGain;
 
    UWORD uwCurrentMaxPu;
    SWORD swCurrentmax_torAssPu;
    SWORD swCurrentmax_cadAssPu;
    
    UWORD uwAssistCurveGain;  //基于250W助力曲线，根据电机最大电流调整助力曲线

} ASS_PER_COEF;

typedef struct
{
    BOOL blAssistflag;              /* 力矩助力输出标志 */

    UWORD swTorAssistSum1;          /* Q14 BASE 90N 总助力力矩 */
    SWORD swTorAss2CurrentTemp;     /* Q14 BASE 60A 力矩助力指令电流绝对值 */
    SWORD swCadAss2CurrentTemp;     /* Q14 BASE 60A 踏频助力指令电流绝对值 */
    SWORD swTorRefTarget;           /* Q14 BASE 60A 总助力指令电流绝对值 */
    SWORD swTorRefEnd;              /* Q14 BASE 60A 总助力指令电流带方向 */
    SWORD swTorAssistCurrentTemp;   /* Q14 BASE 60A 总助力指令电流-状态机处理 */
    SWORD swTorAssistCurrent;       /* Q14 BASE 60A 总助力指令电流-误差限幅处理 */

    SWORD swBikeSpd2MotSpd;         /* Q15 BASE 1400HZ 当前车速对应电机转速 */

    UWORD uwPreStartCnt;            /* 预启动计数，单位1ms */
    UWORD uwPreStartCntMax;         /* 预启动计数最大值，由起动时刻车速决定 */
    SWORD swUqLimStep;              /* Q22 电压限幅积分步进量（1ms周期） */
    SLONG slUqLimSum;               /* Q22 电压限幅积分量 */
    SWORD swUqLimInit;              /* Q14 电压限幅初值 */
    
    SWORD swVoltLimitPu;            /* Q14 电压限幅输出 */
} 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 uwBikeSpdThresHold1; // Q15 the start bike speed to limit iqref
    UWORD uwBikeSpdThresHold2; // Q15 the end bike speed to limit iqref
    ULONG ulBikeSpdDeltInv;    // Q14
} ASS_CURLIM_COEF;
typedef struct
{
UWORD uwLimitdSpeed_S[GEAR_NUM];
UWORD uwLimitdSpeed_E[GEAR_NUM];
ULONG ulBikeSpdGearDeltInv[GEAR_NUM];    // Q14
UWORD uwAssistLimitSpdStart[GEAR_NUM];
UWORD uwAssistLimitSpdStop[GEAR_NUM];
} ASS_SpedLinit_COEF;

typedef struct
{
    SWORD  value;
    UWORD  length; // 6: 2^6 value to average
    SLONG  sum;
    SLONG  AverValue;
    SWORD *buffer;
    UWORD  index;
    BOOL   blSecFlag;
} MAF_IN;

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

typedef struct
{
    UWORD uwIqLimitInitAbs;
    UWORD uwIqLimitStartSoc; // IqLimit
    UWORD uwIqLimitEndSoc;
    SWORD swIqLImitK;
    
    UWORD uwIqLimitStartIdc;
    UWORD uwIqLimitEndIdc;
    SWORD swIqLImitIdcK;
} ASS_LIMIT_ACCORDING_VOL_COF;

typedef struct
{
    UWORD uwIqLimitAbs; // IqLimit
    UWORD uwIdcLimIqAbs;
} ASS_LIMIT_ACCORDING_VOL_OUT;

typedef struct
{
    UWORD uwReserve1;    //助力模块备用EE参数1
    UWORD uwReserve2;    //助力模块备用EE参数2
    UWORD uwReserve3;    //助力模块备用EE参数3
    UWORD uwReserve4;    //助力模块备用EE参数4
} ASS_RESERVE_PARA;

/****************************************
 *
 *           Exported variable
 *
 ****************************************/
extern LPF_OUT ass_pvt_stCurLpf;
   
extern ASS_LIMIT_ACCORDING_VOL_OUT ass_CurLimitCalBMSOut;
extern ASS_LIMIT_ACCORDING_VOL_COF ass_CurLimCalBMSCoef;

extern ASS_FSM_STATUS Ass_FSM;

extern ASS_PER_COEF ass_stCalCoef;

extern ASS_PER_OUT  ass_CalOut;
extern SWORD Assist_torqueper;

extern ASS_PARA_CONFIGURE ass_ParaCong;
extern ASS_PARA_SET       ass_ParaSet;

extern ASS_PER_IN   ass_CalIn;
extern ASS_PER_COEF ass_CalCoef;

extern ASS_CURLIM_OUT  ass_CurLimOut;
extern ASS_CURLIM_COEF ass_CurLimCoef;

extern SWORD  MAF_buffer[60];
extern MAF_IN maf_torque;

extern ASS_RESERVE_PARA ass_stReservePara;
/************************************************************************
 Ram Allocation
*************************************************************************/

/***************************************
 *
 *          Function  Definations
 *
 ***************************************/
/**
 * @brief Assistant force curve
 *
 * @param in
 * @return UWORD
 */
void AssitEEInit(void);
void AssitCoefInit(void);
void Assist(void);
void MoveAverageFilter(MAF_IN *in);
void MoveAverageFilterClear(MAF_IN *in);
void AssistCurveCoef(ASS_CURVE_COMP_COEF *comp);
 SLONG AssistCurveCal(ASS_CURVE_COEF *coef, SWORD *value);


void ass_voTorqAssistInput(void);
void AssistCurrentLimitAccordingBMS(UWORD uwSOCvalue);
void AssCurLimAccordIdc(UWORD uwIdcPu);
void Gear_LimitSpd(UWORD Gear);
/**
 * @brief 选择相应档位曲线
 *
 */
void AssistCurveRatio(void);
#endif