伟图尔定制

User project/3.BasicFunction/Include/AssistCurve.h

@@ -27,9 +27,12 @@ extern "C" {
 #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
@@ -133,6 +136,30 @@ extern "C" {
         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//////////////
@@ -147,6 +174,40 @@ extern "C" {
  *          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
@@ -285,6 +346,14 @@ 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];
@@ -478,7 +547,8 @@ 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);
+void ass_voAssistCurveCoef(ASS_CURVE_COMP_COEF *comp);
+SLONG ass_voAssistCurveCal(ASS_CURVE_COEF *coef, SWORD *value);
 
 #ifdef __cplusplus
 }

User project/3.BasicFunction/Source/AssistCurve.c

@@ -88,6 +88,88 @@ static SLONG ass_slPolynomial(const POLY_COEF *coef, const SWORD *value, UWORD Q
     return out;
 }
 
+void ass_voAssistCurveRatio(void) // 上电运行一次or助力参数更新后，AssistCoef需要重新计算
+{
+    /* 根据OBC调整补偿系数*/
+    if (ass_stParaCong.uwStartMode == 1) //
+    {
+        ass_stCalCoef.uwAssCurvGain = _IQ12(0.9);
+    }
+    else if (ass_stParaCong.uwStartMode  == 2)
+    {
+        ass_stCalCoef.uwAssCurvGain = _IQ12(1.0);
+    }
+    else if (ass_stParaCong.uwStartMode  == 3)
+    {
+        ass_stCalCoef.uwAssCurvGain = _IQ12(1.1);
+    }
+    else
+    {
+        ass_stCalCoef.uwAssCurvGain = _IQ12(1.0);
+    }
+    ass_stCalCoef.uwAssistCurveGain = ass_stCalCoef.uwAssCurvGain;
+    /*线性段补偿系数*/
+    ass_stCalCoef.swAssCompCoef.swKHigh = ass_stCalCoef.uwAssistCurveGain;
+    /*曲线段补偿系数*/
+    ass_stCalCoef.swAssCompCoef.swKLow = ass_stCalCoef.uwAssistCurveGain;
+    
+    memcpy(&ass_stCalCoef.uwCadencAsseGain[1], &flash_stPara.slCadAssGain[0], sizeof(flash_stPara.slCadAssGain));
+}
+void ass_voAssistCurveCoef(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;
+    UBYTE num = 5;
+#if(WEITUER_ENABLE == 1)
+    num = 7;
+    SLONG slTorqAssGain[7][4] = {10,50,200,100,\
+                                    20,70,270,100,\
+                                    30,90,390,100,\
+                                    40,110,550,100,\
+                                    50,130,700,100,\
+                                    60,150,1050,100,\
+                                    40,10,1050,100};
+
+    memcpy(&ass_stCalCoef.slAssCurPre[0], &slTorqAssGain[0], sizeof(slTorqAssGain));
+#else    
+    /*电机输出力矩参考点*/
+    memcpy(&ass_stCalCoef.slAssCurPre[0], &flash_stPara.slTorqAssGain[0], sizeof(ass_stCalCoef.slAssCurPre));
+#endif    
+    for(UWORD i = 0; i < num; i++)
+    {
+        swY1 = (ass_stCalCoef.slAssCurPre[i].slY1 * TORQUE2PU >> 6) * comp->swKLow >> 12 ; //Q20 - Q6 + Q12 - Q12 = Q14
+        swY2 = (ass_stCalCoef.slAssCurPre[i].slY2 * TORQUE2PU >> 6) * comp->swKLow >> 12;  //Q20 - Q6 + Q12 - Q12 = Q14
+        swY3 = (ass_stCalCoef.slAssCurPre[i].slY3 * TORQUE2PU >> 6) * comp->swKHigh >> 12; //Q20 - Q6 + Q12 - Q12 = Q14
+        swZ = ass_stCalCoef.slAssCurPre[i].slZ * TORQUE2PU >> 6;  //Q20 -Q16 = Q14
+        /* k1 =(y2-y1)/(x2-x1) */
+        ass_stCalCoef.swAssCurCoef[i].swk1 = (((SLONG)swY2-(SLONG)swY1)<<10)/(swX2-swX1); // Q10  
+        /* b = y2-k2*x1 */
+        ass_stCalCoef.swAssCurCoef[i].swb = ((SLONG)swY2 - ((SLONG)ass_stCalCoef.swAssCurCoef[i].swk1 * swX2 >> 10));// Q14
+        /* k2 = (y3-k1*x3-b)/(x3-Z)^2 */
+        sltmpk2 = (SLONG)swY3 - ((SLONG)ass_stCalCoef.swAssCurCoef[i].swk1 * swX3 >> 10)- (SLONG)ass_stCalCoef.swAssCurCoef[i].swb; //Q14
+        ass_stCalCoef.swAssCurCoef[i].swk2 = ((SQWORD)sltmpk2 << 20 )/((SLONG)(swX3-swZ)*(swX3-swZ)); //Q20
+        ass_stCalCoef.swAssCurCoef[i].swZ = swZ;
+    }
+}
+SLONG ass_voAssistCurveCal(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;
+}
 /**
  * @brief   Y = z*(x-h)^2 + k to  Y = a*X^3 + b*X^2 + c*x +d
  *
@@ -297,6 +379,10 @@ void ass_voAssitCoef(void)
     		(SWORD)ass_stCurLimCalBMSCoef.uwIqLimitInitAbs / ((SWORD)ass_stCurLimCalBMSCoef.uwIqLimitStartSoc - (SWORD)ass_stCurLimCalBMSCoef.uwIqLimitEndSoc);
     /*助力曲线初始化*/
     ass_voAssistModeSelect();
