编码器错误检查调试

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

Applications/app/app.c

@@ -135,7 +135,7 @@ static u32 _app_report_task(void *p) {
 	
 	if (++loop % 10 == 0) {
 		//sys_debug("rst 0x%x\n", get_mcu_reset_source());
-		//sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
+		sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
 		sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
 		sys_debug("FOC time err %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error);
 		//sys_debug("acc vol %d, bid %d\n", get_acc_vol(), gpio_board_id());

Applications/foc/core/PI_Controller.h

@@ -35,8 +35,6 @@ typedef struct {
 	float  sat;
 	float  DT;
 	bool   is_sat;
-	float  last_err;
-	float  b_dstart;
 }PI_Controller;
 
 static __INLINE void PI_Controller_Change_Kpi(PI_Controller *pi, float kp, float ki) {
@@ -53,7 +51,6 @@ static __INLINE void PI_Controller_Reset(PI_Controller *pi, float init) {
 	pi->Ui = (init);
 	pi->sat = 0.0f;
 	pi->is_sat = false;
-	pi->b_dstart = false;
 }
 
 static __INLINE float PI_Controller_Run(PI_Controller *pi, float err) {
@@ -71,57 +68,21 @@ static __INLINE float PI_Controller_Run(PI_Controller *pi, float err) {
 	return sat_out;
 }
 
-static __INLINE float PI_Controller_RunLimit(PI_Controller *pi, float err) {
+static __INLINE float PI_Controller_RunVel(PI_Controller *pi, float err) {
 	float kp_err = (err) * pi->kp;
 	float ki_err = (err) * pi->ki;
-	float integral = ki_err * pi->DT;
-	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
-	float out = pi->Ui + kp_err;
-	float sat_out = MATH_sat(out, pi->min, pi->max);
-	
-	return sat_out;
-}
-
-
-static __INLINE float PI_Controller_RunWithDiff(PI_Controller *pi, float err) {
-	float kp_err = (err) * pi->kp;
-	float ki_err = (err) * pi->ki;
-	float integral = ki_err * pi->DT;
-	if (!pi->b_dstart) {
-		pi->last_err = err;
-		pi->b_dstart = true;
-	}
-	
-	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
-	float out = pi->Ui + kp_err + pi->kd * (err - pi->last_err);
-	float sat_out =  (MATH_sat(out, pi->min, pi->max));
-	if (out != sat_out) {
-		pi->is_sat = true;
-	}else {
-		pi->is_sat = false;
-	}
-	pi->last_err = err;
-	return sat_out;
-}
+	float i_max = pi->max - kp_err;
 
-#if 0
-static __INLINE float PI_Controller_RunSat(PI_Controller *pi, float err) {
-	float kp_err = (err) * pi->kp;
-	float ki_err = (err) * pi->ki;
-	float integral = ki_err * pi->DT;
-	pi->Ui = pi->Ui + integral + pi->sat * pi->kd;
-	float out = pi->Ui + kp_err ;
-	float out_sat = MATH_sat(out, pi->min, pi->max);
-	if (out != out_sat) {
-		pi->is_sat = true;
-	}else {
-		pi->is_sat = false;
+	if (i_max < pi->min) {
+		i_max = pi->min;
 	}
-	pi->sat = out_sat - out;
+	pi->Ui += ki_err * pi->DT;
+	pi->Ui = fclamp(pi->Ui, pi->min, i_max);
+	float out = pi->Ui + kp_err;
 
-	return out_sat;
+	return fclamp(out, pi->min, pi->max);
 }
-#endif
+
 
 /* 电流环，PI 串联结构 */
 static __INLINE float PI_Controller_Current(PI_Controller *pi, float err, float ff) {

Applications/foc/core/PMSM_FOC_Core.h

@@ -96,7 +96,6 @@ typedef struct {
 	DQ_t    s_targetVdq;
 	float   s_targetTorque; //限速后的实际扭矩
 	float 	s_vDC;
-	EPM_Dir_t epmDirection;
 	u8      n_ctlMode;
 	bool    b_motEnable;
 	bool    b_cruiseEna;

Applications/foc/core/ladrc_observer.c

@@ -49,7 +49,6 @@ void ladrc_observer_init(float Wo, float vel_min, float lpf_cut_off) {
 		observer.angle_array[i] = 0;
 	}
 	ladrc_observer_band(0);
-	sys_debug("ld:%f, lq:%f, vmin:%f, wo:%f\n", observer.ld, observer.lq, observer.vel_min, observer.Wo);
 }
 
 float ladrc_observer_update(float va, float vb, float ia, float ib) {

Applications/foc/mc_error.c

@@ -52,7 +52,7 @@ void mc_crit_err_add(u8 code, s16 value1, s16 value2) {
 }
 
 void mc_crit_err_add_s16(u8 code, s16 value) {
-	mc_crit_err_add(code, value, 0);
+	mc_crit_err_add(code, value, -1);
 }
 
 

Applications/foc/motor/encoder.c

@@ -23,11 +23,11 @@
 
 encoder_t g_encoder;
 
-static __INLINE u32 _abi_count(void) {
+static __INLINE s16 _abi_count(void) {
 #ifdef ENCODER_CC_INVERT
-	return (g_encoder.cpr - ENC_COUNT);
+	return (g_encoder.cpr - (s16)ENC_COUNT);
 #else
-	return ENC_COUNT;
+	return (s16)ENC_COUNT;
 #endif
 }
 
@@ -73,6 +73,7 @@ void encoder_init_clear(void) {
 	g_encoder.pwm_angle = 0.0f;
 	g_encoder.est_angle_counts = 0;
 	g_encoder.est_vel_counts = 0;
+	g_encoder.est_vel_cnt_filter = 0;
 	g_encoder.position = 0.0f;
 	g_encoder.interpolation = 0.0f;
 	//g_encoder.cali_angle = INVALID_ANGLE;
@@ -126,8 +127,9 @@ static __INLINE bool encoder_run_pll(float cnt) {
 	g_encoder.est_vel_counts = PLL_run(&g_encoder.est_pll, cnt, pll_comp);
 	g_encoder.est_angle_counts = g_encoder.est_pll.observer;
     bool snap_to_zero_vel = false;
+	g_encoder.est_vel_cnt_filter = LowPass_Filter(g_encoder.est_vel_cnt_filter, g_encoder.est_vel_counts, 0.1f);
     if (ABS(g_encoder.est_pll.out) < 0.5f * g_encoder.est_pll.DT * g_encoder.est_pll.ki) {
-        g_encoder.est_vel_counts = g_encoder.est_pll.out = 0.0f;  // align delta-sigma on zero to prevent jitter
+        g_encoder.est_vel_cnt_filter = g_encoder.est_vel_counts = g_encoder.est_pll.out = 0.0f;  // align delta-sigma on zero to prevent jitter
         snap_to_zero_vel = true;
     }
 	return snap_to_zero_vel;
@@ -152,14 +154,32 @@ static __INLINE float _eccentricity_compensation(int cnt) {
 u32 enc_pwm_err_ms = 0;
 s16 enc_delta_err1 = 0;
 s16 enc_delta_err2 = 0;
-static void encoder_detect_error(u32 cnt) {
+static s16 enc_r = 0;
+static s16 enc_cnt = 0;
+static s16 enc_last_cnt = 0;
+static s16 enc_test1 = 0;
+static s16 enc_test2 = 0;
+static s16 enc_test3 = 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) {
 #ifdef CONFIG_ENC_DETECT_ERR
 	static u32 _mayerr_cnt = 0;
 	if (ENCODER_NO_ERR == g_encoder.enc_maybe_err) {
 		s16 delta_cnt = cnt - g_encoder.last_cnt;
 		bool skip = false;
 		if (g_encoder.b_timer_ov) {
-			delta_cnt = (cnt + ENC_MAX_RES) - g_encoder.last_cnt;
+			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;
+			}
+		}
+		if ((delta_cnt > 200) || (delta_cnt < -200)) {
+			enc_test1 = cnt;
+			enc_test2 = g_encoder.last_cnt;
+			enc_test3 = g_encoder.b_timer_ov;
 		}
 #ifdef CONFIG_ENC_ERR_TEST
 		if (g_encoder.produce_error) {
@@ -170,9 +190,10 @@ static void encoder_detect_error(u32 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) >= 4) {
+		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) {
 			g_encoder.start_dianostic_cnt ++;
@@ -204,7 +225,10 @@ static void encoder_detect_error(u32 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 = delta_cnt;
+					enc_delta_err2 = (s16)((g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f);
+					enc_r = r;
+					enc_cnt = cnt;
+					enc_last_cnt = g_encoder.last_cnt;
 				}
 			}else {
 				_mayerr_cnt = 0;
@@ -223,11 +247,16 @@ float encoder_get_theta(void) {
 	if (!g_encoder.b_index_found) {
 		return g_encoder.pwm_angle;
 	}
-	u32 cnt = _abi_count();
+	s16 cnt = _abi_count();
 	__NOP();__NOP();__NOP();__NOP();
 	if (ENC_OverFlow()) {
 		cnt = _abi_count();
-		g_encoder.b_timer_ov = true;
+		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附近震荡
+			g_encoder.b_timer_ov = false;
+		}else {
+			g_encoder.b_timer_ov = true;
+		}
 		ENC_ClearUpFlags();
 	}
 
@@ -239,7 +268,7 @@ float encoder_get_theta(void) {
 		g_encoder.interpolation = 0.1f;
 	}else {
 		if (cnt == g_encoder.last_cnt) {
-			g_encoder.interpolation += g_encoder.est_vel_counts * FOC_CTRL_US;
+			g_encoder.interpolation += g_encoder.est_vel_cnt_filter * FOC_CTRL_US;
 			if (g_encoder.interpolation > ENC_MAX_interpolation) {
 				g_encoder.interpolation = ENC_MAX_interpolation;
 			}else if (g_encoder.interpolation < -ENC_MAX_interpolation) {
@@ -364,10 +393,20 @@ void ENC_TIMER_Overflow(void) {
 /*PWM 信号捕获一个周期的处理 */
 static int pwm_count = 0;
 static int pwm_check_count = 0;
+static int pwm_duty_err = 0;
+static float pwm_err_min = 0;
+static float pwm_err_max = 0;
 void ENC_PWM_Duty_Handler(float t, float d) {
 	float duty = ENC_Duty(d, t);
 	if (duty < ENC_PWM_Min_P || duty > ENC_PWM_Max_P) {
-		return;
+		pwm_duty_err++;
+		if (duty < ENC_PWM_Min_P) {
+			pwm_err_min = duty;
+			duty = ENC_PWM_Min_P;
+		}else {
+			pwm_err_max = duty;
+			duty = ENC_PWM_Max_P;
+		}
 	}
 	float Nr = ENC_Duty_2_Pluse_Nr(duty);
 	if (Nr < 0) {
@@ -417,7 +456,7 @@ float encoder_get_pwm_angle(void) {
 }
 
 float encoder_get_abi_angle(void) {
-	u32 cnt = _abi_count();
+	s16 cnt = _abi_count();
 	float angle = ENC_Pluse_Nr_2_angle((float)cnt) * g_encoder.motor_poles + g_encoder.enc_offset;
 	rand_angle(angle);
 	return angle;
@@ -426,5 +465,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\n", g_encoder.pwm_count, abi_I_delta);
-	sys_debug("pwm freq %f, err %d\n", enc_get_pwm_freq(), g_encoder.enc_maybe_err);
+	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);
+	}
 }

Applications/foc/motor/encoder.h

@@ -18,12 +18,13 @@ typedef struct {
 	bool  b_timer_ov;
 	bool  b_lock_pos;
 	bool  b_cali_err;
-	u32   cpr;
+	s16   cpr;
 	s16   last_cnt;
 	u32   b_index_cnt;
 	u32   last_us;
 	s8    direction; //motor running dir, 逆时针 正，顺时针负
 	float est_vel_counts; //每秒多少个计数
+	float est_vel_cnt_filter;
 	float est_angle_counts;
 	float rpm;
 	//u8   *encoder_off_count;

Applications/foc/motor/motor.c

@@ -993,12 +993,13 @@ void ADC_IRQHandler(void) {
 			mc_crit_err_add_s16(FOC_CRIT_Angle_Err, (s16)motor_encoder_get_speed());
 			if (motor_encoder_may_error() == ENCODER_PWM_ERR) {
 				mc_set_critical_error(FOC_CRIT_Encoder_Err);
-				mc_crit_err_add_s16(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms);
+				mc_crit_err_add(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms, enc_delta_err2);
 			}else if (motor_encoder_may_error() == ENCODER_AB_ERR) {
 				mc_set_critical_error(FOC_CRIT_ENC_AB_Err);
 				mc_crit_err_add(FOC_CRIT_ENC_AB_Err, enc_delta_err1, enc_delta_err2);
 			}
 			PMSM_FOC_Stop();
+			g_meas_foc.first = true;
 		}
 	}
 	TIME_MEATURE_END();
@@ -1187,6 +1188,7 @@ static void mc_motor_runstop(void) {
 			mask = cpu_enter_critical();
 			PMSM_FOC_Stop();
 			pwm_disable_channel();
+			g_meas_foc.first = true;
 			cpu_exit_critical(mask);
 		}
 	}
@@ -1196,6 +1198,7 @@ static void mc_motor_runstop(void) {
 		mc_gear_vmode_changed();
 		thro_torque_reset();
 		pwm_enable_channel();
+		g_meas_foc.first = true;
 		cpu_exit_critical(mask);
 	}
 }
@@ -1226,8 +1229,9 @@ void Sched_MC_mTask(void) {
 	if ((PMSM_FOC_GetVbusCurrent() > (CONFIG_MAX_VBUS_CURRENT * 1.1f)) || (PMSM_FOC_GetVbusCurrent() < CONFIG_MAX_CHRG_CURRENT)) {
 		vbus_err_cnt ++;
 		if (vbus_err_cnt > 10) {
-			mc_error.ibus_x10 = (s16)(PMSM_FOC_GetVbusCurrent() * 10.0f);
 			mc_set_critical_error(FOC_CRIT_IDC_OV);
+			mc_crit_err_add(FOC_CRIT_IDC_OV, (s16)sample_vbus_raw(), (s16)sample_ibus_raw());
+			mc_save_err_runtime();
 		}
 	}else {
 		vbus_err_cnt = 0;
@@ -1243,7 +1247,7 @@ void Sched_MC_mTask(void) {
 		if (foc_observer_sensorless_stable()) {//unstable 记录在ADC中断处理中
 			if (motor_encoder_may_error() == ENCODER_PWM_ERR) {
 				mc_set_critical_error(FOC_CRIT_Encoder_Err);
-				mc_crit_err_add_s16(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms);
+				mc_crit_err_add(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms, enc_delta_err2);
 			}else if (motor_encoder_may_error() == ENCODER_AB_ERR) {
 				mc_set_critical_error(FOC_CRIT_ENC_AB_Err);
 				mc_crit_err_add(FOC_CRIT_ENC_AB_Err, enc_delta_err1, enc_delta_err2);

Applications/libs/time_measure.c

@@ -34,6 +34,9 @@ void time_measure_start(measure_time_t *m){
 	}
 }
 void time_measure_end(measure_time_t *m) {
+	if (m->first) {
+		return;
+	}
 	m->exec_time = time_delta_us(m->exec_count, NULL);
 	if (m->exec_time > m->exec_max_time) {
 		m->exec_time_error ++;

Applications/os/os_task.c

@@ -5,8 +5,8 @@
 #include "bsp/bsp.h"
 extern uint32_t get_system_sleep_time(void);
 
-static u64 shark_mseconds;
-static u32 _g_ticks;
+static u64 shark_mseconds = 0;
+static u32 _g_ticks = 0;
 static u32 shark_timer_task_handler(void *);
 
 static shark_timer_t shark_timer_head = {