/** * @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) // Km/h #define BIKE_SPEED_IQLIMIT_THRESHOLD2 (BIKE_SPEED_IQLIMIT_THRESHOLD1 + 2) // Km/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 + 2) // 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 + 2) // 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 + 2) // 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 + 2) // 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 + 2) // 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 4200 // 250W Maximum Current #define TORQUR_CP(x) ((ULONG)(x)*M_IS_PEAK_MAX_AP /TORQUR_ASSIST_BASE) #define TORQUE_ASSIST_DEFAULT \ { \ TORQUR_CP(10), TORQUR_CP(40), TORQUR_CP(150), 100,\ TORQUR_CP(20), TORQUR_CP(60), TORQUR_CP(250), 100,\ TORQUR_CP(30), TORQUR_CP(80), TORQUR_CP(350), 100, \ TORQUR_CP(40), TORQUR_CP(100), TORQUR_CP(450), 100,\ TORQUR_CP(10), TORQUR_CP(60), TORQUR_CP(400), 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[64]; 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); uint32_t Get_CAD_PORT(void); #endif