打开编码器错误检测，角度错误后保存runtime现场

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

Applications/app/app.c

@@ -104,7 +104,7 @@ void app_start(void){
 	set_log_level(MOD_SYSTEM, L_disable);
 	can_message_init();
 	nv_storage_init();
-	mc_crit_err_block_init();
+	mc_err_block_init();
 	mc_init();
 #ifdef GD32_FOC_DEMO	
 	extern void key_init(void);
@@ -202,7 +202,7 @@ static u32 _app_plot_task(void * args) {
 static u32 _app_low_task(void *args) {
 	wdog_reload();
 	if (!PMSM_FOC_Is_Start()) {
-		mc_crit_err_save();
+		mc_err_block_save();
 	}
 	fetch_jtag_cmd();
 	return 1;

Applications/foc/commands.c

@@ -603,16 +603,16 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			len += 18;
 			break;
 		}
-		case Foc_Get_MC_NV_err:
+		case Foc_Get_MC_NV_Crit_Err:
 		{
 			s16 offset = decode_s16((u8 *)command->data);
 			len += mc_crit_err_get(offset, response+3, sizeof(response) - 3);
 			break;
 		}
-		case Foc_Get_MC_RT_Err:
+		case Foc_Get_MC_NV_Err_RT:
 		{
 			s16 offset = decode_s16((u8 *)command->data);
-			len += mc_runtime_get(offset, response+3, sizeof(response) - 3);
+			len += mc_err_runtime_get(offset, response+3, sizeof(response) - 3);
 			break;
 		}
 

Applications/foc/commands.h

@@ -44,8 +44,8 @@ typedef enum {
 	Foc_Set_Gear_Mode,//0-3, eco, normal, sport, turbo
 	Foc_Set_Adrc_Params,
 	Foc_Get_Adrc_Params,
-	Foc_Get_MC_RT_Err,
-	Foc_Get_MC_NV_err,
+	Foc_Get_MC_NV_Err_RT,
+	Foc_Get_MC_NV_Crit_Err,
 	Foc_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,

Applications/foc/mc_error.c

@@ -12,7 +12,7 @@ static bool _w_pending_rt = false;
 #define ERR_MAX_SIZE 200
 #define RUNTIME_MAX_SIZE 20
 
-void mc_crit_err_block_init(void) {
+void mc_err_block_init(void) {
 	_e_nv = (err_nv_t *)page_buff;
 	nv_read_crit_errblock(page_buff, one_page_size);
 	u16 crc16 = crc16_get((u8 *)_e_nv->node, one_page_size - sizeof(err_nv_t));
@@ -20,6 +20,7 @@ void mc_crit_err_block_init(void) {
 		memset(page_buff, 0, one_page_size);
 		_e_nv->crc = crc16_get((u8 *)_e_nv->node, one_page_size - sizeof(err_nv_t));
 		nv_write_crit_errblock(page_buff, one_page_size);
+		sys_debug("mc crc\n");
 	}
 
 	_r_nv = (runtime_nv_t *)page_buff_runtime;
@@ -29,6 +30,7 @@ void mc_crit_err_block_init(void) {
 		memset(page_buff_runtime, 0, one_page_size);
 		_r_nv->crc = crc16_get((u8 *)_r_nv->node, one_page_size - sizeof(runtime_nv_t));
 		nv_write_runtime_block(page_buff_runtime, one_page_size);
+		sys_debug("mc rt crc\n");
 	}
 
 	sys_debug("mc err p: 0x%x\n", _e_nv->node);
@@ -54,7 +56,7 @@ void mc_crit_err_add_s16(u8 code, s16 value) {
 }
 
 
-void mc_runtime_add(runtime_node_t *rt) {
+void mc_err_runtime_add(runtime_node_t *rt) {
 	runtime_node_t *node = _r_nv->node;
 	node = node + _r_nv->tail;
 	memcpy(node, rt, sizeof(runtime_node_t));
@@ -68,7 +70,7 @@ void mc_runtime_add(runtime_node_t *rt) {
 }
 
 
-void mc_crit_err_save(void) {
+void mc_err_block_save(void) {
 	if (_w_pending) {
 		_e_nv->crc = crc16_get((u8 *)_e_nv->node, one_page_size - sizeof(err_nv_t));
 		nv_write_crit_errblock(page_buff, one_page_size);
@@ -106,7 +108,7 @@ int mc_crit_err_get(s16 offset, u8 *buff, int len) {
 }
 
 
-int mc_runtime_get(s16 offset, u8 *buff, int len) {
+int mc_err_runtime_get(s16 offset, u8 *buff, int len) {
 	int count = len / sizeof(runtime_node_t);
 	int index = offset;
 	u8 *p_remain = buff++;
@@ -138,4 +140,3 @@ void mc_err_code_log(void) {
 		sys_debug("mc rt count %d, tail %d\n", _r_nv->count, _r_nv->tail);
 	}
 }
-

Applications/foc/mc_error.h

@@ -2,11 +2,6 @@
 #define _MC_ERROR_H__
 #include "os/os_types.h"
 
-typedef struct {
-	s16 err_value;
-	u32 err_time;
-}err_record_t;
-
 #pragma  pack (push,1)
 typedef struct {
 	u8 idx;
@@ -53,13 +48,13 @@ typedef struct{
 	runtime_node_t node[0];
 }runtime_nv_t;
 
-void mc_crit_err_block_init(void);
+void mc_err_block_init(void);
 void mc_crit_err_add(u8 code, s16 value1, s16 value2);
 void mc_crit_err_add_s16(u8 code, s16 value);
-void mc_crit_err_save(void);
+void mc_err_block_save(void);
 int mc_crit_err_get(s16 offset, u8 *buff, int len);
-void mc_runtime_add(runtime_node_t *rt);
-int mc_runtime_get(s16 offset, u8 *buff, int len);
+void mc_err_runtime_add(runtime_node_t *rt);
+int mc_err_runtime_get(s16 offset, u8 *buff, int len);
 
 #endif /*_MC_ERROR_H__ */
 

Applications/foc/motor/encoder.c

@@ -147,6 +147,7 @@ static __INLINE float _eccentricity_compensation(int cnt) {
 }
 
 #define CONFIG_ENC_DETECT_ERR
+///#define CONFIG_ENC_ERR_TEST
 #define CONFIG_ENC_DIANOSTIC_MIN_CNT (10.0F * ENC_MAX_RES * FOC_CTRL_US) //600rpm, 每隔控制周期 0.2232 机械角度
 u32 enc_pwm_err_ms = 0;
 s16 enc_delta_err1 = 0;
@@ -154,7 +155,7 @@ s16 enc_delta_err2 = 0;
 static void encoder_detect_error(u32 cnt) {
 #ifdef CONFIG_ENC_DETECT_ERR
 	static u32 _mayerr_cnt = 0;
-	if (ENCODER_NO_ERR != g_encoder.enc_maybe_err) {
+	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) {
@@ -390,6 +391,8 @@ void ENC_PWM_Duty_Handler(float t, float d) {
 	if (!g_encoder.produce_error) {
 		g_encoder.pwm_time_ms = get_tick_ms();
 	}
+#else
+	g_encoder.pwm_time_ms = get_tick_ms();
 #endif
 }
 static u32 _check_time = 0;

Applications/foc/motor/motor.c

@@ -22,6 +22,8 @@
 extern float target_d;
 extern float target_q;
 #endif
+extern u32 enc_pwm_err_ms;
+extern s16 enc_delta_err1, enc_delta_err2;
 
 static bool mc_detect_hwbrake(void);
 static bool mc_is_gpio_mlock(void);
@@ -118,7 +120,7 @@ static void MC_Check_MosVbusThrottle(void) {
 
 static u32 _self_check_task(void *p) {
 	if (ENC_Check_error()) {
-		mc_crit_err_add_s16(FOC_CRIT_Encoder_Err, 0);
+		mc_crit_err_add_s16(FOC_CRIT_Encoder_Err, -1);
 		mc_set_critical_error(FOC_CRIT_Encoder_Err);
 	}
 	if (get_tick_ms() < 5000) { //启动后5s内检测锁电机线
@@ -914,12 +916,7 @@ static void _pwm_brake_prot_timer_handler(shark_timer_t *t){
 	sys_debug("MC protect error\n");
 }
 
-void MC_Protect_IRQHandler(void){
-	pwm_brake_enable(false);
-	shark_timer_post(&_brake_prot_timer, 1000);
-	if (!motor.b_start) {
-		return;
-	}
+static void mc_save_err_runtime(void) {
 	mc_error.vbus_x10 = (s16)(sample_vbus_raw() * 10.0f); 
 	mc_error.ibus_x10 = (s16)(sample_ibus_raw() * 10.0f);
 	mc_error.vacc_x10 = (s16) (sample_acc_vol_raw() * 10.0f);
@@ -939,13 +936,22 @@ void MC_Protect_IRQHandler(void){
 	mc_error.enc_error = motor_encoder_may_error();
 	mc_error.sensorless_rpm = (s16)foc_observer_sensorless_speed();
 
+	mc_err_runtime_add(&mc_error);
+}
+
+void MC_Protect_IRQHandler(void){
+	pwm_brake_enable(false);
+	shark_timer_post(&_brake_prot_timer, 1000);
+	if (!motor.b_start) {
+		return;
+	}
 	mc_set_critical_error(FOC_CRIT_Phase_Err);
+	mc_save_err_runtime();
 	_mc_internal_init(CTRL_MODE_OPEN, false);
 	adc_stop_convert();
 	pwm_stop();
 	PMSM_FOC_Stop();
 	pwm_up_enable(true);
-	mc_runtime_add(&mc_error);
 }
 
 void motor_debug(void) {
@@ -979,9 +985,21 @@ void ADC_IRQHandler(void) {
 	}
 	TIME_MEATURE_START();
 	if (!PMSM_FOC_Schedule()) {/* FOC 角度错误，立即停机 */
-		mc_set_critical_error(FOC_CRIT_Angle_Err);
-		PMSM_FOC_Stop();
-		pwm_disable_channel();
+		if (PMSM_FOC_Is_Start()) {
+			pwm_disable_channel();
+			/* 记录错误 */
+			mc_save_err_runtime();
+			mc_set_critical_error(FOC_CRIT_Angle_Err);
+			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);
+			}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();
+		}
 	}
 	TIME_MEATURE_END();
 }
@@ -1186,8 +1204,6 @@ static void mc_motor_runstop(void) {
 measure_time_t g_meas_MCTask;
 #define mc_TaskStart time_measure_start(&g_meas_MCTask)
 #define mc_TaskEnd time_measure_end(&g_meas_MCTask)
-extern u32 enc_pwm_err_ms;
-extern s16 enc_delta_err1, enc_delta_err2;
 void Sched_MC_mTask(void) {
 	static int vbus_err_cnt = 0;
 	static bool _sensorless_run = false;
@@ -1224,12 +1240,14 @@ void Sched_MC_mTask(void) {
 	bool sensor_less = !foc_observer_is_encoder();
 	if (mc_detect_vbus_mode() || (limted == FOC_LIM_CHANGE_L) || (!_sensorless_run && sensor_less)) {
 		mc_gear_vmode_changed();
-		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);
-		}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);
+		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);
+			}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);
+			}
 		}
 		motor.b_limit_pending = false;
 	}else if (limted == FOC_LIM_CHANGE_H) {

Applications/foc/motor/motor.h

@@ -103,7 +103,7 @@ static __INLINE float motor_encoder_get_angle(void) {
 #endif
 }
 
-static __INLINE bool motor_encoder_may_error(void) {
+static __INLINE u8 motor_encoder_may_error(void) {
 #ifdef USE_ENCODER_HALL
 	return false;
 #elif defined (USE_ENCODER_ABI)