速度扭矩环平滑切换

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

Applications/app/key_process.c

@@ -14,7 +14,7 @@
 static u8 key_value[3];
 static float foc_current = 0.0f;
 static u8 ctrl_mode = CTRL_MODE_OPEN;
-static float max_speed = 6000;
+static float max_speed = 2000;
 static u32 key_task(void *p) {
 	foc_cmd_body_t foc_cmd;
 	u8 cmd_data[16];
@@ -26,11 +26,12 @@ static u32 key_task(void *p) {
 			ctrl_mode = CTRL_MODE_OPEN;
 			foc_cmd.cmd = Foc_Start_Motor;
 			if (PMSM_FOC_Is_Start()) {
-				cmd_data[0] = Foc_Stop; 
+				PMSM_FOC_Set_Torque(foc_current);
 			}else {
-				cmd_data[0] = Foc_Start; 
+				cmd_data[0] = Foc_Start;
+				foc_send_command(&foc_cmd);
+				
 			}
-			foc_send_command(&foc_cmd);
 		}
 		key_value[KEY_START] = value;
 	}
@@ -38,9 +39,10 @@ static u32 key_task(void *p) {
 	value = gpio_stopkey_value();
 	if (value != key_value[KEY_STOP]) {
 		if (value) {
-			if (foc_current < 2.0f) {
-				foc_current += 0.1f;
+			if (foc_current < 5.0f) {
+				foc_current += 1.0f;
 			}
+			PMSM_FOC_SpeedLimit(max_speed);
 			PMSM_FOC_Set_Torque(foc_current);
 			PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
 		}
@@ -50,14 +52,18 @@ static u32 key_task(void *p) {
 	value = gpio_funckey_value();
 	if (value != key_value[KEY_FUNC]) {
 		if (value) {
-			if (ctrl_mode == CTRL_MODE_OPEN) {
-				//ctrl_mode = CTRL_MODE_TRQ;
+			if (ctrl_mode == CTRL_MODE_TRQ) {
+				PMSM_FOC_EnableCruise(true);
+				ctrl_mode = CTRL_MODE_SPD;
+			}else if(ctrl_mode == CTRL_MODE_SPD){
+				PMSM_FOC_EnableCruise(false);
+				ctrl_mode = CTRL_MODE_TRQ;
 			}else {
-				//ctrl_mode = CTRL_MODE_OPEN;
+				ctrl_mode = CTRL_MODE_TRQ;
 			}
 			//PMSM_FOC_SetCtrlMode(ctrl_mode);
-			PMSM_FOC_SpeedLimit(max_speed);
-			max_speed += 10;
+			//PMSM_FOC_SpeedLimit(max_speed);
+			//max_speed += 1000;
 		}
 		key_value[KEY_FUNC] = value;
 	}

Applications/bsp/bsp.h

@@ -23,7 +23,7 @@
 #define ADC_CLOCK_MHz (30u)
 #define NS_PER_TCLK (8u) /* (1/120000000 * 1000000000) */
 #define NS_2_TCLK(ns) (((ns)/NS_PER_TCLK) + 1u) //ns תΪpwmʹ�õ��Ǹ�TIM��clk count
-#define FOC_PWM_FS (10000u)
+#define FOC_PWM_FS (30000u)
 #define FOC_PWM_period (TIM_CLOCK/FOC_PWM_FS)
 #define FOC_PWM_Half_Period (FOC_PWM_period/2)
 
@@ -84,7 +84,7 @@
 #define NO_SAMPLE_IDC //如果硬件没有采集母线电流，定义一下
 
 #ifdef GD32_FOC_DEMO
-#define ADC_TO_CURR_ceof (0.004f)
+#define ADC_TO_CURR_ceof (0.008f)
 #define VBUS_VOL_CEOF (ADC_REFERENCE_VOLTAGE*16/4096.0f)
 #else
 #define ADC_TO_CURR_ceof (0.0942f)

Applications/bsp/enc_intf.h

@@ -21,14 +21,14 @@
 #define ENC_PWM_RCU RCU_GPIOB
 #define ENC_PWM_MODE GPIO_MODE_IN_FLOATING
 
-#define ENC_I_GROUP GPIOC     /*测量编码器的ABI的I信号，360度同步一次*/
-#define ENC_I_PIN GPIO_PIN_10
-#define ENC_I_RCU RCU_GPIOC
+#define ENC_I_GROUP GPIOB     /*测量编码器的ABI的I信号，360度同步一次*/
+#define ENC_I_PIN GPIO_PIN_0
+#define ENC_I_RCU RCU_GPIOB
 #define ENC_I_MODE GPIO_MODE_IPU
-#define ENC_I_IRQ  EXTI10_15_IRQn
-#define ENC_I_EXTI EXTI_10
-#define ENC_I_EXIT_SRC_GROUP GPIO_PORT_SOURCE_GPIOC
-#define ENC_I_EXIT_SRC_PIN GPIO_PIN_SOURCE_10
+#define ENC_I_IRQ  EXTI0_IRQn
+#define ENC_I_EXTI EXTI_0
+#define ENC_I_EXIT_SRC_GROUP GPIO_PORT_SOURCE_GPIOB
+#define ENC_I_EXIT_SRC_PIN GPIO_PIN_SOURCE_0
 
 #define ENC_TIMER TIMER2  /* 测量编码器的ABI信号的AB信号 */
 #define ENC_TIMER_RCU RCU_TIMER2

Applications/bsp/mc_irqs.c

@@ -121,7 +121,13 @@ void EXTI0_IRQHandler(void)
 {
 	if(RESET != exti_interrupt_flag_get(EXTI_0)){
 		exti_interrupt_flag_clear(EXTI_0);
+#ifdef USE_ENCODER_ABI
+		ABI_I_IRQHandler();
+#elif defined (USE_ENCODER_HALL)
 		HALL_IRQHandler();
+#else
+	#error "Postion sensor ERROR"
+#endif
 	}	
 }
 
@@ -177,9 +183,6 @@ void EXTI5_9_IRQHandler(void){
 void EXTI10_15_IRQHandler(void){
 	if(RESET != exti_interrupt_flag_get(EXTI_10)){
 		exti_interrupt_flag_clear(EXTI_10);
-#ifdef GD32_FOC_DEMO
-		ABI_I_IRQHandler();
-#endif
 	}
 	if(RESET != exti_interrupt_flag_get(EXTI_11)){
 		exti_interrupt_flag_clear(EXTI_11);

Applications/bsp/pwm.c

@@ -303,6 +303,7 @@ static void timer0_dma_config(void)
 
 #if USER_ITMER_BRAKE==0
 static void _gpio_brakein_irq_enable(void){
+#ifndef GD32_FOC_DEMO
 	gpio_exti_source_select(GPIO_PORT_SOURCE_GPIOB, GPIO_PIN_SOURCE_4);
 	exti_init(EXTI_4, EXTI_INTERRUPT, EXTI_TRIG_BOTH);
 	nvic_irq_enable(EXTI4_IRQn, EBREAK_IRQ_PRIORITY, 0U);
@@ -313,7 +314,8 @@ static void _gpio_brakein_irq_enable(void){
 	exti_init(EXTI_5, EXTI_INTERRUPT, EXTI_TRIG_BOTH);
 	nvic_irq_enable(EXTI5_9_IRQn, EBREAK_IRQ_PRIORITY, 0U);
 	exti_interrupt_flag_clear(EXTI_5);
-	exti_interrupt_enable(EXTI_5);	
+	exti_interrupt_enable(EXTI_5);
+#endif	
 }
 #endif
 

Applications/bsp/sched_timer.c

@@ -9,10 +9,10 @@ void sched_timer_enable(u32 us) {
     /* initialize TIMER init parameter struct */
     timer_struct_para_init(&timer_initpara);
     /* TIMER1 configuration */
-    timer_initpara.prescaler         = TIM_CLOCK_MHz;
+    timer_initpara.prescaler         = TIM_CLOCK_MHz-1;
     timer_initpara.alignedmode       = TIMER_COUNTER_EDGE;
     timer_initpara.counterdirection  = TIMER_COUNTER_UP;
-    timer_initpara.period            = us;//400;
+    timer_initpara.period            = us-1;//400;
     timer_initpara.clockdivision     = TIMER_CKDIV_DIV1;
 	timer_initpara.repetitioncounter = 0;
     timer_init(SCHED_TIMER, &timer_initpara);

Applications/foc/commands.c

@@ -103,7 +103,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			break;
 		}
 	}
-	can_send_ack(command->can_src, CMD_2_CAN_KEY(command->cmd), (u8)erroCode);
+	//can_send_ack(command->can_src, CMD_2_CAN_KEY(command->cmd), (u8)erroCode);
 }
 
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -60,12 +60,12 @@ static __INLINE float Circle_Limitation(DQ_t *vdq, float vDC, float module, DQ_t
 static __INLINE void FOC_Set_DqRamp(dq_Rctrl *c, float target, int time) {	
 	float cp = c->s_Cp;
 	c->s_FinalTgt = target; 
-	c->s_Step = (c->s_FinalTgt - cp) / time;
-	if ((c->s_Step == 0) && (c->s_FinalTgt - cp > 0)) {
+	c->s_Step = (c->s_FinalTgt - cp) / (float)time;
+	if ((c->s_Step == 0) && ((c->s_FinalTgt - cp) != 0.0f)) {
 		if (c->s_FinalTgt - cp > 0) {
 			c->s_Step = 0.001;	
 		}else if (c->s_FinalTgt - cp < 0){
-			c->s_Step = 0.001;
+			c->s_Step = -0.001;
 		}
 	}
 }
@@ -197,7 +197,7 @@ void PMSM_FOC_Schedule(void) {
 
 	PMSM_FOC_Update_Hardware();
 	if (_gFOC_Ctrl.ctrl_count % 5 == 0) {
-		///plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), _gFOC_Ctrl.out.test_sample * 100);
+		//plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), FtoS16x1000(iabc[2]));
 	}
 
 	if (_gFOC_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
@@ -215,6 +215,7 @@ void PMSM_FOC_Schedule(void) {
 		float target_q = FOC_Get_DqRamp(&_gFOC_Ctrl.idq_ctl[1]);
 		err = target_q - _gFOC_Ctrl.out.s_RealIdq.q;
 		_gFOC_Ctrl.in.s_targetVdq.q = PI_Controller_RunSerial(_gFOC_Ctrl.pi_ctl_iq, err);
+		_gFOC_Ctrl.out.test_targetIQ = target_q;
 
 	}else {
 		_gFOC_Ctrl.in.s_targetVdq.d = FOC_Get_DqRamp(&_gFOC_Ctrl.vdq_ctl[0]);
@@ -235,15 +236,17 @@ void PMSM_FOC_Schedule(void) {
 	LowPass_Filter(_gFOC_Ctrl.out.s_FilterIdq.d, _gFOC_Ctrl.out.s_RealIdq.d, 0.01f);
 	LowPass_Filter(_gFOC_Ctrl.out.s_FilterIdq.q, _gFOC_Ctrl.out.s_RealIdq.q, 0.01f);
 #endif
-	if (_gFOC_Ctrl.ctrl_count % 5 == 0) {
+	if (_gFOC_Ctrl.ctrl_count % 8 == 0) {
+		//plot_1data16(_gFOC_Ctrl.out.test_sample);
 		//plot_1data16(FtoS16x1000(PMSM_FOC_Get_iDC()));
 		//plot_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
 		//plot_2data16(FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.q));
 		//plot_2data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.in.s_targetTorque));
 		//plot_2data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle), FtoS16(_gFOC_Ctrl.in.s_motAngle));
 		//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.q));
-		plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), FtoS16x1000(iabc[2]));
-		//plot_1data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle));
+		plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.q));
+		//plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), _gFOC_Ctrl.out.test_sample*100);
+		//plot_2data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle), FtoS16(encoder_get_pwm_angle()));
 	}	
 }
 
@@ -268,7 +271,9 @@ u8 PMSM_FOC_CtrlMode(void) {
 	}else if (_gFOC_Ctrl.in.n_ctlMode == CTRL_MODE_CURRENT) {
 		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_CURRENT;
 	}else {
-		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_TRQ;
+		if (!_gFOC_Ctrl.in.b_cruiseEna) {
+			_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_TRQ;
+		}
 	}
 	if (preMode != _gFOC_Ctrl.out.n_RunMode) {
 		if ((preMode == CTRL_MODE_SPD) && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
@@ -335,6 +340,7 @@ void PMSM_FOC_Stop(void) {
 	if (!_gFOC_Ctrl.in.b_motEnable) {
 		return;
 	}
+	PMSM_FOC_CoreInit();
 	_gFOC_Ctrl.in.b_motEnable = false;
 }
 
@@ -347,6 +353,9 @@ void PMSM_FOC_iBusLimit(float ibusLimit) {
 }
 
 void PMSM_FOC_SpeedLimit(float speedLimit) {
+	if (speedLimit > MAX_SPEED) {
+		speedLimit = MAX_SPEED;
+	}
 	_gFOC_Ctrl.params.s_maxRPM = (speedLimit);
 }
 

Applications/foc/core/PMSM_FOC_Core.h

@@ -62,6 +62,7 @@ typedef struct {
 	float s_FilteriDC;
 	float f_vdqRation;
 	s16   test_sample;
+	float test_targetIQ;
 	u8    n_Error;
 }FOC_OutP;
 

Applications/foc/core/e_ctrl.c

@@ -3,7 +3,7 @@
 #include "math/fix_math.h"
 #include "libs/logger.h"
 
-static e_Ctrl g_eCtrl;
+e_Ctrl g_eCtrl;
 static void _eCtrl_set_TgtCurrent(float c);
 static void _eCtrl_set_TgtSpeed(float s);
 static void _eCtrl_set_TgtTorque(float t);
@@ -73,7 +73,7 @@ static void _eCtrl_set_target(e_Ramp *ramp, float c) {
 	}
 	u32 step_ms = eCTRL_STEP_TS;	
 	if (sign > 0) { //增加扭矩
-		step_val = (c_now - c)/(g_eCtrl.accl_time/step_ms);
+		step_val = (c - c_now)/(g_eCtrl.accl_time/step_ms);
 		if (step_val < MIN_S16Q5) {
 			step_val = MIN_S16Q5;
 		}
@@ -82,6 +82,7 @@ static void _eCtrl_set_target(e_Ramp *ramp, float c) {
 		if (step_val < MIN_S16Q5) {
 			step_val = MIN_S16Q5;
 		}
+		step_val = -step_val;
 	}
 	eRamp_set_target(ramp, c);
 	eRamp_set_step(ramp, step_val);

Applications/foc/foc_config.h

@@ -9,11 +9,11 @@
 #define THROTTLE_MAX_VALUE 4.9f /* 转把最大值 */
 #define THROTTLE_MIN_RPM   10   /* 转把对应最小的速度 */
 #define THROTTLE_MIN_IDQ   20   /* 转把对应最小的扭矩电流 Q轴 */
-#define MAX_iDQ            150  /* 最大DQ轴电流， A*/
+#define MAX_iDQ            15  /* 最大DQ轴电流， A*/
 #define MAX_SPEED          8200 /* 最大转速 RPM*/
 #define MAX_iDC            45   /* 最大母线电流 A*/
 #define MIN_CRUISE_RPM 	   1000     /* 能启动定速巡航的最小速度 */
-#define MAX_TORQUE         30
+#define MAX_TORQUE         15
 #ifdef GD32_FOC_DEMO
 #define MAX_vDC (16)   /* 母线最大电压 V*/
 #else
@@ -28,11 +28,11 @@
 #define eCTRL_NEG_TORQUE (-100)   /* ebrake 的最大方向DQ电流，单位 ACKED_KEY*/
 
 #define IDQ_CTRL_TS FOC_PWM_FS
-#define SPD_CTRL_TS 100
-#define SPD_CTRL_MS (1000/SPD_CTRL_TS * 100)
+#define SPD_CTRL_TS 1000
+#define SPD_CTRL_MS (1000/SPD_CTRL_TS * 1000)
 #define VDQ_RAMP_TS 100
 #define VDQ_RAMP_FINAL_TIME 3000
-#define CURRENT_BANDWITH 1000 /* 电流环带宽 */
+#define CURRENT_BANDWITH 500 /* 电流环带宽 */
 
 #define SVM_Modulation 1.0f //(0.96f)
 

Applications/foc/motor/current.c

@@ -177,7 +177,7 @@ void phase_current_point(void *p){
 	out->n_CPhases = cs->c_phases;
 }
 #else
-#define LOW_FP_COEF 1.0f
+#define LOW_FP_COEF 0.5f
 void phase_current_get(float *iABC){
 	current_samp_t *cs = &g_cs;
 	s32 phase_current1, phase_current2;
@@ -189,16 +189,17 @@ void phase_current_get(float *iABC){
 		/* Ia = PhaseAOffset - ADC converted value) */
 		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
 		cs->adc_ib = (phase_current2 - cs->adc_offset_b);
+		LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
+		LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
+		cs->adc_ic_filter = -(cs->adc_ia_filter + cs->adc_ib_filter);
 
 		if (cs->c_ignore_phase == IGNORE_NONE) {
-			LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
-			LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
-			LowPass_Filter(cs->adc_ic_filter, -(cs->adc_ia + cs->adc_ib), LOW_FP_COEF);
+
 		}else if (cs->c_ignore_phase == IGNORE_A) {
-			LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
+			//LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
 			cs->adc_ia = cs->adc_ia_filter;
 		}else if (cs->c_ignore_phase == IGNORE_B) {
-			LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
+			//LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
 			cs->adc_ib = cs->adc_ib_filter;
 		}else if (cs->c_ignore_phase == IGNORE_ALL) {
 			cs->adc_ia = cs->adc_ia_filter;
@@ -210,16 +211,16 @@ void phase_current_get(float *iABC){
 		/* Ib = PhaseBOffset - ADC converted value) */
 		cs->adc_ib = (phase_current1 - cs->adc_offset_b);
 		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
-		
+		LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
+		LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
+		cs->adc_ia_filter = -(cs->adc_ib_filter + cs->adc_ic_filter);
 		if (cs->c_ignore_phase == IGNORE_NONE) {
-			LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
-			LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
-			LowPass_Filter(cs->adc_ia_filter, -(cs->adc_ib + cs->adc_ic), LOW_FP_COEF);
+
 		}else if (cs->c_ignore_phase == IGNORE_B) {
-			LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
+			//LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
 			cs->adc_ib = cs->adc_ib_filter;
 		}else if (cs->c_ignore_phase == IGNORE_C) {
-			LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
+			//LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
 			cs->adc_ic = cs->adc_ic_filter;
 		}else if (cs->c_ignore_phase == IGNORE_ALL) {
 			cs->adc_ib = cs->adc_ib_filter;
@@ -232,21 +233,22 @@ void phase_current_get(float *iABC){
 		/* Ia = PhaseAOffset - ADC converted value) */
 		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
 		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
+		LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
+		LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
+		cs->adc_ib_filter = -(cs->adc_ia_filter + cs->adc_ic_filter);
+
 		if (cs->c_ignore_phase == IGNORE_NONE) {
-			LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
-			LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
-			LowPass_Filter(cs->adc_ib_filter, -(cs->adc_ia + cs->adc_ic), LOW_FP_COEF);
+
 		}else if (cs->c_ignore_phase == IGNORE_A) {
-			LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
+			//LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
 			cs->adc_ia = cs->adc_ia_filter;
 		}else if (cs->c_ignore_phase == IGNORE_C) {
-			LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
+			//LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
 			cs->adc_ic = cs->adc_ic_filter;
 		}else if (cs->c_ignore_phase == IGNORE_ALL) {
 			cs->adc_ia = cs->adc_ia_filter;
 			cs->adc_ic = cs->adc_ic_filter;
 		}
-
 		cs->adc_ib = -(cs->adc_ia + cs->adc_ic);
 	}
 	iABC[0] = -cs->adc_ia * ADC_TO_CURR_ceof;

Applications/foc/motor/encoder.c

@@ -8,7 +8,7 @@
 #define ANGLE_OFFSET (-50.0f)//133.0f
 /* 磁编码器使用一对极的磁铁，所以编码器获取的角度和机械角度相同需要转为电角度*/
 #define DIR_ADJUGE_MAX_CNT 10
-#define PLL_BANDWIDTH 1000
+#define PLL_BANDWIDTH 200
 static encoder_t g_encoder;
 
 #define USE_PWM_ONLY
@@ -71,7 +71,7 @@ static __INLINE void encoder_run_pll(float cnt) {
 	g_encoder.est_vel_counts = PLL_run(&g_encoder.est_pll, cnt, pll_comp);
 	g_encoder.est_angle_counts = g_encoder.est_pll.observer;
 }
-static int _count = 0;
+
 float encoder_get_theta(void) {
 	if (!g_encoder.b_index_found) {
 		return g_encoder.pwm_angle;
@@ -99,10 +99,6 @@ float encoder_get_theta(void) {
 
 	encoder_run_pll((float)(cnt));
 
-	if (_count++ % 5 == 0) {
-		//plot_2data16(cnt, (s16)(g_encoder.est_vel_counts/((float)g_encoder.cpr) * 60.0f));
-	}
-
 	g_encoder.last_cnt = cnt;
 	g_encoder.last_us = timer_count32_get();
 	return g_encoder.abi_angle;
@@ -122,19 +118,15 @@ void ENC_ABI_IRQHandler(void) {
 	g_encoder.b_index_cnt = ENC_COUNT;	
 	if (!g_encoder.b_index_found){
 		ENC_COUNT = g_encoder.pwm_count;
-		g_encoder.last_cnt = 0;
+		g_encoder.last_cnt = g_encoder.pwm_count;
+		g_encoder.est_pll.observer = (float)g_encoder.pwm_count;
 		g_encoder.b_index_found = true;
-	}else {
-		if ((g_encoder.b_index_cnt > 100) && (g_encoder.b_index_cnt < (g_encoder.cpr - 100))) {
-			//g_encoder.b_index_found = false;
-		}
-		//g_encoder.last_cnt = g_encoder.b_index_cnt;
 	}
 }
 
 /* 编码器AB信号读书溢出处理 */
 void ENC_TIMER_Overflow(void) {
-	g_encoder.b_timer_ov = true;
+	//g_encoder.b_timer_ov = true;
 }
 
 
@@ -149,12 +141,17 @@ void ENC_PWM_Duty_Handler(float t, float d) {
 		return;
 	}
 	g_encoder.pwm_count = (u32)Nr;
-	g_encoder.pwm_angle = ENC_Pluse_Nr_2_angle(Nr) * MOTOR_POLES + g_encoder.enc_offset;
-	
+	g_encoder.pwm_angle = ENC_Pluse_Nr_2_angle(Nr) * MOTOR_POLES + g_encoder.enc_offset;	
 	rand_angle(g_encoder.pwm_angle);
-
-	//plot_1data16(g_encoder.est_vel_counts);
-	//plot_1data16(g_encoder.pwm_angle);
-	//plot_3data16(g_encoder.last_cnt, g_encoder.est_angle_counts, g_encoder.est_vel_counts);
+	if (!g_encoder.b_index_found) {
+		ENC_COUNT = g_encoder.pwm_count;
+		g_encoder.last_cnt = g_encoder.pwm_count;
+		g_encoder.est_pll.observer = (float)g_encoder.pwm_count;
+		g_encoder.b_index_found = true;
+	}
 }
 
+
+float encoder_get_pwm_angle(void) {
+	return g_encoder.pwm_angle;
+}

Applications/foc/motor/motor.c

@@ -41,7 +41,7 @@ bool mc_start(u8 mode) {
 		PMSM_FOC_SetErrCode(FOC_Param_Err);
 		return false;
 	}
-	if (PMSM_FOC_GetSpeed() > 10) {
+	if (PMSM_FOC_GetSpeed() > 10.0f) {
 		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 		return false;
 	}
@@ -49,7 +49,7 @@ bool mc_start(u8 mode) {
 		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
 		return false;
 	}
-	eCtrl_init(1000, 1000);
+	eCtrl_init(1, 1000);
 	motor_encoder_start(motor.s_direction);
 	PMSM_FOC_Start(mode);
 	pwm_turn_on_low_side();
@@ -68,7 +68,7 @@ bool mc_stop(void) {
 	if (!motor.b_start) {
 		return true;
 	}
-	if (PMSM_FOC_GetSpeed() > 10) {
+	if (PMSM_FOC_GetSpeed() > 10.0f) {
 		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 		return false;
 	}
@@ -114,7 +114,7 @@ void mc_encoder_calibrate(s16 vd) {
 			
 			motor_encoder_offset(angle);
 			
-			delay_ms(1);
+			delay_us(1000);
 		}
 	}
 	delay_ms(500);

Applications/foc/motor/motor_param.h

@@ -12,6 +12,7 @@
 #define MOTOR_Ld 0.000032f
 #define MOTOR_Lq 0.000032f
 #define MOTOR_POLES  10
+
 /*
 #define MOTOR_R   0.67f//0.33F
 #define MOTOR_Ld   0.00149f//0.00136F

Applications/libs/utils.h

@@ -95,7 +95,7 @@ static __inline__ void encode_s32(u8 *buff, s32 value)
 
 
 #define ARRAY_SIZE(a) (sizeof(a)/sizeof(a[0]))
-#define min(a,b) (a>b?b:a)
+#define min(a,b) ((a)>(b)?(b):(a))
 #define MAX(x, y) ((x)>(y)?(x):(y))
 
 #define F2I(f) ((int)(f))

Applications/math/fix_math.h

@@ -41,7 +41,9 @@ typedef int32_t   s32q19_t;
 #define S16_mul(a, b, q) (((s32)(a)*(b)) >> q)
 
 #define FtoS16(f) ((s16)f)
-#define FtoS16x1000(f) ((s16)(f * 100))
+#define FtoS16x1000(f) ((s16)(f * 1000.0f))
+#define FtoS16x10(f) ((s16)(f * 10.0f))
+
 #define LowPass_Filter_Fix(value, sample, filter_constant, shift)	(value = ((s32)value * (S16Q14(1) - filter_constant) + (s32)sample * filter_constant) >> (shift))
 
 #define MIN_S16Q5 (1.0F/32.0F)

Project/GD32_DEMO.uvoptx

@@ -120,7 +120,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
-          <Name>m</Name>
+          <Name>0</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -215,6 +215,11 @@
           <WinNumber>1</WinNumber>
           <ItemText>g_encoder,0x0A</ItemText>
         </Ww>
+        <Ww>
+          <count>13</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>g_eCtrl,0x0A</ItemText>
+        </Ww>
       </WatchWindow1>
       <MemoryWindow1>
         <Mm>