编码器通过Z信号校准AB的偶发干扰，防止角度误差累计

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

Applications/foc/core/controller.h

@@ -79,6 +79,7 @@ typedef enum {
 	FOC_EV_THRO_START_V,
 	FOC_START_Err_Code,
 	FOC_Reset_Reson,
+	FOC_NV_Err,
 }ctrl_event_r_t;
 
 

Applications/foc/mc_config.c

@@ -1,8 +1,10 @@
 #include "foc/mc_config.h"
+#include "foc/core/controller.h"
 #include "libs/utils.h"
 #include "libs/crc16.h"
 #include "app/nv_storage.h"
 #include "libs/logger.h"
+#include "foc/mc_error.h"
 
 mc_config conf;
 
@@ -119,15 +121,18 @@ void mc_conf_load(void) {
 		sys_debug("conf block 1 OK!\n");
 		memcpy(&conf, &temp, sizeof(temp));
 		nv_write_config_block(0, (u8 *)&temp, sizeof(temp));
+		mc_crit_err_add(FOC_NV_Err, 0, 0);
 	}else if (crc16 != temp.crc) {
 		if (idx0_success) {
 			sys_debug("conf block1 Bad!\n");
 			nv_write_config_block(1, (u8 *)&conf, sizeof(conf));
+			mc_crit_err_add(FOC_NV_Err, 1, 1);
 		}else {
 			sys_debug("conf block0&1 Bad!\n");
 			mc_conf_default();
 			nv_write_config_block(0, (u8 *)&conf, sizeof(conf));
 			nv_write_config_block(1, (u8 *)&conf, sizeof(conf));
+			mc_crit_err_add(FOC_NV_Err, 0, 1);
 		}
 	}else {
 		sys_debug("conf block 1 OK!\n");

Applications/foc/motor/encoder.c

@@ -72,6 +72,7 @@ void encoder_init_clear(void) {
 	g_encoder.start_dianostic_cnt = 0;
 	g_encoder.align_cnt = 0;
 	g_encoder.align_step = 0;
+	g_encoder.align_cnt_final = 0;
 	g_encoder.pwm_time_ms = get_tick_ms();
 	_init_pll();
 }
@@ -286,8 +287,9 @@ float encoder_get_theta(bool detect_err) {
 			g_encoder.interpolation = 0.0f;
 		}
 	}
-	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;
+	step_towards(&g_encoder.align_step, (float)g_encoder.align_cnt_final, 0.01f);
+	float angle_count = cnt + g_encoder.align_step + g_encoder.interpolation;
+	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle(angle_count) * 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;
@@ -379,30 +381,42 @@ static void encoder_sync_pwm_abs(void) {
 	g_encoder.b_index_found = true;
 	g_encoder.last_delta_cnt = MAX_S16;
 	g_encoder.align_cnt = g_encoder.align_step = 0;
+	g_encoder.align_cnt_final = 0;
+	g_encoder.z_index_counter = 0;
 	PLL_Reset(&g_encoder.est_pll, (float)_abi_count());
 	cpu_exit_critical(mask);
 }
 
 /*I 信号的中断处理，一圈一个中断*/
-//static int abi_I_delta = 0xFFFFFFF;
-static int abi_z_count = 0;
+#define CONFIG_Z_IDX_MARGIN 5
+#define CONFIG_Z_IDX_FIXED_COUNT 30  /* Z信号最大允许的补偿机角度为 (30-5)/4096*360=2.2度*/
+#define CONFIG_Z_IDX_MAX_CNT_PER_IRQ 4 /* 每次z信号中断，最大补偿机械角度(4)/4096*360=0.34，类似做低通滤波 */
+#define CONFIG_Z_IDX_IGNORE_MAX_CNT 5
 void ENC_ABI_IRQHandler(u32 count) {
 #ifdef ENCODER_CC_INVERT
 	g_encoder.z_index_cnt = (g_encoder.cpr -1) - count;
 #else
 	g_encoder.z_index_cnt = count;
 #endif
-	abi_z_count++;
-
-	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;
+  	g_encoder.z_index_counter++;
+	s16 pre_cnt = g_encoder.align_cnt;
+	if (g_encoder.z_index_cnt <= CONFIG_Z_IDX_MARGIN) {
+		g_encoder.align_cnt = 0;
+	}else if (g_encoder.z_index_cnt >= (g_encoder.cpr - CONFIG_Z_IDX_MARGIN)) {
+		g_encoder.align_cnt = 0;
+	}else if (g_encoder.z_index_cnt > CONFIG_Z_IDX_MARGIN && g_encoder.z_index_cnt < CONFIG_Z_IDX_FIXED_COUNT) {
+		g_encoder.align_cnt = -(g_encoder.z_index_cnt - CONFIG_Z_IDX_MARGIN);
+	}else if (g_encoder.z_index_cnt > (g_encoder.cpr - CONFIG_Z_IDX_FIXED_COUNT) && g_encoder.z_index_cnt < (g_encoder.cpr - CONFIG_Z_IDX_MARGIN)) {
+		g_encoder.align_cnt =  g_encoder.cpr - g_encoder.z_index_cnt - CONFIG_Z_IDX_MARGIN;
 	}else {
-		if (g_encoder.z_index_cnt <=10 || g_encoder.z_index_cnt >= (g_encoder.cpr - 10)) {
-			g_encoder.align_cnt = 0;
-		}
+		//maybe error?
+		g_encoder.z_index_err_counter++;
+	}
+	s16 delta = pre_cnt - g_encoder.align_cnt;
+	if (ABS(delta) >= CONFIG_Z_IDX_IGNORE_MAX_CNT) {
+		g_encoder.align_cnt = pre_cnt;
 	}
+	step_towards_s16(&g_encoder.align_cnt_final, g_encoder.align_cnt, CONFIG_Z_IDX_MAX_CNT_PER_IRQ);
 }
 
 /* 编码器AB信号读书溢出处理 */
@@ -485,7 +499,8 @@ 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, g_encoder.pwm_start_cnt, g_encoder.z_index_cnt, abi_z_count);
+	sys_debug("Z idx %d,%d,%d,%d\n", g_encoder.z_index_err_counter, g_encoder.pwm_start_cnt, g_encoder.z_index_cnt, g_encoder.z_index_counter);
+	sys_debug("Z err: %f, %d,%d\n", g_encoder.align_step, g_encoder.align_cnt, g_encoder.align_cnt_final);
 	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_cnt, enc_last_cnt, enc_test4);

Applications/foc/motor/encoder.h

@@ -21,8 +21,11 @@ typedef struct {
 	s16   cpr;
 	s16   last_cnt;
 	s16   align_cnt; //z 中断的时候的cnt数，需要计算角度的是时候处理这个align
-	s16   align_step;
+	s16   align_cnt_final; //最终需要补偿的计数，类似对align_cnt 做低通滤波，防止Z信号的干扰导致错误补偿
+	float align_step;
 	s16   z_index_cnt;
+	u32   z_index_counter;
+	u32   z_index_err_counter;
 	s16   pwm_start_cnt;
 	u32   last_us;
 	s8    direction; //motor running dir, 逆时针 正，顺时针负