零点校准、防飞车，限制最大转速

User project/2.MotorDrive/Include/spdctrFSM.h

@@ -87,7 +87,7 @@ _STARTFSM_EXT void Stop_TbsHook(void);
 _STARTFSM_EXT void scm_voSpdCtrMdFSM(void);
 _STARTFSM_EXT void Switch_speed_FSM(const SPD_STATE_HOOK *in);
 _STARTFSM_EXT void Switch_speed_FSMInit(void);
-
+_STARTFSM_EXT UWORD StartUp_SpiSensorTest(void);
 /************************************************************************
  Exported Variables
 *************************************************************************/

User project/2.MotorDrive/Source/spdctrFSM.c

@@ -34,10 +34,20 @@ Include File
 #include "cmdgennew.h"
 #include "power.h"
 #include "sys_ctrl.h"
+#include "can.h"
+#include "canAppl.h"
+#include "string.h"
 /************************************************************************
  Constant Table:
 ************************************************************************/
-
+UWORD T_rotate_count = 0;
+UWORD T_rotateDelay_count = 0;
+UWORD T_rotateCoef = 0;
+UWORD T_rotateDelayCoef = 0;
+UWORD uwAngle = 0;
+UWORD uwAngle_sector = 0;
+UWORD Angle[44] = {0};//0-SpiThetaOrigin;1-SpiThetaNow;2-44:Angle
+UWORD AngleGet_count = 2;
 /************************************************************************
  Exported Functions:
 ************************************************************************/
@@ -69,8 +79,19 @@ void StartUp_TbcupHook(void)
     scm_swIdRefPu = align_stOut.swIdRefPu;
     scm_swIqRefPu = align_stOut.swIqRefPu;
     scm_slAngManuPu = align_stOut.slAngManuPu;
-    scm_uwAngRefPu = align_stOut.uwAngRefPu;
+//    scm_uwAngRefPu = align_stOut.uwAngRefPu;
     scm_StartUpOvrFlg = align_stOut.blStartUpOvrFlg;
+    
+    if(AngleTest == TRUE)
+    {   
+        scm_swIdRefPu = align_stCoef.uwDragCurPu;
+        scm_swIqRefPu = 0;
+        scm_uwAngRefPu = StartUp_SpiSensorTest();
+    }
+    else
+    {
+        scm_uwAngRefPu = align_stOut.uwAngRefPu;
+    }
 }
 
 void Open2Clz_TbcupHook(void)
@@ -145,7 +166,7 @@ void ClzLoop_TbcupHook(void)
     }
     else
     {
-        scm_swIdRefPu = 0;  //flx_stCtrlOut.swIdRefPu;  
+        scm_swIdRefPu = flx_stCtrlOut.swIdRefPu;  //0;  //
     }  
 
     scm_swIqRefPu = cvb_stBrakeOut.swIqRefPu;
@@ -505,6 +526,14 @@ void scm_voSpdCtrMdFSM(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
 
                 Switch_speed_FSM(&Stop_state);
             }
+            
+            if (uart_swTorqRefNm == 0)
+            {
+                scm_swIdRefPu = 0;
+                scm_swIqRefPu = 0;
+
+                Switch_speed_FSM(&Stop_state);
+            }
         }
         else
         {}
@@ -559,6 +588,58 @@ void Switch_speed_FSMInit(void)
     curSpeed_state = Stop_state;
 }
 
