1. 更新通过pwm和三相电流计算母线电流的方式

2. 超前角标定，使用s_dqAngle
3. 油门开度不变的情况下，改变挡位不会立即生效
4. 定速巡航加减速的ramp给定3s
5. 强制V3版本，V3的board id 贴片不对,后面需要修改回来

Signed-off-by: huhui <huhui@sharkgulf.com>

Applications/app/app.c

@@ -146,7 +146,7 @@ static u32 _app_report_task(void *p) {
 	}
 	return 200;
 }
-int plot_type = 0;
+int plot_type = 9;
 static void plot_smo_angle(void) {
 	float smo_angle = foc_observer_sensorless_angle();
 	float delta = smo_angle - PMSM_FOC_Get()->in.s_motAngle;
@@ -176,6 +176,8 @@ static u32 _app_plot_task(void * args) {
 		#ifdef CONFIG_DQ_STEP_RESPONSE
 		can_plot2((s16)(target_q*10.0f), (s16)(PMSM_FOC_Get()->out.s_RealIdq.q * 10.0f));
 		#endif
+	}else if (plot_type == 9) {
+		can_plot3((s16)PMSM_FOC_Get()->out.s_FilteriDC, (s16)PMSM_FOC_Get()->out.s_CalciDC, (s16)PMSM_FOC_Get()->out.s_CalciDC2);
 	}
 	
 	return 20;

Applications/bsp/gd32/board_mc105_v3.h

@@ -48,7 +48,7 @@
 #define SCHED_TIMER_IRQHandler TIMER5_IRQHandler
 
 #define PWM_DEAD_TIME_NS 400u
-#define HW_DEAD_TIME_NS  200u
+#define HW_DEAD_TIME_NS  300u
 #define HW_RISE_TIME_NS  500u
 #define HW_NOISE_TIME_NS 300u
 

Applications/bsp/gd32/gpio.c

@@ -151,8 +151,8 @@ void gpio_board_id_init(void) {
 }
 
 u8  gpio_hw_board_id(void) {
-	u8 id = BOARD_105_VERSION_4;
-#ifdef BOOT_PIN_0_GROUP
+	u8 id = BOARD_105_VERSION_3;
+#if 0//def BOOT_PIN_0_GROUP
 	u8 b0 = gpio_input_bit_get(BOOT_PIN_0_GROUP, BOOT_PIN_0_PIN);
 	u8 b1 = gpio_input_bit_get(BOOT_PIN_1_GROUP, BOOT_PIN_1_PIN);
 	id = ((b1 << 1) | b0);

Applications/foc/commands.c

@@ -420,7 +420,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 				s16 is_angle = decode_s16((u8 *)command->data + 2);
 				sys_debug("curr %d, angle %d\n", is_curr, is_angle);
 				PMSM_FOC_Set_Current(is_curr);
-				PMSM_FOC_Set_Angle(is_angle);
+				PMSM_FOC_Set_Dq_Angle(is_angle);
 			}
 			break;
 		}

Applications/foc/core/PMSM_FOC_Core.c