+    /*根据仪表选型配置整体曲线补偿系数*/
+    ass_voAssistCurveRatio();
+    /*助力曲线系数计算*/
+    ass_voAssistCurveCoef(&ass_stCalCoef.swAssCompCoef);
 
     /*助力启动阈值初始化*/
     ass_stCalCoef.uwAssThreshold = (UWORD)(((ULONG)ass_stParaSet.uwAssistStartNm << 14) / TORQUEBASE);    // Q14
@@ -562,7 +648,36 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
         ass_stCalCoef.swCadanceGain = 4096; //Q12
     }
     /*  Assist torque Cal using Assist Curve */
-#if(GIANT_ENABLE != 1)
+#if(GIANT_ENABLE == 1)
+    slTeTorAssitTmpPu = (SLONG)giant_slPolynomial(&swTorqCmd);
+#elif(WEITUER_ENABLE == 1)
+        UWORD uwBikeGear_tmp;
+        if (MC_ControlCode.GearSt <= 0x05)
+        {
+            uwBikeGear_tmp = (UWORD)MC_ControlCode.GearSt;
+        }
+        else if (MC_ControlCode.GearSt == 0x33)
+        {
+            uwBikeGear_tmp = 6;
+        }
+        else if (MC_ControlCode.GearSt == 0x44)
+        {
+            uwBikeGear_tmp = 7;
+        }
+        else
+        {
+            uwBikeGear_tmp = 0;
+        }
+
+        if(uwBikeGear_tmp == 0)
+        {
+            slTeTorAssitTmpPu = 0;
+        }
+        else
+        {
+            slTeTorAssitTmpPu = ass_voAssistCurveCal(&ass_stCalCoef.swAssCurCoef[uwBikeGear_tmp-1], &swTorqCmd);
+        }
+#else
     slTeTorAssitTmpPu = (SLONG)(ass_slPolynomial(&ass_stCalCoef.uwTorqueAssGain[ass_stCalIn.uwGearSt], &swTorqCmd, 14)); // Q14  转矩助力曲线
     if(ass_stCalIn.uwGearSt == 5)
     {
@@ -590,8 +705,6 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
     {
         slTeTorAssitTmpPu = (slTeTorAssitTmpPu * ass_stCalCoef.ucAssistRatioGain[ass_stCalIn.uwGearSt-1] / 100);//助力比增益
     }
-#else
-    slTeTorAssitTmpPu = (SLONG)giant_slPolynomial(&swTorqCmd);
 #endif
                                         
     swCadCmd = (SWORD)((((SLONG)ass_stCalIn.uwcadance * ass_stCalCoef.swSmoothGain) >> 12) * 10);  // 踏频指令斜坡

User project/3.BasicFunction/Source/bikespeed.c

@@ -20,6 +20,8 @@
 #include "api.h"
 #include "board_config.h"
 #include "AssistCurve.h"
+#include "bikethrottle.h"
+#include "Cadence.h"
 /******************************
  *
  *  Parameter

User project/3.BasicFunction/Source/canAppl.c

@@ -1098,6 +1098,13 @@ void Can_GearSt_switch(void)
         {
             // do nothing
         }
+#if(WEITUER_ENABLE == 1)
+        
+        if(MC_ControlCode.GearSt == 0x44)
+        {
+            cp_stBikeRunInfoPara.uwBikeGear = 5;
+        }
+#endif
     }
     else 
     {