+UWORD StartUp_SpiSensorTest(void)
+{
+    T_rotateCoef = 8*1000/cp_stControlPara.swDragSpdHz/6;
+    T_rotateDelayCoef = T_rotateCoef*50;
+    if(T_rotate_count > 0)
+    {
+        T_rotateDelay_count++;
+        if(T_rotateDelay_count == T_rotateDelayCoef)
+        {
+            Angle[AngleGet_count] = spi_stResolverOut.uwSpiThetaPu;
+            AngleGet_count++;
+            if(AngleGet_count >= 44)
+            {
+              AngleTest = FALSE;
+              Switch_speed_FSM(&Stop_state);
+              AngleGet_count = 2;
+              MC_MotorSPD_rpm_Percent = 0;
+              cp_stFlg.RunModelSelect = MC_UpcInfo.stTestParaInfo.RunModelSelect;
+
+              memcpy((UBYTE*)(Angle), (UBYTE*)(&spi_stResolverOut.swSpiThetaOffsetOrignPu), (uint32_t)2);
+              memcpy((UBYTE*)(Angle+1), (UBYTE*)(&spi_stResolverOut.swSpiThetaOffsetPu), (uint32_t)2);
+              SendData(ID_MC_TO_CDL, MODE_REPORT, 0xBF58, (UBYTE *)&Angle[0]);
+            }
+          
+          T_rotate_count = 0;
+          T_rotateDelay_count = 0;
+          uwAngle_sector++;
+          if(uwAngle_sector==6)
+          {
+            uwAngle_sector = 0;
+          }
+        }
+    }
+    else
+    {
+        if(uwAngle < ((uwAngle_sector+1)*cof_sl60DegreePu - (scm_slDragSpdPu * TBC_TM >> 10)))
+        {
+          uwAngle += ((SQWORD)scm_slDragSpdPu * TBC_TM >> 10);
+        }
+        else
+        {
+          T_rotate_count++;
+          uwAngle = (uwAngle_sector+1)*cof_sl60DegreePu;
+          if (uwAngle >= cof_sl360DegreePu)
+          {
+              uwAngle -= cof_sl360DegreePu;
+          }
+        }
+    }
+    
+    return uwAngle;
+}
 /************************************************************************
  Function: void RUN_FSM_Main(void)
  Description:

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

@@ -26,6 +26,8 @@ extern "C" {
 #define TIMEUNIT 1 // run time unit  1ms
 #define CURSWITCH 1// 开关电流斜坡
    
+#define MOTORSPEEDLIMIT_ENABLE 1 // 0-disable,1-enable
+   
 #define GEAR_NUM                6 // number of gear
 #define CADENCE_PULSES_PER_CIRC 64
 
@@ -290,6 +292,7 @@ typedef struct
 
     SWORD swSpeedlimtrpm;
     SWORD swBikeSpeedGain;
+    SWORD swMotorSpeedGain;
 
     UWORD uwCurrentMaxPu;
     SWORD swCurrentmax_torAssPu;
@@ -348,6 +351,7 @@ typedef struct
     UWORD uwBikeSpdThresHold2; // Q15 the end bike speed to limit iqref
     ULONG ulBikeSpdDeltInv;    // Q14
     UWORD uwBikeSpdIqLimitK;
+    UWORD uwMotorSpdThresHold;
 } ASS_CURLIM_COEF;
 
 typedef struct

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

@@ -653,6 +653,7 @@ extern UBYTE MC_BC_COM;
 extern UWORD LightSwitch;
 extern ULONG ODO_T, TRIP_T;
 extern BOOL  blFlashSaveflg;
+extern BOOL AngleTest;
 /************************************************************************
  Ram Allocation
 *************************************************************************/

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

@@ -325,6 +325,7 @@ void ass_voAssitCoef(void)
 
     ass_stCalCoef.swSpeedlimtrpm = -100;
     ass_stCalCoef.swBikeSpeedGain = 0;
+    ass_stCalCoef.swMotorSpeedGain = Q12_1;
 
     /*设置电流限幅*/
     ass_stCalCoef.uwCurrentMaxPu = ass_stParaCong.uwCofCurMaxPu;
@@ -361,6 +362,11 @@ void ass_voAssitCoef(void)
     ass_stCurLimCoef.ulBikeSpdDeltInv = (ULONG)(((UQWORD)1 << 20) / (ass_stCurLimCoef.uwBikeSpdThresHold2 - ass_stCurLimCoef.uwBikeSpdThresHold1)); // Q20;
     ass_stCurLimCoef.uwBikeSpdIqLimitK =
     		(UWORD)((((ULONG)ass_stCurLimCoef.uwBikeSpdThresHold2 - ass_stCurLimCoef.uwBikeSpdThresHold1) << 8) / ass_stParaCong.uwCofCurMaxPu); // Q28-q14 = Q14;
+#if ((MOTOR_ID_SEL == MOTOR_WELLING_MTB_36V) || (MOTOR_ID_SEL == MOTOR_WELLING_MTB_48V))
+    ass_stCurLimCoef.uwMotorSpdThresHold = SPD_RPM2PU(4290);//125Candance
+#elif ((MOTOR_ID_SEL == MOTOR_WELLING_CITY_36V) || (MOTOR_ID_SEL == MOTOR_WELLING_CITY_48V))
+    ass_stCurLimCoef.uwMotorSpdThresHold = SPD_RPM2PU(3953);//115Candance
+#endif
 
     /*设置转矩电流标定系数*/
     ass_Tor2CurCalCoef.uwMotorFluxWb = cp_stMotorPara.swFluxWb; // 0.001mWb
