1. 编码器Z信号使用timer处理上升沿，可以做数字滤波

2. 数字滤波的系数不能超过0xF

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

Applications/bsp/gd32/board_mc105_v3.h

@@ -353,12 +353,22 @@
 #define ENC_I_GROUP GPIOB     /*测量编码器的ABI的I信号，360度同步一次*/
 #define ENC_I_PIN GPIO_PIN_6
 #define ENC_I_RCU RCU_GPIOB
+#if 0
 #define ENC_I_MODE GPIO_MODE_IPU
 #define ENC_I_IRQ  EXTI5_9_IRQn
 #define ENC_I_EXTI EXTI_6
 #define ENC_I_EXIT_SRC_GROUP GPIO_PORT_SOURCE_GPIOB
 #define ENC_I_EXIT_SRC_PIN GPIO_PIN_SOURCE_6
-
+#else
+#define ENC_I_MODE GPIO_MODE_IN_FLOATING
+#define ENC_I_TIMER TIMER3    /* 测量绝对编码器PWM输出的占空比，获取转子angle*/
+#define ENC_I_TIMER_RCU RCU_TIMER3
+#define ENC_I_TIMER_IRQ TIMER3_IRQn
+#define ENC_I_TIMER_CHAN  TIMER_CH_0
+#define ENC_I_TIMER_IRQ_CH TIMER_INT_CH0
+#define ENC_I_TIMER_INT_FLG TIMER_INT_FLAG_CH0
+#define ENC_I_IRQHandler TIMER3_IRQHandler
+#endif
 #define ENC_TIMER TIMER2  /* 测量编码器的ABI信号的AB信号 */
 #define ENC_TIMER_RCU RCU_TIMER2
 #define ENC_TIMER_IRQ TIMER2_IRQn
@@ -383,7 +393,9 @@
 
 #define ENC_MAX_interpolation 1.0F
 
-#define ENC_FILTER_NR          30
+#define CONFIG_ENC_FILTER_NR          12 //1100： fSAMP=fDTS/16, N=8,采样率 120M/16 = 7.5M
+#define CONFIG_PWM_FILTER_NR          12
+
 #ifdef CONFIG_PWM_UV_SWAP
 #define ENCODER_CC_INVERT 1
 #endif

Applications/bsp/gd32/enc_intf.c

@@ -18,20 +18,22 @@ static void _io_init(void) {
 	gpio_init(ENC_I_GROUP, ENC_I_MODE, GPIO_OSPEED_50MHZ, ENC_I_PIN);
 }
 
