编码器z index 校准AB干扰

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

Applications/foc/motor/encoder.c

@@ -225,8 +225,12 @@ static void encoder_detect_error(s16 cnt) {
 				r_thr = 0.7f;
 				cnt_thr = 1;
 			}
-			if (r_abs <= 0.01f) {
-				cnt_thr = 1;
+			if (r_thr >= 0.5f) {
+				if (r < 0) {
+					r_thr = 2.0f;
+				}else if (r_abs <= 0.01f) {
+					cnt_thr = 1;
+				}
 			}
 			if (r_abs <= r_thr || r_abs >= (2.0f - r_thr)) {
 				_mayerr_cnt ++;
@@ -376,6 +380,7 @@ static void encoder_sync_pwm_abs(void) {
 	g_encoder.abi_angle = g_encoder.pwm_angle;
 	g_encoder.b_index_found = true;
 	g_encoder.last_delta_cnt = MAX_S16;
+	g_encoder.align_cnt = g_encoder.align_step = 0;
 	PLL_Reset(&g_encoder.est_pll, (float)_abi_count());
 	cpu_exit_critical(mask);
 }
@@ -383,26 +388,25 @@ static void encoder_sync_pwm_abs(void) {
 /*I 信号的中断处理，一圈一个中断*/
 //static int abi_I_delta = 0xFFFFFFF;
 void ENC_ABI_IRQHandler(void) {
-	g_encoder.b_index_cnt = _abi_count();
+	g_encoder.z_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)) {
+	if (g_encoder.z_index_cnt > 10 && g_encoder.z_index_cnt < (g_encoder.cpr - 10)) {
 		if (abi_I_delta == 0xFFFFFFF) {
-			abi_I_delta = g_encoder.b_index_cnt;
+			abi_I_delta = g_encoder.z_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;
+	if (g_encoder.z_index_cnt > 10 && g_encoder.z_index_cnt < 50) {
+		g_encoder.align_cnt = -(g_encoder.z_index_cnt - 5);
+	}else if (g_encoder.z_index_cnt > (g_encoder.cpr - 50) && g_encoder.z_index_cnt < (g_encoder.cpr - 10)) {
+		g_encoder.align_cnt =  g_encoder.cpr - g_encoder.z_index_cnt - 5;
 	}else {
-		if (g_encoder.b_index_cnt <=10 || g_encoder.b_index_cnt >= (g_encoder.cpr - 10)) {
+		if (g_encoder.z_index_cnt <=10 || g_encoder.z_index_cnt >= (g_encoder.cpr - 10)) {
 			g_encoder.align_cnt = 0;
 		}
-		//abi_I_delta = g_encoder.b_index_cnt;
 	}
 #endif
 }

Applications/foc/motor/encoder.h

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