1. 没有start的时候，需要通过pwm timer的update中断处理速度

2. 重写epm方法，放到motor.c中

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

Applications/bsp/pwm.c

@@ -167,9 +167,7 @@ static void _init_pwm_timer(void) {
 
 	timer_interrupt_disable(timer, TIMER_INT_UP);
 	timer_interrupt_flag_clear(timer, TIMER_INT_FLAG_UP);
-#ifdef ENABLE_PWM_UP_IRQ	
 	nvic_irq_enable(TIMER0_UP_IRQn, TIMER_UP_IRQ_PRIORITY, 0);
-#endif
     timer_enable(timer);
 
 #ifdef GD32_FOC_DEMO
@@ -211,11 +209,6 @@ void pwm_start(void){
 	timer_flag_clear(pwm_timer, TIMER_FLAG_UP);
 
 	timer_primary_output_config(pwm_timer,ENABLE);
-
-	timer_flag_clear(pwm_timer, TIMER_FLAG_UP);
-#ifdef ENABLE_PWM_UP_IRQ
-	timer_interrupt_enable(pwm_timer, TIMER_INT_UP);
-#endif
 }
 
 void pwm_stop(void){

Applications/bsp/pwm.h

@@ -72,6 +72,17 @@
 		} \
 	}while(0)
 
+#define pwm_up_enable(n) \
+	do { \
+		if (n) { \
+			timer_flag_clear(pwm_timer, TIMER_FLAG_UP); \
+			timer_interrupt_enable(pwm_timer, TIMER_INT_UP); \
+		}else { \
+			timer_flag_clear(pwm_timer, TIMER_FLAG_UP); \
+			timer_interrupt_disable(pwm_timer, TIMER_INT_UP); \
+		} \
+	}while(0)
+
 void pwm_3phase_init(void);
 void pwm_start(void);
 void pwm_stop(void);

Applications/foc/core/PMSM_FOC_Core.c

@@ -431,9 +431,14 @@ void PMSM_FOC_idqCalc(void) {
 		}
 	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
 		float refTorque = eCtrl_get_FinalTorque();
