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"
/****************************************
 *
 *          Definitions & Macros
 *
 ****************************************/
#define TIMEUNIT 1 // run time unit  1ms
#define CURSWITCH 1// 开关电流斜坡
   
#define GEAR_NUM                6 // number of gear
#define CADENCE_PULSES_PER_CIRC 64

#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       6                                 // Km/h
#define BIKE_FRONTTEETH_NMB          38                                 // number
#define BIKE_BACKTEETH_NMB            11                                 // number
#define BIKE_BACKLIGHT_MODE           1      //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_ASSIST_MODE1              0x0155
#define BIKE_ASSIST_MODE2              0x02AA
#define TORQUE_START_THRESHOLD   50 // 0.1Nm
#define TORQUE_STOP_THRESHOLD    50 // 0.1Nm
#define TORQUE_SWITCH2_THRESHOLD 120 // 0.1Nm
#define TORQUE_SWITCH1_THRESHOLD 100 // 0.1Nm

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

#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 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 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
} ASS_FSM_STATUS;
/**
 * @brief AssistCurve Para Configure of Bike
 *
 */
typedef struct
{
    // Bike info
    UWORD uwWheelPerimeter;     // Q0  0.1cm    
    UWORD uwMechRationMotor;
    UWORD uwAssistMaxSpdKmH;
    UWORD uwThrottleMaxSpdKmH;
    UWORD uwNmFrontChainring;
    UWORD uwNmBackChainring;
    UWORD uwAssistSelect1;
    UWORD uwAssistSelect2;
    UWORD uwLightVoltage;
    SWORD swDeltPerimeter;
    UWORD uwStartMode;
    UWORD uwAutoPowerOffTime;

    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];
    UWORD     uwStartupGain;
    UWORD     uwStartupCruiseGain; // Q12     assist gain during startup cruise
    SWORD     swCoefStep;

    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;

    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;
} 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;
/************************************************************************
 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_voAssitTorqPI(ASS_TORQ_PI_IN *in, ASS_TORQ_PI_OUT *out);

#endif