优化编码器错误检测

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

Applications/app/app.c

@@ -161,7 +161,7 @@ static u32 _app_report_task(void *p) {
 		encoder_log();
 		//motor_debug();
 		//sample_log();
-		//PMSM_FOC_LogDebug();
+		PMSM_FOC_LogDebug();
 		//F_debug();
 		//eCtrl_debug_log();
 		//sys_debug("enc err %d, %d\n", foc_observer_enc_errcount(), foc_observer_sensorless_stable());

Applications/foc/motor/encoder.c

@@ -198,27 +198,31 @@ static void encoder_detect_error(s16 cnt) {
 			g_encoder.last_delta_cnt = delta_cnt;
 			skip = true;
 		}
-		if ((g_encoder.last_delta_cnt <= CONFIG_ENC_DIANOSTIC_MIN_CNT*1.2f) && get_delta_ms(g_encoder.pwm_time_ms) >= 8) {
-			g_encoder.enc_maybe_err = ENCODER_PWM_ERR;
-			enc_pwm_err_ms = get_delta_ms(g_encoder.pwm_time_ms);
-			enc_delta_err2 = (s16)((g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f);
-		}
-		if (g_encoder.start_dianostic_cnt < 0xFFFF) {
+		float ab_thr = CONFIG_ENC_DIANOSTIC_MIN_CNT*1.2f;
+		if (g_encoder.last_delta_cnt <= ab_thr) {
+			if (get_delta_ms(g_encoder.pwm_time_ms) >= 8) {
+				g_encoder.enc_maybe_err = ENCODER_PWM_ERR;
+				enc_pwm_err_ms = get_delta_ms(g_encoder.pwm_time_ms);
+				enc_delta_err2 = (s16)((g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f);
+			}
+			skip = true;
+			g_encoder.start_dianostic_cnt = 0;
+		}else if (g_encoder.start_dianostic_cnt < 0xFFFFFF) {
 			g_encoder.start_dianostic_cnt ++;
 		}
-		if (!skip && ((g_encoder.last_delta_cnt > CONFIG_ENC_DIANOSTIC_MIN_CNT) || (_mayerr_cnt != 0)) && (g_encoder.start_dianostic_cnt >= 1000)) {
+		if (!skip && ((g_encoder.last_delta_cnt > ab_thr) || (_mayerr_cnt != 0)) && (g_encoder.start_dianostic_cnt >= FOC_PWM_FS)) {
 			float last_delta = (float)g_encoder.last_delta_cnt;
 			float r = (float)delta_cnt / (last_delta + 0.0000001f);
 			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个机械角度
+			if (g_encoder.last_delta_cnt <= ab_thr * 2) {
 				r_thr = 0.3f;
 				cnt_thr = 4;
-			}else if (g_encoder.last_delta_cnt <= CONFIG_ENC_DIANOSTIC_MIN_CNT * 4) { //0.8个机械角度
+			}else if (g_encoder.last_delta_cnt <= ab_thr * 4) {
 				r_thr = 0.5f;
 				cnt_thr = 3;
-			}else if (g_encoder.last_delta_cnt <= CONFIG_ENC_DIANOSTIC_MIN_CNT * 6) { //1.3个机械角度
+			}else if (g_encoder.last_delta_cnt <= ab_thr * 6) {
 				r_thr = 0.6f;
 				cnt_thr = 2;
 			}else {
@@ -228,6 +232,7 @@ static void encoder_detect_error(s16 cnt) {
 			if (r_thr >= 0.5f) {
 				if (r < 0) {
 					r_thr = 2.0f;
+					cnt_thr = 1;
 				}else if (r_abs <= 0.01f) {
 					cnt_thr = 1;
 				}

Applications/foc/motor/encoder.h

@@ -33,7 +33,7 @@ typedef struct {
 	//s16  *encoder_off_map;
 	bool  produce_error;
 	s16   last_delta_cnt;
-	u16   start_dianostic_cnt;
+	u32   start_dianostic_cnt;
 	u8    enc_maybe_err;
 	u32   pwm_time_ms;
 }encoder_t;