@@ -980,6 +986,19 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
         uwTorqDecStep = 10;
     }
 
+    if (ass_stCalIn.uwSpdFbkAbsPu <= ass_stCurLimCoef.uwMotorSpdThresHold)
+    {
+        ass_stCalCoef.swMotorSpeedGain = Q12_1; // Q12
+    }
+    else
+    {
+#if MOTORSPEEDLIMIT_ENABLE
+        ass_stCalCoef.swMotorSpeedGain = 0; // Q12
+#else
+        ass_stCalCoef.swMotorSpeedGain = Q12_1; // Q12
+#endif
+    }
+    
     /* Assist Current Output in each FSM */
     switch (Ass_FSM)
     {
@@ -1116,6 +1135,7 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
     mth_voLPFilter(ass_stCalOut.swTorAssistCurrent, &ass_pvt_stCurLpf);   
     /* Bikespeed Limit Coef*/
     ass_stCalOut.swAssitCurRef = (SWORD)((SLONG)ass_pvt_stCurLpf.slY.sw.hi * ass_stCalCoef.swBikeSpeedGain >> 12);
+    ass_stCalOut.swAssitCurRef = (SWORD)((SLONG)ass_stCalOut.swAssitCurRef * ass_stCalCoef.swMotorSpeedGain >> 12);
     ass_stCalOut.swAssitCurRef  = (SLONG)ass_stCalOut.swAssitCurRef  * ass_stCalCoef.swSmoothStopGain >> 12; 
     //ass_stCalOut.swAssitCurRef =ass_stCalOut.swTorAssistCurrent;
 }

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

