1. encoder angle offset 修改为s16格式

2. 通过编码器方向判断给定的补偿margin，正负方向margin不能一样

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

Applications/foc/motor/encoder.c

@@ -327,7 +327,7 @@ float encoder_get_position(void) {
 float encoder_zero_phase_detect(float *enc_off) {	
 	delay_ms(5);	
 	float total_enc_off = g_encoder.pwm_count;
-	float prev_offset = g_encoder.enc_offset;
+	s16 prev_offset = g_encoder.enc_offset;
 	float phase = encoder_get_pwm_angle();
 	float total_ph = phase;
 	int count = 0;
@@ -348,7 +348,7 @@ float encoder_zero_phase_detect(float *enc_off) {
 	}
 	sys_debug("count = %d, %f, %d\n", count, total_enc_off, g_encoder.pwm_count);
 	float offset_now = total_ph/(float)(count + 1);
-	g_encoder.enc_offset = offset_now;
+	g_encoder.enc_offset = (s16)offset_now;
 	g_encoder.enc_count_off = (u32)(total_enc_off/(float)(count + 1));
 	if (enc_off) {
 		*enc_off = (float)g_encoder.enc_count_off;
@@ -388,26 +388,38 @@ static void encoder_sync_pwm_abs(void) {
 }
 
 /*I 信号的中断处理，一圈一个中断*/
-#define CONFIG_Z_IDX_MARGIN 5
+#define CONFIG_Z_IDX_MARGIN_B 4
+#define CONFIG_Z_IDX_MARGIN_F 1
 #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
+#define CONFIG_Z_IDX_IGNORE_MAX_CNT 10
 void ENC_ABI_IRQHandler(u32 count) {
+	s16 z_margin;
 #ifdef ENCODER_CC_INVERT
 	g_encoder.z_index_cnt = (g_encoder.cpr -1) - count;
+	if (g_encoder.est_vel_cnt_filter > 0){
+		z_margin = CONFIG_Z_IDX_MARGIN_B;
+	}else {
+		z_margin = CONFIG_Z_IDX_MARGIN_F;
+	}
 #else
 	g_encoder.z_index_cnt = count;
+	if (g_encoder.est_vel_cnt_filter > 0){
+		z_margin = CONFIG_Z_IDX_MARGIN_F;
+	}else {
+		z_margin = CONFIG_Z_IDX_MARGIN_B;
+	}
 #endif
   	g_encoder.z_index_counter++;
 	s16 pre_cnt = g_encoder.align_cnt;
-	if (g_encoder.z_index_cnt <= CONFIG_Z_IDX_MARGIN) {
+	if (g_encoder.z_index_cnt <= z_margin) {
 		g_encoder.align_cnt = 0;
-	}else if (g_encoder.z_index_cnt >= (g_encoder.cpr - CONFIG_Z_IDX_MARGIN)) {
+	}else if (g_encoder.z_index_cnt >= (g_encoder.cpr - z_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 > z_margin && g_encoder.z_index_cnt < CONFIG_Z_IDX_FIXED_COUNT) {
+		g_encoder.align_cnt = -(g_encoder.z_index_cnt - z_margin);
+	}else if (g_encoder.z_index_cnt > (g_encoder.cpr - CONFIG_Z_IDX_FIXED_COUNT) && g_encoder.z_index_cnt < (g_encoder.cpr - z_margin)) {
+		g_encoder.align_cnt =  g_encoder.cpr - g_encoder.z_index_cnt - z_margin;
 	}else {
 		//maybe error?
 		g_encoder.z_index_err_counter++;
@@ -416,7 +428,10 @@ void ENC_ABI_IRQHandler(u32 count) {
 	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);
+	/* 编码器没有做零位置校准不能补偿 */
+	if (g_encoder.enc_offset == 0) {
+		step_towards_s16(&g_encoder.align_cnt_final, g_encoder.align_cnt, CONFIG_Z_IDX_MAX_CNT_PER_IRQ);
+	}
 }
 
 /* 编码器AB信号读书溢出处理 */

Applications/foc/motor/encoder.h

@@ -3,7 +3,7 @@
 #include "foc/core/PI_Controller.h"
 typedef struct {
 	bool  b_index_found; //I 对齐
-	float enc_offset;
+	s16   enc_offset;
 	u32   enc_count_off;
 	u8    motor_poles;
 	float pwm_angle;