-static void _io_init_irq(void) {
+static void z_io_init_irq(void) {
+#ifdef ENC_I_IRQ
 	gpio_exti_source_select(ENC_I_EXIT_SRC_GROUP, ENC_I_EXIT_SRC_PIN);
 	exti_init(ENC_I_EXTI, EXTI_INTERRUPT, EXTI_TRIG_RISING);
 	exti_interrupt_flag_clear(ENC_I_EXTI);
 	exti_interrupt_enable(ENC_I_EXTI);
-
 	nvic_irq_enable(ENC_I_IRQ, ENC_I_EXIT_IRQ_PRIORITY, 0U);
+#endif
 }
 
 void enc_intf_init(u32 rate) {
 	_io_init();
 	enc_intf_pwm_counter();
 	enc_intf_quadrature_init(rate);
-	_io_init_irq();
+	z_io_init_irq();
+	enc_intf_z_counter();
 }
 
 void enc_intf_quadrature_init(u32 rate) {
@@ -60,7 +62,7 @@ void enc_intf_quadrature_init(u32 rate) {
     timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
     timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
     timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
-    timer_icinitpara.icfilter    = ENC_FILTER_NR;//采样频率120M, 这个必须要加入滤波，否则AB的计数可能不对
+    timer_icinitpara.icfilter    = CONFIG_ENC_FILTER_NR;//采样频率120M, 这个必须要加入滤波，否则AB的计数可能不对
     timer_input_capture_config(timer,TIMER_CH_0,&timer_icinitpara);
 	
 	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
@@ -105,7 +107,7 @@ void enc_intf_pwm_counter(void) {
 	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
 	timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
 	timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
-	timer_icinitpara.icfilter	 = ENC_FILTER_NR;
+	timer_icinitpara.icfilter	 = CONFIG_PWM_FILTER_NR;
 	timer_input_pwm_capture_config(timer, ENC_PWM_TIMER_CHAN, &timer_icinitpara);
 
 	/* slave mode selection: TIMER */
@@ -126,6 +128,54 @@ void enc_intf_pwm_counter(void) {
     timer_enable(timer);
 }
 
+
+void enc_intf_z_counter(void) {
+#ifdef ENC_I_TIMER
+	timer_ic_parameter_struct timer_icinitpara;
+	timer_parameter_struct timer_initpara;
+	u32 timer = ENC_I_TIMER;
+
+	rcu_periph_clock_enable(ENC_I_TIMER_RCU);
+
+	timer_deinit(timer);
+
+	/* TIMER configuration */
+	timer_struct_para_init(&timer_initpara);
+	timer_initpara.prescaler		= TIM_CLOCK/PWM_TIME_CLK - 1;
+	timer_initpara.alignedmode		= TIMER_COUNTER_EDGE;
+	timer_initpara.counterdirection  = TIMER_COUNTER_UP;
+	timer_initpara.period		   = 65535;
+	timer_initpara.clockdivision	= TIMER_CKDIV_DIV1;
+	timer_initpara.repetitioncounter = 0;
+	timer_init(timer,&timer_initpara);
+
+	timer_channel_input_struct_para_init(&timer_icinitpara);
+	/* TIMER CH0 PWM input capture configuration */
+	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
+	timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
+	timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
+	timer_icinitpara.icfilter	 = CONFIG_PWM_FILTER_NR;
+	timer_input_capture_config(timer, ENC_I_TIMER_CHAN, &timer_icinitpara);
+
+	/* slave mode selection: TIMER */
+	timer_input_trigger_source_select(timer, TIMER_SMCFG_TRGSEL_CI0FE0);
+	timer_slave_mode_select(timer, TIMER_SLAVE_MODE_RESTART);
+
+	/* select the master slave mode */
+	timer_master_slave_mode_config(timer, TIMER_MASTER_SLAVE_MODE_ENABLE);
+
+    /* auto-reload preload enable */
+    timer_auto_reload_shadow_enable(timer);
+    /* clear channel 0 interrupt bit */
+    timer_interrupt_flag_clear(timer, ENC_I_TIMER_INT_FLG);
+    /* channel 0 interrupt enable */
+    timer_interrupt_enable(timer, ENC_I_TIMER_IRQ_CH);
+	nvic_irq_enable(ENC_I_TIMER_IRQ, ENC_I_EXIT_IRQ_PRIORITY, 0);
+    /* TIMER2 counter enable */
+    timer_enable(timer);
+#endif
+}
+
 __weak void ENC_TIMER_Overflow(void) {
 
 }
@@ -143,7 +193,19 @@ __weak void ENC_ABI_IRQHandler(void) {
 }
 
 void ABI_I_IRQHandler(void) {
+#ifdef ENC_I_IRQ
 	ENC_ABI_IRQHandler();
+#endif
+}
+
+void ENC_I_IRQHandler(void) {
+#ifdef ENC_I_TIMER
+    if(SET == timer_interrupt_flag_get(ENC_I_TIMER, ENC_I_TIMER_INT_FLG)){
+        /* clear channel 0 interrupt bit */
+        timer_interrupt_flag_clear(ENC_I_TIMER, ENC_I_TIMER_INT_FLG);
+		ENC_ABI_IRQHandler();
+    }
+#endif
 }
 
 __weak void ENC_PWM_Duty_Handler(float t, float d) {

Applications/bsp/gd32/enc_intf.h

@@ -20,6 +20,8 @@
 
 void enc_intf_quadrature_init(u32 rate);
 void enc_intf_pwm_counter(void);
+void enc_intf_z_counter(void);
+
 void enc_intf_init(u32 rate);
 float enc_get_pwm_freq(void);
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -345,10 +345,11 @@ static __INLINE void PMSM_FOC_Calc_iDC_Fast(void) {
 }
 
 #define CONFIG_PEAK_CNT 3 //计算经过的电周期内的最大值(peak 峰值)
-#define CONFIG_PHASE_UNBALANCE_THROLD 10.0F
+#define CONFIG_PHASE_UNBALANCE_THROLD 4.0F
 #define CONFIG_PHASE_UNBALANCE_R      0.1F
 static float phase_unbalance_r = 0.0f;
 static float phase_a_max, phase_b_max, phase_c_max;
+static u32 phase_unbalance_cnt;
 static __INLINE void PMSM_FOC_Phase_Unbalance(void) {
 	static u32 _cycle_cnt = 0, _last_mod_cnt = 0;
 	static float a_max = 0, b_max = 0, c_max = 0;
@@ -383,9 +384,9 @@ static __INLINE void PMSM_FOC_Phase_Unbalance(void) {
 	}
 	_last_mod_cnt = mod_cnt;
 
-	a_max = MAX(a_max, gFoc_Ctrl.in.s_iABCFilter[0] * (1.0f / 0.707f));
-	b_max = MAX(b_max, gFoc_Ctrl.in.s_iABCFilter[1] * (1.0f / 0.707f));
-	c_max = MAX(c_max, gFoc_Ctrl.in.s_iABCFilter[2] * (1.0f / 0.707f));
+	a_max = MAX(a_max, gFoc_Ctrl.in.s_iABCFilter[0] * (2.2f));
+	b_max = MAX(b_max, gFoc_Ctrl.in.s_iABCFilter[1] * (2.2f));
+	c_max = MAX(c_max, gFoc_Ctrl.in.s_iABCFilter[2] * (2.2f));
 	if (trigger) { //经过CONFIG_PEAK_CNT个周期，已经得到peak值
 		float i_min = 1000.0f, i_max = 0;
 		if (a_max > i_max) {
@@ -421,6 +422,7 @@ static __INLINE void PMSM_FOC_Phase_Unbalance(void) {
 		phase_a_max = a_max;
 		phase_b_max = b_max;
 		phase_c_max = c_max;
+		phase_unbalance_cnt = _unbalance_cnt;
 		a_max = b_max = c_max = 0;
 	}
 }
@@ -481,6 +483,7 @@ static  __INLINE void PMSM_FOC_DeadTime_Compensate(s32 PWM_Half_Period) {
 //#define UPDATE_Lq_By_iq   /* Q轴电感 通过Iq电流补偿 */
 #define CONFIG_Volvec_Delay_Comp /* 电压矢量角度补偿 */
 #define CONFIG_Volvec_Delay_Comp_Start_Vel 500 // rpm
+static float encoder_angle,obser_angle, obser_vel = 111111;
 static __INLINE bool PMSM_FOC_Update_Input(void) {
 	AB_t iAB;
 	float *iabc = gFoc_Ctrl.in.s_iABC;
@@ -501,8 +504,14 @@ static __INLINE bool PMSM_FOC_Update_Input(void) {
 			gFoc_Ctrl.in.s_motVelocity = 0;
 			return false;
 		}
+		if (obser_vel == 111111) {
+			obser_vel = foc_observer_sensorless_speed();
+			obser_angle = foc_observer_sensorless_angle();
+			encoder_angle = enc_angle;
+		}
 		enc_angle = foc_observer_sensorless_angle();
 		enc_vel = foc_observer_sensorless_speed();
+
 	}
 	
 	if (!gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
@@ -667,7 +676,10 @@ bool PMSM_FOC_Schedule(void) {
 void PMSM_FOC_LogDebug(void) {
 	sys_debug("DC curr %f --- %f, - %f\n", gFoc_Ctrl.out.s_CalciDC, gFoc_Ctrl.out.s_CalciDC2, gFoc_Ctrl.hwLim.s_iDCMax);
 	sys_debug("%s\n", gFoc_Ctrl.out.empty_load?"NoLoad Running":"Load Runing");
-	sys_debug("unbalance: %f, %f, %f, %f\n", phase_unbalance_r, phase_a_max, phase_b_max, phase_c_max);
+	sys_debug("unbalance: %d, %f, %f, %f, %f\n", phase_unbalance_cnt, phase_unbalance_r, phase_a_max, phase_b_max, phase_c_max);
+	if (obser_vel != 111111) {
+		sys_debug("AB error: %f,%f,%f\n", obser_angle, encoder_angle, obser_vel);
+	}
 }
 
 /*called in media task */

Applications/foc/motor/encoder.c

@@ -387,8 +387,10 @@ static void encoder_sync_pwm_abs(void) {
 
 /*I 信号的中断处理，一圈一个中断*/
 static int abi_I_delta = 0xFFFFFFF;
+static int abi_z_count = 0;
 void ENC_ABI_IRQHandler(void) {
 	g_encoder.z_index_cnt = _abi_count();
+	abi_z_count++;
 #if 0
 	if (!g_encoder.b_index_found){
 		encoder_sync_pwm_abs();
@@ -493,7 +495,7 @@ float encoder_get_abi_angle(void) {
 
 void encoder_log(void) {
 	sys_debug("pwm %f, abi %f\n", encoder_get_pwm_angle(), encoder_get_abi_angle());
-	sys_debug("pwm count %d, I count %d,%d,%d\n", g_encoder.pwm_count, abi_I_delta, g_encoder.align_cnt, g_encoder.align_step);
+	sys_debug("pwm count %d, I count %d,%d,%d\n", g_encoder.pwm_count, abi_I_delta, g_encoder.z_index_cnt, abi_z_count);
 	sys_debug("pwm freq %f, err %d, %f, %f\n", enc_get_pwm_freq(), pwm_duty_err, pwm_err_min, pwm_err_max);
 	if (g_encoder.enc_maybe_err) {
 		sys_debug("E:%d,%d,%d,%d,%d,%d, %d\n", enc_test1, enc_test2, enc_test3, enc_r, enc_cnt, enc_last_cnt, enc_test4);