@@ -1021,6 +1021,28 @@ void DataProcess(UWORD ID, UBYTE Mode, UWORD Cmd, UBYTE Data[]) /* parasoft-supp
             SendData(ID_MC_BC, MODE_REPORT, 0xA903, (UBYTE *)"ACK");
             break;
         }
+        case 0x5002: //启动校准
+        {
+            AngleTest = TRUE;
+            MC_MotorSPD_rpm_Percent = 10;
+            cp_stFlg.RunModelSelect = IFContorl;
+            SendData(ID_MC_BC, MODE_REPORT, 0xA903, (UBYTE *)"ACK");
+            break;
+        }
+        case 0x5104: //位置传感器零点校准
+        {
+            memcpy(&MC_UpcInfo.stMContorlInfo, Data, DataLength);
+            
+            if (MC_UpcInfo.stMContorlInfo.uwSaveFlg == 1)
+            {
+                cp_stFlg.ParaSaveEEFlg = TRUE;
+            }
+            cp_stFlg.ParaUpdateFlg = TRUE;
+            cp_stFlg.ParaMCInfoUpdateFlg = TRUE;
+            cp_stFlg.ParaMotorDriveUpdateFinishFlg = TRUE;
+            SendData(ID_MC_BC, MODE_REPORT, 0xA903, (UBYTE *)"ACK");
+            break;
+        }
         default:
             break;
         }

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

@@ -61,6 +61,7 @@ UBYTE                    MC_BC_COM = 0;
 static LPF_OUT                    BMS_swCurIdcLpf;
 static BOOL blBMSCommFault = FALSE;
 static UWORD uwRemainDistanceCal;
+BOOL AngleTest = FALSE;
 
 void Can_voUpdateMC_UpcInfo(void)
 {
@@ -238,7 +239,7 @@ void Can_voInitMC_Run(void)
     memcpy((uint8_t*)(MC_VerInfo.HW_Version + 12), (uint8_t*)(&MCU_ID_CRC32), 4);
     
     // Software version
-    char chFwVersion[16]="V1r0r3_";
+    char chFwVersion[16]="V1r0r4_";
     strncat(chFwVersion,COMMIT_TIME,9);
     strncpy(MC_VerInfo.FW_Version, (char *)chFwVersion, 16);
     

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

@@ -184,7 +184,7 @@ void  spi_voResolver(const SPI_RESOLVER_COEF *coef, SPI_RESOLVER_OUT *out)
         uwSpiThetaTmpPu2 = uwSpiThetaTmpPu;
         
         swThetaCompPu = (SWORD)(((SLONG)out->swSpdLpfTmpPu * TLatency_TM) >> 10); // Q15, Consider decoding and SPI Latency:10us
-        ulTmp1 = uwSpiThetaTmpPu + swThetaCompPu + out->swSpiThetaOffsetPu + cof_sl720DegreePu;
+        ulTmp1 = uwSpiThetaTmpPu + swThetaCompPu + out->swSpiThetaOffsetOrignPu + cof_sl720DegreePu;//swSpiThetaOffsetPu
         uwSpiThetaTmpPu = (UWORD)(ulTmp1 & 0x7FFF);
 
         ulTmp2 = uwSpiThetaTmpPu2 + cof_sl720DegreePu;

User project/4.BasicHardwSoftwLayer/2.BasicSoftwLayer/Include/user.h

@@ -58,6 +58,9 @@ Update Time
 #if (MOTOR_ID_SEL == MOTOR_WELLING_MTB_36V || MOTOR_ID_SEL == MOTOR_WELLING_CITY_36V)
 #define ADC_IPHASE_CUR_MAX_AP 15600
 #define ADC_IPHASE_CUR_OVER_K 780 // SWORD最大电流/采样最大电流=120A/156A=0.769=787(Q10)
+#elif (MOTOR_ID_SEL == MOTOR_WELLING_MTB_48V)
+#define ADC_IPHASE_CUR_MAX_AP 16500
+#define ADC_IPHASE_CUR_OVER_K 1024
 #else
 #define ADC_IPHASE_CUR_MAX_AP 10355
 #define ADC_IPHASE_CUR_OVER_K 1024
@@ -97,10 +100,10 @@ Update Time
 
 #elif (MOTOR_ID_SEL == MOTOR_WELLING_MTB_48V)
 #define M_POLE_PAIRS        7       // unit:Null,Pole pairs
-#define M_RS_OHM            170     // unit: 0.1mOhm, Phase resistance
+#define M_RS_OHM            345     // unit: 0.1mOhm, Phase resistance
 #define M_MATERIAL          Al      // unit:Null
-#define M_LD_NOLOAD_MH      11000   // unit: 0.01uH, D axis inductance
-#define M_LQ_NOLOAD_MH      11500   // unit: 0.01uH, Q axis inductance
+#define M_LD_NOLOAD_MH      22250   // unit: 0.01uH, D axis inductance
+#define M_LQ_NOLOAD_MH      23000   // unit: 0.01uH, Q axis inductance
 #define M_LD_TURN1_ID_AP    5000    // unit: 0.01A,
 #define M_LD_TURN1_LD_MH    11000   // unit: 0.01uH,D axis inductance
 #define M_LD_TURN2_ID_AP    700     // unit: 0.01A,
@@ -113,10 +116,10 @@ Update Time
 #define M_LQ_MIN_MH         11500   // unit: 0.01uH,Q axis inductance
 #define M_JD                2500    // unit: 10-7Kg*m2, Rotational inertia
 #define M_MAGNETIC_MATERIAL Ferrite // unit:Null
-#define M_FLUX_WB           5360    // unit: 0.001mWb, Flux linkage
+#define M_FLUX_WB           8920    // unit: 0.001mWb, Flux linkage
 #define M_ID_MAX_AP         0       //-25      // unit: 0.01A, Max d axis current
-#define M_ID_MIN_AP         -300    // unit: 0.01A, Min d axis current
-#define M_IS_PEAK_MAX_AP    5000    // unit: 0.01A, Max phase current(below base speed)
+#define M_ID_MIN_AP         -600    // unit: 0.01A, Min d axis current
+#define M_IS_PEAK_MAX_AP    3800    // unit: 0.01A, Max phase current(below base speed)
 #define M_POWER_MAX_WT      600     // unit: W, Max power
 #define M_R_SPD_RPM         5000    // unit: r/min, rate spd
 #define M_R_PWR_WT          250     // unit: W, rate power
@@ -316,7 +319,7 @@ Update Time
 
 /* Drag speed & current */
 #define DRAG_CUR_AP         1000 // 1500 //100 Huawei      // unit: 0.01A, Drag current value
-#define DRAG_VOL_AP         18   // 0.1v
+#define DRAG_VOL_AP         10   // 0.1v
 #define DRAG_SPD_HZ         20   // unit: Hz, Final speed of drag
 #define DRAG_SPD_RAMP_TM_MS 4000 // unit: ms, Time of speed from 0Hz to target Hz
 /* Open to close */