@@ -184,9 +184,9 @@ void PMSM_FOC_RT_LimInit(void) {
 	gFoc_Ctrl.protLim.s_iDCLim = HW_LIMIT_NONE;
 	gFoc_Ctrl.protLim.s_TorqueLim = HW_LIMIT_NONE;
 
-	eRamp_init_target(&gFoc_Ctrl.rtLim.rpmLimRamp, gFoc_Ctrl.userLim.s_motRPMLim, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
-	eRamp_init_target(&gFoc_Ctrl.rtLim.torqueLimRamp, gFoc_Ctrl.userLim.s_torqueLim, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
-	eRamp_init_target(&gFoc_Ctrl.rtLim.DCCurrLimRamp, gFoc_Ctrl.userLim.s_iDCLim, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
+	eRamp_init_target2(&gFoc_Ctrl.rtLim.rpmLimRamp, gFoc_Ctrl.userLim.s_motRPMLim, CONFIG_LIMIT_RAMP_TIME);
+	eRamp_init_target2(&gFoc_Ctrl.rtLim.torqueLimRamp, gFoc_Ctrl.userLim.s_torqueLim, CONFIG_LIMIT_RAMP_TIME);
+	eRamp_init_target2(&gFoc_Ctrl.rtLim.DCCurrLimRamp, gFoc_Ctrl.userLim.s_iDCLim, CONFIG_LIMIT_RAMP_TIME);
 }
 
 void PMSM_FOC_CoreInit(void) {
@@ -222,14 +222,15 @@ void PMSM_FOC_CoreInit(void) {
 	gFoc_Ctrl.params.ld = nv_get_motor_params()->ld;
 	gFoc_Ctrl.params.flux = nv_get_motor_params()->flux_linkage;
 	gFoc_Ctrl.in.s_manualAngle = INVALID_ANGLE;
+	gFoc_Ctrl.in.s_dqAngle     = INVALID_ANGLE;
 	gFoc_Ctrl.in.b_fwEnable = nv_get_foc_params()->n_FwEnable;
 	gFoc_Ctrl.in.s_vDC = nv_get_foc_params()->s_maxDCVol;//(CONFIG_RATED_DC_VOL);
-//	gFoc_Ctrl.params.f_DCLim = get_vbus_float();
-	eRamp_init_target(&gFoc_Ctrl.in.cruiseRpmRamp, 0, CONFIG_ACC_TIME, CONFIG_DEC_TIME);
 	
 	gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
 	gFoc_Ctrl.out.f_vdqRation = 0;
 
+	eRamp_init_target2(&gFoc_Ctrl.in.cruiseRpmRamp, 0, CONFIG_CRUISE_RAMP_TIME);
+
 	FOC_DqRamp_init(&gFoc_Ctrl.idq_ctl[0], 1);
 	FOC_DqRamp_init(&gFoc_Ctrl.idq_ctl[1], 1);
 
@@ -244,7 +245,7 @@ void PMSM_FOC_CoreInit(void) {
 
 /* 通过三相电流重构母线电流，和单电阻采样正好相反，原理一致 */
 static __INLINE void PMSM_FOC_Calc_iDC_Fast(void) {
-	float deadtime = (float)(NS_2_TCLK(PWM_DEAD_TIME_NS))/(float)FOC_PWM_Half_Period;
+	float deadtime = (float)(NS_2_TCLK(PWM_DEAD_TIME_NS + HW_DEAD_TIME_NS))/(float)FOC_PWM_Half_Period;
 	float duty_pu[3];
 	duty_pu[0] = (float)gFoc_Ctrl.out.n_Duty[0] / (float)FOC_PWM_Half_Period;
 	duty_pu[1] = (float)gFoc_Ctrl.out.n_Duty[1] / (float)FOC_PWM_Half_Period;
@@ -254,18 +255,84 @@ static __INLINE void PMSM_FOC_Calc_iDC_Fast(void) {
 	float iDC;
 	if ((duty_pu[0] >= duty_pu[1]) && (duty_pu[1] >= duty_pu[2])) {
 		iDC = iABC[0] * MAX(duty_pu[0] - duty_pu[1] - deadtime, 0) + (iABC[0] + iABC[1]) * MAX(duty_pu[1] - duty_pu[2] - deadtime, 0);
+		if (iABC[0] < 0) {
+			iDC = iDC + iABC[0] * deadtime;
+		}
+		if (iABC[1] > 0) {
+			iDC = iDC + iABC[0] * deadtime;
+		}else {
+			iDC = iDC + (iABC[0] + iABC[1]) * deadtime;
+		}
+		if (iABC[2] > 0) {
+			iDC = iDC + (iABC[0] + iABC[1]) * deadtime;
+		}
 	}else if ((duty_pu[0] >= duty_pu[2]) && (duty_pu[2] >= duty_pu[1])) {
 		iDC = iABC[0] * MAX(duty_pu[0] - duty_pu[2] - deadtime, 0) + (iABC[0] + iABC[2]) * MAX(duty_pu[2] - duty_pu[1] - deadtime, 0);
+		if (iABC[0] < 0) {
+			iDC = iDC + iABC[0] * deadtime;
+		}
+		if (iABC[2] > 0) {
+			iDC = iDC + iABC[0] * deadtime;
+		}else {
+			iDC = iDC + (iABC[0] + iABC[2]) * deadtime;
+		}
+		if (iABC[1] > 0) {
+			iDC = iDC + (iABC[0] + iABC[2]) * deadtime;
+		}
 	}else if ((duty_pu[1] >= duty_pu[0]) && (duty_pu[0] >= duty_pu[2])) {
 		iDC = iABC[1] * MAX(duty_pu[1] - duty_pu[0] - deadtime, 0) + (iABC[1] + iABC[0]) * MAX(duty_pu[0] - duty_pu[2] - deadtime, 0);
+		if (iABC[1] < 0) {
+			iDC = iDC + iABC[1] * deadtime;
+		}
+		if (iABC[0] > 0) {
+			iDC = iDC + iABC[1] * deadtime;
+		}else {
+			iDC = iDC + (iABC[1] + iABC[0]) * deadtime;
+		}
+		if (iABC[2] > 0) {
+			iDC = iDC + (iABC[1] + iABC[0]) * deadtime;
+		}
 	}else if ((duty_pu[1] >= duty_pu[2]) && (duty_pu[2] >= duty_pu[0])) {
 		iDC = iABC[1] * MAX(duty_pu[1] - duty_pu[2] - deadtime, 0) + (iABC[1] + iABC[2]) * MAX(duty_pu[2] - duty_pu[0] - deadtime, 0);
+		if (iABC[1] < 0) {
+			iDC = iDC + iABC[1] * deadtime;
+		}
+		if (iABC[2] > 0) {
+			iDC = iDC + iABC[1] * deadtime;
+		}else {
+			iDC = iDC + (iABC[1] + iABC[2]) * deadtime;
+		}
+		if (iABC[0] > 0) {
+			iDC = iDC + (iABC[1] + iABC[2]) * deadtime;
+		}
 	}else if ((duty_pu[2] >= duty_pu[0]) && (duty_pu[0] >= duty_pu[1])) {
 		iDC = iABC[2] * MAX(duty_pu[2] - duty_pu[0] - deadtime, 0) + (iABC[2] + iABC[0]) * MAX(duty_pu[0] - duty_pu[1] - deadtime, 0);
+		if (iABC[2] < 0) {
+			iDC = iDC + iABC[2] * deadtime;
+		}
+		if (iABC[0] > 0) {
+			iDC = iDC + iABC[2] * deadtime;
+		}else {
+			iDC = iDC + (iABC[2] + iABC[0]) * deadtime;
+		}
+		if (iABC[1] > 0) {
+			iDC = iDC + (iABC[2] + iABC[0]) * deadtime;
+		}
 	}else { // duty_pu[2] >= duty_pu[1] && duty_pu[1] >= duty_pu[0]
 		iDC = iABC[2] * MAX(duty_pu[2] - duty_pu[1] - deadtime, 0) + (iABC[2] + iABC[1]) * MAX(duty_pu[1] - duty_pu[0] - deadtime, 0);
+		if (iABC[2] < 0) {
+			iDC = iDC + iABC[2] * deadtime;
+		}
+		if (iABC[1] > 0) {
+			iDC = iDC + iABC[2] * deadtime;
+		}else {
+			iDC = iDC + (iABC[2] + iABC[1]) * deadtime;
+		}
+		if (iABC[0] > 0) {
+			iDC = iDC + (iABC[2] + iABC[1]) * deadtime;
+		}
 	}
-	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC2, iDC, 0.01f);
+	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC2, iDC, 0.005f);
 }
 
 //#define UPDATE_Lq_By_iq   /* Q轴电感 通过Iq电流补偿 */
@@ -553,9 +620,9 @@ static __INLINE float PMSM_FOC_Limit_Speed(float maxTrq) {
 
 static __INLINE void PMSM_FOC_idq_Assign(void) {
 	if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) {
-		if (gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
+		if (gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_dqAngle != INVALID_ANGLE)) {
 			float s, c;
-			normal_sincosf(degree_2_pi(gFoc_Ctrl.in.s_manualAngle + 90.0f), &s, &c);
+			normal_sincosf(degree_2_pi(gFoc_Ctrl.in.s_dqAngle + 90.0f), &s, &c);
 			gFoc_Ctrl.in.s_targetIdq.d = gFoc_Ctrl.in.s_targetCurrent * c;
 			
 			if (gFoc_Ctrl.in.s_targetIdq.d > gFoc_Ctrl.hwLim.s_FWDCurrMax) {
@@ -819,7 +886,7 @@ void PMSM_FOC_RT_PhaseCurrLim(float lim) {
 	if (lim > gFoc_Ctrl.hwLim.s_PhaseCurrMax) {
 		lim = gFoc_Ctrl.hwLim.s_PhaseCurrMax;
 	}
-	eRamp_init_target(&gFoc_Ctrl.rtLim.torqueLimRamp, lim, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
+	eRamp_init_target2(&gFoc_Ctrl.rtLim.torqueLimRamp, lim, CONFIG_LIMIT_RAMP_TIME);
 }
 
 float PMSM_FOC_GetPhaseCurrLim(void) {
@@ -839,7 +906,7 @@ bool PMSM_FOC_EnableCruise(bool enable) {
 			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 			return false;
 		}
-		eRamp_init_target(&gFoc_Ctrl.in.cruiseRpmRamp, motSpd, CONFIG_ACC_TIME, CONFIG_DEC_TIME);
+		eRamp_init_target2(&gFoc_Ctrl.in.cruiseRpmRamp, motSpd, CONFIG_CRUISE_RAMP_TIME);
 		gFoc_Ctrl.in.s_cruiseRPM = motSpd;
 		gFoc_Ctrl.in.b_cruiseEna = enable;
 	}
@@ -854,7 +921,7 @@ bool PMSM_FOC_PauseCruise(void) {
 
 bool PMSM_FOC_ResumeCruise(void) {
 	gFoc_Ctrl.in.b_cruiseEna = true;
-	eRamp_init_target(&gFoc_Ctrl.in.cruiseRpmRamp, PMSM_FOC_GetSpeed(), CONFIG_ACC_TIME, CONFIG_DEC_TIME);
+	eRamp_init_target2(&gFoc_Ctrl.in.cruiseRpmRamp, PMSM_FOC_GetSpeed(), CONFIG_CRUISE_RAMP_TIME);
 	eRamp_set_step_target(&gFoc_Ctrl.in.cruiseRpmRamp, gFoc_Ctrl.in.s_cruiseRPM, CONFIG_eCTRL_STEP_TS);
 	return true;
 }
@@ -914,17 +981,21 @@ bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
 void PMSM_FOC_MTPA_Calibrate(bool enable) {
 	if (enable) {
 		gFoc_Ctrl.in.b_MTPA_calibrate = true;
-		gFoc_Ctrl.in.s_manualAngle = 0;
+		gFoc_Ctrl.in.s_dqAngle = 0;
 	}else {
-		gFoc_Ctrl.in.s_manualAngle = INVALID_ANGLE;
+		gFoc_Ctrl.in.s_dqAngle = INVALID_ANGLE;
 		gFoc_Ctrl.in.b_MTPA_calibrate = false;
 	}
 }
 
-void PMSM_FOC_Set_Angle(float angle) {
+void PMSM_FOC_Set_MotAngle(float angle) {
 	gFoc_Ctrl.in.s_manualAngle = (angle);
 }
 
+void PMSM_FOC_Set_Dq_Angle(float angle) {
+	gFoc_Ctrl.in.s_dqAngle = (angle);
+}
+
 void PMSM_FOC_Get_TgtIDQ(DQ_t * dq) {
 	dq->d = gFoc_Ctrl.in.s_targetIdq.d;
 	dq->q = gFoc_Ctrl.in.s_targetIdq.q;
@@ -940,7 +1011,6 @@ void PMSM_FOC_AutoHold(bool lock) {
 		motor_encoder_lock_pos(lock);
 		PI_Controller_Reset(&gFoc_Ctrl.pi_lock, 0);
 		if (!lock) {
-			//解锁后为了防止倒溜，需要把当前
 			if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
 #ifdef CONFIG_SPEED_LADRC
 				ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, 0, gFoc_Ctrl.in.s_targetTorque * 1.1f);
@@ -1040,7 +1110,7 @@ void PMSM_FOC_Calc_Current(void) {
 	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC, raw_idc, 0.1f);
 
 	raw_idc = get_vbus_current();
-	LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.1f);
+	LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.05f);
 
 	gFoc_Ctrl.out.s_RealCurrentFiltered = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
 

Applications/foc/core/PMSM_FOC_Core.h

@@ -106,7 +106,8 @@ typedef struct {
 	float   s_motRPMFilted; //电机滤波后的转速
 	float   s_motVelDegreePers; //电机的电角速度
 	volatile bool    b_MTPA_calibrate;
-	float   s_manualAngle; //mainly used when calibrate hall/mtpa. 
+	float   s_manualAngle; //mainly used when calibrate hall/mtpa.
+	float   s_dqAngle; //D轴电流超前角
 }FOC_InP;
 
 typedef struct {
@@ -270,7 +271,8 @@ void PMSM_FOC_Brake(bool brake);
 void PMSM_FOC_Calc_Current(void);
 void PMSM_FOC_AutoHold(bool lock);
 void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kd);
-void PMSM_FOC_Set_Angle(float angle);
+void PMSM_FOC_Set_MotAngle(float angle);
+void PMSM_FOC_Set_Dq_Angle(float angle);
 bool PMSM_FOC_Is_Start(void);
 void PMSM_FOC_SetErrCode(u8 error);
 u8 PMSM_FOC_GetErrCode(void);

Applications/foc/core/e_ctrl.h

@@ -60,6 +60,11 @@ static void eRamp_init_target(e_Ramp *r, float target, u32 acc, u32 dec) {
 	r->dect = (float)dec;
 }
 
+static void eRamp_init_target2(e_Ramp *r, float target, u32 time) {
+	eRamp_init_target(r, target, time, time);
+} 
+
+
 static void eRamp_reset_target(e_Ramp *r, float target) {
 	r->start = target;
 	r->target = target;

Applications/foc/core/thro_torque.c

@@ -15,6 +15,7 @@ void thro_torque_reset(void) {
 	_torque.thro_filted = 0.0f;
 	_torque.thro_ration = _torque.thro_ration_last = 0.0f;
 	_torque.torque_req = _torque.torque_real = 0.0f;
+	_torque.gear = mc_get_gear();
 }
 
 void thro_torque_init(void) {
@@ -26,8 +27,8 @@ static __INLINE float gear_rpm_torque(u8 trq, s16 max) {
 	return (float)trq/100.0f * max;
 }
 
-float thro_torque_gear_map(s16 rpm) {
-	mc_gear_t *_current_gear = mc_get_gear_config();
+float thro_torque_gear_map(s16 rpm, u8 gear) {
+	mc_gear_t *_current_gear = mc_get_gear_config_by_gear(gear);
 	if (_current_gear == NULL) {
 		return 0;
 	}
@@ -73,18 +74,25 @@ float thro_ration_to_voltage(float r) {
 	return fclamp(vol, 0, nv_get_foc_params()->n_endThroVol);
 }
 
-static float thro_torque_for_accelerate(void) {
-	float max_torque = thro_torque_gear_map((s16)_torque.spd_filted);
+static float _thro_torque_for_accelerate(float ration) {
+	float max_torque = thro_torque_gear_map((s16)_torque.spd_filted, _torque.gear);
 	float acc_r = 1.0f;
 	if (_torque.thro_ration_last < 1.0f) {
-		acc_r = (_torque.thro_ration - _torque.thro_ration_last)/ (1.0f - _torque.thro_ration_last);
+		acc_r = (ration - _torque.thro_ration_last)/ (1.0f - _torque.thro_ration_last);
 	}
 	acc_r = fclamp(acc_r, 0, 1.0f);
 	float acc_torque = _torque.torque_real + acc_r * (max_torque - _torque.torque_real);
-
+	if (acc_torque < 0) {
+		acc_torque = 0;
+	}
 	return acc_torque;
 }
 
+
+static float thro_torque_for_accelerate(void) {
+	return _thro_torque_for_accelerate(_torque.thro_ration);
+}
+
 static float thro_torque_for_decelerate(void) {
 	if (_torque.thro_ration_last == 0.0f) {
 		return 0;
@@ -158,19 +166,24 @@ void thro_torque_process(u8 run_mode, float f_throttle) {
 	thro_torque_filter(f_throttle);
 
 	float thro_r = thro_ration(_torque.thro_filted);
+	u8    n_gear = mc_get_gear();
+
 	if (thro_r > _torque.thro_ration) {
 		if (!_torque.accl) {
 			_torque.thro_ration_last = _torque.thro_ration;
 			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
 		}
+		_torque.gear = n_gear;
 		_torque.accl = true;
 	}else if (thro_r < _torque.thro_ration) {
 		if (_torque.accl) {
 			_torque.thro_ration_last = _torque.thro_ration;
 			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
 		}
+		_torque.gear = n_gear;
 		_torque.accl = false;
 	}
+
 	_torque.thro_ration = thro_r;
 	if (run_mode == CTRL_MODE_TRQ) {
 		thro_torque_request();

Applications/foc/core/thro_torque.h

@@ -10,6 +10,7 @@ typedef struct {
 	float spd_filted;
 	float thro_ration;
 	float thro_ration_last;
+	u8    gear;
 }thro_torque_t;
 
 void thro_torque_reset(void);
@@ -17,7 +18,7 @@ void thro_torque_init(void);
 float thro_ration_to_voltage(float r);
 void thro_torque_process(u8 run_mode, float f_throttle);
 float get_user_request_torque(void);
-float thro_torque_gear_map(s16 rpm);
+float thro_torque_gear_map(s16 rpm, u8 gear);
 
 #endif /* _THRO_TORQUE_H__ */
 

Applications/foc/foc_config.h

@@ -43,6 +43,8 @@
 #define CONFIG_EBRK_RAMP_TIME 500
 #define CONFIG_AUTOHOLD_DETECT_TIME 3000
 
+#define CONFIG_CRUISE_RAMP_TIME 5000
+
 #define CONFIG_LIMIT_RAMP_TIME (2 * 1000)
 
 #define CONFIG_MTPA_CALI_RAMP_TIME (10 * 1000)

Applications/foc/motor/motor.c

@@ -218,7 +218,7 @@ void mc_need_update(void) {
 	motor.b_updated = true;
 }
 
-mc_gear_t *mc_get_gear_config(void) {
+mc_gear_t *mc_get_gear_config_by_gear(u8 n_gear) {
 	mc_gear_t *gears;
 
 	if (motor.b_is96Mode) {
@@ -226,9 +226,15 @@ mc_gear_t *mc_get_gear_config(void) {
 	}else {
 		gears = &nv_get_gear_configs()->gears_48[0];
 	}
-	return &gears[motor.n_gear];
+	return &gears[n_gear];
+}
+
+mc_gear_t *mc_get_gear_config(void) {
+	return mc_get_gear_config_by_gear(motor.n_gear);
 }
 
+
+
 bool mc_unsafe_critical_error(void) {
 	u32 err = motor.n_CritiCalErrMask & (~(FOC_Cri_Err_Mask(FOC_CRIT_Fan1_Err)));
 	err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_Fan2_Err)));
@@ -601,7 +607,7 @@ void mc_force_run_open(s16 vd, s16 vq) {
 	adc_start_convert();
 	pwm_enable_channel();
 	phase_current_calibrate_wait();
-	PMSM_FOC_Set_Angle(0);
+	PMSM_FOC_Set_MotAngle(0);
 	PMSM_FOC_SetOpenVdq((float)vd * 1.0f, 0);
 	_force_wait = 2000;
 	motor.b_force_run = true;
@@ -620,13 +626,13 @@ void mc_encoder_off_calibrate(s16 vd) {
 	pwm_start();
 	adc_start_convert();
 	phase_current_calibrate_wait();
-	PMSM_FOC_Set_Angle(0);
+	PMSM_FOC_Set_MotAngle(0);
 	PMSM_FOC_SetOpenVdq(vd, 0);
 	delay_ms(2000);
 	motor_encoder_set_direction(POSITIVE);
 	for (int i = 0; i < 200; i++) {
 		for (float angle = 0; angle < 360; angle++) {
-			PMSM_FOC_Set_Angle(angle);
+			PMSM_FOC_Set_MotAngle(angle);
 			delay_ms(1);
 			if (i > 20) {
 				motor_encoder_offset(angle);
@@ -641,7 +647,7 @@ void mc_encoder_off_calibrate(s16 vd) {
 	delay_ms(100);
 	for (int i = 0; i < 200; i++) {
 		for (float angle = 360; angle > 0; angle--) {
-			PMSM_FOC_Set_Angle(angle);
+			PMSM_FOC_Set_MotAngle(angle);
 			delay_ms(1);
 			if (i > 10) {
 				motor_encoder_offset(angle);
@@ -716,7 +722,7 @@ bool mc_encoder_zero_calibrate(s16 vd) {
 	adc_start_convert();
 	pwm_enable_channel();
 	phase_current_calibrate_wait();
-	PMSM_FOC_Set_Angle(0);
+	PMSM_FOC_Set_MotAngle(0);
 	PMSM_FOC_SetOpenVdq(vd, 0);
 
 	shark_timer_post(&_encoder_zero_off_timer, 6*1000);
@@ -1112,7 +1118,7 @@ static bool mc_process_force_running(void) {
 			}else {
 				_force_angle += 1.5f;
 				rand_angle(_force_angle);
-				PMSM_FOC_Set_Angle(_force_angle);
+				PMSM_FOC_Set_MotAngle(_force_angle);
 			}
 		}
 		return true;

Applications/foc/motor/motor.h

@@ -98,6 +98,8 @@ bool mc_is_cruise_enabled(void);
 bool mc_set_cruise_speed(bool rpm_abs, float target_rpm);
 void mc_set_idc_limit(s16 limit);
 mc_gear_t *mc_get_gear_config(void);
+mc_gear_t *mc_get_gear_config_by_gear(u8 n_gear);
+
 int mc_get_phase_errinfo(u8 *data, int dlen);
 
 static __INLINE float motor_encoder_get_angle(void) {