1. 编码器Z信号对齐补偿，后期需要优化

2. 提高Z信号gpio中断优先级

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

Applications/bsp/bsp.h

@@ -1,7 +1,7 @@
 #ifndef __BSP_H__
 #define __BSP_H__
 
-#define TIMER_UP_IRQ_PRIORITY 0
+#define TIMER_UP_IRQ_PRIORITY 2
 #define ADC_IRQ_PRIORITY 1
 #define HALL_IRQ_PRIORITY 2
 #define SCHED_TIMER_IRQ_PRIORITY 3
@@ -11,7 +11,7 @@
 #define UART_IRQ_PRIORITY      6 
 #define ENC_TIMER_IRQ_PRIORITY 2
 #define ENC_PWM_IRQ_PRIORITY 2
-#define ENC_I_EXIT_IRQ_PRIORITY 2
+#define ENC_I_EXIT_IRQ_PRIORITY 0
 #define ENC_OTHER_IRQ_PRIORITY 8
 
 #define THREE_SHUNTS_SAMPLE 1

Applications/foc/motor/encoder.c

@@ -25,7 +25,7 @@ encoder_t g_encoder;
 
 static __INLINE s16 _abi_count(void) {
 #ifdef ENCODER_CC_INVERT
-	return (g_encoder.cpr - (s16)ENC_COUNT);
+	return ((g_encoder.cpr -1) - (s16)ENC_COUNT);
 #else
 	return (s16)ENC_COUNT;
 #endif
@@ -83,6 +83,8 @@ void encoder_init_clear(void) {
 	g_encoder.last_delta_cnt = MAX_S16;
 	g_encoder.enc_maybe_err = ENCODER_NO_ERR;
 	g_encoder.start_dianostic_cnt = 0;
+	g_encoder.align_cnt = 0;
+	g_encoder.align_step = 0;
 	g_encoder.pwm_time_ms = get_tick_ms();
 	_init_pll();
 }
@@ -160,6 +162,7 @@ static s16 enc_last_cnt = 0;
 static s16 enc_test1 = 0;
 static s16 enc_test2 = 0;
 static s16 enc_test3 = 0;
+static s16 enc_test4 = 0;
 #define MAX_CPR_CNT_PER_CTL 40
 /* 9000RPM = 150 RPS = 150 * ENC_MAX_RES * FOC_CTRL_US = 39, 每个控制周期 39个count */
 static void encoder_detect_error(s16 cnt) {
@@ -169,11 +172,16 @@ static void encoder_detect_error(s16 cnt) {
 		s16 delta_cnt = cnt - g_encoder.last_cnt;
 		bool skip = false;
 		if (g_encoder.b_timer_ov) {
-			if((cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2)) ||
-					(cnt < (MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt < (MAX_CPR_CNT_PER_CTL*2))) { //需要处理低速在overflow附近震荡
-				delta_cnt = cnt - g_encoder.last_cnt;
-			}else {
-				delta_cnt = (cnt + ENC_MAX_RES) - g_encoder.last_cnt;
+			bool is_jitter = ((cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2)) ||
+					(cnt < (MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt < (MAX_CPR_CNT_PER_CTL*2))); //需要处理低速在overflow附近震荡
+			if (!is_jitter) {
+				s16 com = _pll_over_comp();
+				if (com > 0) {
+					delta_cnt = delta_cnt + com;
+				}else {
+					delta_cnt = delta_cnt + com;
+				}
+				enc_test4 = delta_cnt;
 			}
 		}
 		if ((delta_cnt > 200) || (delta_cnt < -200)) {
@@ -201,7 +209,7 @@ static void encoder_detect_error(s16 cnt) {
 		if (!skip && ((g_encoder.last_delta_cnt > CONFIG_ENC_DIANOSTIC_MIN_CNT) || (_mayerr_cnt != 0)) && (g_encoder.start_dianostic_cnt >= 1000)) {
 			float last_delta = (float)g_encoder.last_delta_cnt;
 			float r = (float)delta_cnt / (last_delta + 0.0000001f);
-			r = ABS(r);
+			float r_abs = ABS(r);
 			float r_thr;
 			u32   cnt_thr;
 			if (g_encoder.last_delta_cnt <= CONFIG_ENC_DIANOSTIC_MIN_CNT * 2) { //0.4个机械角度
@@ -217,16 +225,16 @@ static void encoder_detect_error(s16 cnt) {
 				r_thr = 0.7f;
 				cnt_thr = 1;
 			}
-			if (r <= 0.01f) {
+			if (r_abs <= 0.01f) {
 				cnt_thr = 1;
 			}
-			if (r <= r_thr || r >= (2.0f - r_thr)) {
+			if (r_abs <= r_thr || r_abs >= (2.0f - r_thr)) {
 				_mayerr_cnt ++;
 				if (_mayerr_cnt >= cnt_thr) {
 					g_encoder.enc_maybe_err = ENCODER_AB_ERR;
 					enc_delta_err1 = g_encoder.last_delta_cnt;
 					enc_delta_err2 = (s16)((g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f);
-					enc_r = r;
+					enc_r = r*10;
 					enc_cnt = cnt;
 					enc_last_cnt = g_encoder.last_cnt;
 				}
@@ -278,8 +286,8 @@ float encoder_get_theta(void) {
 			g_encoder.interpolation = 0.0f;
 		}
 	}
-
-	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle((float)cnt + g_encoder.interpolation) * g_encoder.motor_poles + g_encoder.enc_offset;
+	step_towards_s16(&g_encoder.align_step, g_encoder.align_cnt, 2);
+	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle((float)(cnt/* + g_encoder.align_step*/) + g_encoder.interpolation) * g_encoder.motor_poles + g_encoder.enc_offset;
 	g_encoder.abi_angle += _eccentricity_compensation(cnt);
 	rand_angle(g_encoder.abi_angle);
 	g_encoder.last_cnt = cnt;
@@ -373,15 +381,30 @@ static void encoder_sync_pwm_abs(void) {
 }
 
 /*I 信号的中断处理，一圈一个中断*/
-static int abi_I_delta = 0;
+//static int abi_I_delta = 0xFFFFFFF;
 void ENC_ABI_IRQHandler(void) {
-	g_encoder.b_index_cnt = ENC_COUNT;	
+	g_encoder.b_index_cnt = _abi_count();
+#if 0
 	if (!g_encoder.b_index_found){
 		encoder_sync_pwm_abs();
 	}
 	if (g_encoder.b_index_cnt > 10 && g_encoder.b_index_cnt < (g_encoder.cpr - 10)) {
-		abi_I_delta = g_encoder.b_index_cnt;
-	} 
+		if (abi_I_delta == 0xFFFFFFF) {
+			abi_I_delta = g_encoder.b_index_cnt;
+		}
+	}
+#else
+	if (g_encoder.b_index_cnt > 10 && g_encoder.b_index_cnt < 50) {
+		g_encoder.align_cnt = -(g_encoder.b_index_cnt - 5);
+	}else if (g_encoder.b_index_cnt > (g_encoder.cpr - 50) && g_encoder.b_index_cnt < (g_encoder.cpr - 10)) {
+		g_encoder.align_cnt =  g_encoder.cpr - g_encoder.b_index_cnt - 5;
+	}else {
+		if (g_encoder.b_index_cnt <=10 || g_encoder.b_index_cnt >= (g_encoder.cpr - 10)) {
+			g_encoder.align_cnt = 0;
+		}
+		//abi_I_delta = g_encoder.b_index_cnt;
+	}
+#endif
 }
 
 /* 编码器AB信号读书溢出处理 */
@@ -457,16 +480,16 @@ float encoder_get_pwm_angle(void) {
 
 float encoder_get_abi_angle(void) {
 	s16 cnt = _abi_count();
-	float angle = ENC_Pluse_Nr_2_angle((float)cnt) * g_encoder.motor_poles + g_encoder.enc_offset;
+	float angle = ENC_Pluse_Nr_2_angle((float)(cnt + g_encoder.align_step)) * g_encoder.motor_poles + g_encoder.enc_offset;
 	rand_angle(angle);
 	return angle;
 }
 
 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\n", g_encoder.pwm_count, abi_I_delta);
+	sys_debug("pwm count %d, I count %d,%d,%d\n", g_encoder.pwm_count, _abi_count(), g_encoder.align_cnt, g_encoder.align_step);
 	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\n", enc_test1, enc_test2, enc_test3, enc_r, enc_cnt, enc_last_cnt);
+		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);
 	}
 }

Applications/foc/motor/encoder.h

@@ -20,7 +20,9 @@ typedef struct {
 	bool  b_cali_err;
 	s16   cpr;
 	s16   last_cnt;
-	u32   b_index_cnt;
+	s16   align_cnt; //z 中断的时候的cnt数，需要计算角度的是时候处理这个align
+	s16   align_step;
+	s16   b_index_cnt;
 	u32   last_us;
 	s8    direction; //motor running dir, 逆时针 正，顺时针负
 	float est_vel_counts; //每秒多少个计数