-		gFoc_Ctrl.pi_torque->max = refTorque;
-		gFoc_Ctrl.pi_torque->min = -gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
-		if ((eCtrl_get_FinalTorque() <= 0.0001f) && (gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
+		if (refTorque > 0) {
+			gFoc_Ctrl.pi_torque->max = refTorque;
+			gFoc_Ctrl.pi_torque->min = -gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
+		}else {
+			gFoc_Ctrl.pi_torque->min = refTorque;
+			gFoc_Ctrl.pi_torque->max = gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
+		}
+		if ((eCtrl_get_FinalTorque() == 0) && (gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
 			gFoc_Ctrl.pi_torque->max = 0;
 			gFoc_Ctrl.pi_torque->min = 0; //防止倒转
 		}
@@ -441,8 +446,13 @@ void PMSM_FOC_idqCalc(void) {
 		float maxTrq = PI_Controller_RunSat(gFoc_Ctrl.pi_torque, errRef);
 		gFoc_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_Power(maxTrq);
 	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD){
-		gFoc_Ctrl.pi_speed->max = eRamp_get_intepolation(&gFoc_Ctrl.userLim.phaseCurrLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
-		gFoc_Ctrl.pi_speed->min = 0;//-gFoc_Ctrl.userLim.s_PhaseCurrLim;
+		if (eCtrl_get_FinalSpeed() > 0) {
+			gFoc_Ctrl.pi_speed->max = eRamp_get_intepolation(&gFoc_Ctrl.userLim.phaseCurrLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
+			gFoc_Ctrl.pi_speed->min = -gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
+		}else if (eCtrl_get_FinalSpeed() < 0) {
+			gFoc_Ctrl.pi_speed->min = -eRamp_get_intepolation(&gFoc_Ctrl.userLim.phaseCurrLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
+			gFoc_Ctrl.pi_speed->max = gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
+		}
 		float refSpeed = eCtrl_get_RefSpeed();
 		if (gFoc_Ctrl.in.b_cruiseEna) {
 			refSpeed = gFoc_Ctrl.in.s_cruiseRPM;
@@ -642,6 +652,10 @@ bool PMSM_FOC_Set_Speed(float rpm) {
 }
 
 bool PMSM_FOC_Set_epmMode(bool epm) {
+	if (epm && !gFoc_Ctrl.in.b_motEnable) {
+		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		return false;
+	}
 	if (gFoc_Ctrl.in.b_epmMode != epm) {
 		if (PMSM_FOC_GetSpeed() != 0.0f) {
 			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);

Applications/foc/core/torque.c

@@ -43,14 +43,15 @@ float torque_target_from_throttle(float f_throttle) {
 void torque_speed_target(u8 run_mode, float f_throttle) {
 	if (run_mode == CTRL_MODE_SPD) {
 		float speed_Ref = speed_target_from_throttle(f_throttle);
-		if (PMSM_FOC_is_epmMode()) {
+		if (mc_is_epm()) {
 			if (speed_Ref == 0.0f) {
-				PMSM_FOC_Start_epmMove(false, EPM_Dir_None);
+				mc_start_epm_move(EPM_Dir_None);
 			}else {
-				PMSM_FOC_Start_epmMove(true, EPM_Dir_Forward);
+				mc_start_epm_move(EPM_Dir_Forward);
 			}
+		}else {
+			PMSM_FOC_Set_Speed(speed_Ref);
 		}
-		PMSM_FOC_Set_Speed(speed_Ref);
 	}else if (run_mode == CTRL_MODE_TRQ) {
 		if (mc_throttle_released()) {
 			eCtrl_enable_eBrake(true);

Applications/foc/motor/motor.c

@@ -91,6 +91,7 @@ void mc_init(void) {
 	MC_Check_MosVbusThrottle();
 	sched_timer_enable(CONFIG_SPD_CTRL_US);
 	shark_task_create(_self_check_task, NULL);
+	pwm_up_enable(true);
 }
 
 motor_t * mc_params(void) {
@@ -120,6 +121,7 @@ bool mc_start(u8 mode) {
 		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
 		return false;
 	}
+	pwm_up_enable(false);
 	motor.mode = mode;
 	eCtrl_init(1000, 3000);
 	motor_encoder_start(motor.s_direction);
@@ -164,7 +166,10 @@ bool mc_stop(void) {
 	adc_stop_convert();
 	pwm_stop();
 	PMSM_FOC_Stop();
+	pwm_up_enable(true);
 	motor.b_start = false;
+	motor.b_epm = false;
+	motor.epm_dir = EPM_Dir_None;
 	gpio_led2_enable(false);
 	return true;
 }
@@ -190,6 +195,71 @@ bool mc_set_foc_mode(u8 mode) {
 	return ret;
 }
 
+bool mc_start_epm(bool epm) {
+	if (motor.b_epm == epm) {
+		return true;
+	}
+	if (!motor.b_start) {
+		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		return false;
+	}
+	if (PMSM_FOC_GetSpeed() != 0.0f) {
+		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
+		return false;
+	}
+	if (!mc_throttle_released()) {
+		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
+		return false;
+	}
+	u32 mask = cpu_enter_critical();
+	motor.b_epm = epm;
+	if (epm) {
+		motor.lim_rpm = PMSM_FOC_GetSpeedLimit();
+		eCtrl_set_TgtSpeed(0);
+		motor.mode = CTRL_MODE_SPD;
+		PMSM_FOC_SpeedLimit(nv_get_foc_params()->s_maxEpmRPM);
+		PMSM_FOC_SetCtrlMode(CTRL_MODE_SPD);
+	}else {
+		motor.epm_dir = EPM_Dir_None;
+		motor.mode = CTRL_MODE_TRQ;
+		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
+		PMSM_FOC_SpeedLimit(motor.lim_rpm);
+	}
+	cpu_exit_critical(mask);
+	return false;
+}
+
+bool mc_is_epm(void) {
+	return motor.b_epm;
+}
+
+bool mc_start_epm_move(EPM_Dir_t dir) {
+	if (!motor.b_epm || !motor.b_start) {
+		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		return false;
+	}
+	if (dir == motor.epm_dir) {
+		return true;
+	}
+	motor.epm_dir = dir;
+	if (dir != EPM_Dir_None) {
+		u32 mask = cpu_enter_critical();
+		if (!PMSM_FOC_Is_Start()) {
+			PMSM_FOC_Start(motor.mode);
+			pwm_enable_channel();
+		}
+		cpu_exit_critical(mask);
+		float rpm = nv_get_foc_params()->s_maxEpmRPM;
+		if (dir == EPM_Dir_Back) {
+			rpm = -rpm;
+		}
+		PMSM_FOC_Set_Speed(rpm);
+	}else {
+		PMSM_FOC_Set_Speed(0);
+	}
+	return true;
+}
+
 void mc_set_spd_torque(s32 target) {
 	motor.b_ignor_throttle = true;
 	motor.s_targetFix = target;
@@ -376,8 +446,7 @@ void MC_Protect_IRQHandler(void){
 
 measure_time_t g_meas_timeup = {.intval_max_time = 62, .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
 void TIMER_UP_IRQHandler(void){
-	//phase_current_adc_triger();
-	time_measure_start(&g_meas_timeup);
+	motor_encoder_update();
 }
 
 measure_time_t g_meas_foc = {.exec_max_time = 20, .intval_max_time = 62,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
@@ -399,6 +468,15 @@ void ADC_IRQHandler(void) {
 	TIME_MEATURE_END();
 }
 
+static bool mc_can_stop_foc(void) {
+	if (mc_throttle_released() && PMSM_FOC_GetSpeed() == 0.0f) {
+		if (!PMSM_FOC_MotorLocking() && motor.epm_dir == EPM_Dir_None) {
+			return true;
+		}
+	}
+	return false;
+}
+
 //#define ANGLE_TEST
 #ifdef ANGLE_TEST
 static void _debug_angle(void) {
@@ -447,16 +525,20 @@ void Sched_MC_mTask(void) {
 	if ((runMode != CTRL_MODE_OPEN) || (motor.mode != CTRL_MODE_OPEN)) {
 		if (motor.mode != CTRL_MODE_OPEN) {
 			u32 mask;
-			if (mc_throttle_released() && PMSM_FOC_GetSpeed() == 0.0f) {
-				mask = cpu_enter_critical();
-				PMSM_FOC_Stop();
-				pwm_disable_channel();
-				cpu_exit_critical(mask);
+			if (mc_can_stop_foc()) {
+				if (PMSM_FOC_Is_Start()) {
+					mask = cpu_enter_critical();
+					PMSM_FOC_Stop();
+					pwm_disable_channel();
+					cpu_exit_critical(mask);
+				}
 			}else {
-				mask = cpu_enter_critical();
-				PMSM_FOC_Start(motor.mode);
-				pwm_enable_channel();
-				cpu_exit_critical(mask);
+				if (!PMSM_FOC_Is_Start()) {
+					mask = cpu_enter_critical();
+					PMSM_FOC_Start(motor.mode);
+					pwm_enable_channel();
+					cpu_exit_critical(mask);
+				}
 			}
 		}
 		if (runMode != CTRL_MODE_OPEN) {

Applications/foc/motor/motor.h

@@ -11,6 +11,9 @@ typedef struct {
 	bool   b_ignor_throttle;
 	bool   b_calibrate;
 	bool   b_runStall; //是否堵转
+	bool   b_epm;
+	EPM_Dir_t epm_dir;
+	float  lim_rpm;
 	u32    runStall_time;
 	s16    s_testAngle;
 	s32    s_targetFix;
@@ -30,6 +33,9 @@ void mc_use_throttle(void);
 bool mc_current_sensor_calibrate(float current);
 bool mc_encoder_zero_calibrate(s16 vd);
 bool mc_set_foc_mode(u8 mode);
+bool mc_is_epm(void);
+bool mc_start_epm(bool epm);
+bool mc_start_epm_move(EPM_Dir_t dir);
 
 static __INLINE float motor_encoder_get_angle(void) {
 #ifdef USE_ENCODER_HALL
@@ -41,6 +47,16 @@ static __INLINE float motor_encoder_get_angle(void) {
 #endif
 }
 
+static __INLINE void motor_encoder_update(void) {
+#ifdef USE_ENCODER_HALL
+	hall_sensor_get_theta();
+#elif defined (USE_ENCODER_ABI)
+	encoder_get_theta();
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
 static __INLINE float motor_encoder_get_speed(void) {
 #ifdef USE_ENCODER_HALL
 	return hall_sensor_get_speed();