错误处理保存和工具读取

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

Applications/app/app.c

@@ -17,6 +17,7 @@
 #include "foc/core/F_Calc.h"
 #include "foc/motor/motor_param.h"
 #include "foc/limit.h"
+#include "foc/mc_error.h"
 
 
 #ifdef CONFIG_DQ_STEP_RESPONSE
@@ -28,7 +29,7 @@ static u32 _app_report_task(void *args);
 static u32 _app_plot_task(void *args);
 //static u32 _app_trq_test_task(void *args);
 extern void PMSM_FOC_LogDebug(void);
-extern void err_code_log(void);
+extern void mc_err_code_log(void);
 extern void encoder_log(void);
 extern void sample_log(void);
 extern void thro_torque_log(void);
@@ -103,6 +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_init();
 #ifdef GD32_FOC_DEMO	
 	extern void key_init(void);
@@ -149,8 +151,8 @@ static u32 _app_report_task(void *p) {
 		//F_debug();
 		//eCtrl_debug_log();
 		sys_debug("enc err %d, %d\n", foc_observer_enc_errcount(), foc_observer_sensorless_stable());
+		mc_err_code_log();
 		sys_debug("=====\n");
-		//err_code_log();
 	}
 	return 200;
 }
@@ -199,6 +201,9 @@ 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();
+	}
 	fetch_jtag_cmd();
 	return 1;
 }

Applications/app/nv_storage.c

@@ -22,8 +22,8 @@
 #define limiter_cfg_idx_0 (sn_idx_back + 1)
 #define limiter_cfg_idx_1 (limiter_cfg_idx_0 + 1)
 
-#define mc_err_red_idx_0 (limiter_cfg_idx_1 + 1)
-#define mc_err_red_idx_1 (mc_err_red_idx_0 + 1)
+#define mc_runtime_red_idx (limiter_cfg_idx_1 + 1)
+#define mc_crit_err_idx (mc_runtime_red_idx + 1)
 
 static motor_params_t m_params;
 static foc_params_t   foc_params;
@@ -184,6 +184,23 @@ static void nv_default_gear_config(void) {
 
 }
 
+
+void nv_write_crit_errblock(u8 *data, int len) {
+	fmc_write_data(mc_crit_err_idx, data, len);
+}
+
+void nv_read_crit_errblock(u8 *data, int len) {
+	fmc_read_data(mc_crit_err_idx, data, len);
+}
+
+void nv_write_runtime_block(u8 *data, int len) {
+	fmc_write_data(mc_runtime_red_idx, data, len);
+}
+
+void nv_read_runtime_block(u8 *data, int len) {
+	fmc_read_data(mc_runtime_red_idx, data, len);
+}
+
 void nv_save_motor_params(void) {
 	u16 crc = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
 	m_params.crc16 = crc;
@@ -496,93 +513,6 @@ int nv_read_sn(u8 *data, int len) {
 	}
 	return 0;
 }
-static int _mc_err_r_id = mc_err_red_idx_0;
-static u8  _mc_err_r_idx = 0;
-static void mc_err_block_init(void) {
-	mc_err_red_t me0, me1;
-	fmc_read_data(mc_err_red_idx_0, (u8 *)&me0, sizeof(mc_err_red_t));
-	if (me0.len > (one_page_size - sizeof(mc_err_red_t))) {
-		return;
-	}
-	fmc_read_data(mc_err_red_idx_1, (u8 *)&me1, sizeof(mc_err_red_t));
-	if (me1.len > (one_page_size - sizeof(mc_err_red_t))) {
-		return;
-	}
-	if (me0.index > me1.index) {
-		if (me0.index == 255 && me1.index == 0) {
-			_mc_err_r_id = mc_err_red_idx_0;
-			_mc_err_r_idx = 1;
-		}else {
-			_mc_err_r_id = mc_err_red_idx_1;
-			_mc_err_r_idx = (me0.index + 1) % 255;
-		}
-	}else {
-		if (me0.index == 0 && me1.index == 255) {
-			_mc_err_r_id = mc_err_red_idx_1;
-			_mc_err_r_idx = 1;
-		}else {
-			_mc_err_r_id = mc_err_red_idx_0;
-			_mc_err_r_idx = (me1.index + 1) % 255;
-		}
-	}
-}
-
-
-void mc_err_block_write(u32 err_mask, u8 *data, int len) {
-	mc_err_red_t me;
-	me.index = _mc_err_r_idx;
-	me.len = (u16)len;
-	if (me.len > (one_page_size - sizeof(mc_err_red_t))) {
-		return;
-	}
-	me.err_mask = err_mask;
-	me.crc = crc16_get(data, me.len);
-	fmc_write_data(_mc_err_r_id, data, len);
-	_mc_err_r_idx = (me.index + 1) % 255;
-	if (_mc_err_r_id == mc_err_red_idx_0) {
-		_mc_err_r_id = mc_err_red_idx_1;
-	}else {
-		_mc_err_r_id = mc_err_red_idx_0;
-	}
-}
-
-int mc_error_block_read(int block, u32 *err_mask, u8 *data, int len) {
-	mc_err_red_t me;
-	int index = block?mc_err_red_idx_1:mc_err_red_idx_0;
-	fmc_read_data(index, (u8 *)&me, sizeof(mc_err_red_t));
-	fmc_read_data_with_offset(index, sizeof(mc_err_red_t), data, len);
-	if (me.crc != crc16_get(data, len)) {
-		return -1;
-	}
-	if (err_mask) {
-		*err_mask = me.err_mask;
-	}
-	return me.index;
-}
-
-int mc_error_curr_block_read(u32 *err_mask, u8 *data, int len) {
-	int idx0 = mc_error_block_read(0, err_mask, data, len);
-	int idx1 = mc_error_block_read(1, err_mask, data, len);
-	if (idx0 == -1 && idx1 == -1) {
-		return -1;
-	}
-	if (idx0 == -1) {
-		return idx1;
-	}
-	if (idx1 == -1) {
-		return mc_error_block_read(0, err_mask, data, len);
-	}
-	if (idx0 == 255 && idx1 == 0) {
-		return idx1;
-	}
-	if (idx0 == 0 && idx1 == 255) {
-		return mc_error_block_read(0, err_mask, data, len);
-	}
-	if (idx0 > idx1) {
-		return mc_error_block_read(0, err_mask, data, len);
-	}
-	return idx1;
-}
 
 void nv_storage_init(void) {
 	nv_read_motor_params();
@@ -611,8 +541,6 @@ void nv_storage_init(void) {
 #if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1
 	m_params.offset = 0.0f; //编码器做了零位置校准
 #endif
-	mc_err_block_init();
 	sys_debug("%f -- %f, flux: %f, %d\n", foc_params.n_currentBand, m_params.ld, m_params.flux_linkage, sizeof(m_params));
-	sys_debug("mc err %d, %d\n", _mc_err_r_id, _mc_err_r_idx);
 }
 

Applications/app/nv_storage.h

@@ -134,9 +134,10 @@ void* nv_get_limit_config(int *len);
 bool nv_set_gear_config(u8 mode96, u8 *config, int len);
 void nv_save_gear_configs(void);
 void* nv_get_gear_config(u8 mode96, int *len);
-void mc_err_block_write(u32 err_mask, u8 *data, int len);
-int mc_error_block_read(int block, u32 *err_mask, u8 *data, int len);
-int mc_error_curr_block_read(u32 *err_mask, u8 *data, int len);
+void nv_write_crit_errblock(u8 *data, int len);
+void nv_read_crit_errblock(u8 *data, int len);
+void nv_write_runtime_block(u8 *data, int len);
+void nv_read_runtime_block(u8 *data, int len);
 
 #endif /* _NV_Storage_H__ */
 

Applications/foc/commands.c

@@ -9,6 +9,7 @@
 #include "prot/can_foc_msg.h"
 #include "app/nv_storage.h"
 #include "foc/core/foc_observer.h"
+#include "foc/mc_error.h"
 
 #ifdef CONFIG_DQ_STEP_RESPONSE
 extern float target_d;
@@ -139,7 +140,7 @@ static bool _can_process_with_speed(u8 cmd) {
 
 static void process_foc_command(foc_cmd_body_t *command) {
 	u8 erroCode = 0;
-	u8 response[32];
+	u8 response[128];
 	int len = 3;
 	if ((command->ext_key != 0) && (command->cmd == 0)) {
 		process_ext_command(command);
@@ -602,6 +603,19 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			len += 18;
 			break;
 		}
+		case Foc_Get_MC_NV_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:
+		{
+			s16 offset = decode_s16((u8 *)command->data);
+			len += mc_runtime_get(offset, response+3, sizeof(response) - 3);
+			break;
+		}
+
 		default:
 		{
 			erroCode = FOC_Unknow_Cmd;

Applications/foc/commands.h

@@ -44,6 +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_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,

Applications/foc/core/PMSM_FOC_Core.h

@@ -66,6 +66,7 @@ typedef enum {
 	FOC_CRIT_Fan_Err,
 	FOC_CRIT_IDC_OV,
 	FOC_CRIT_THRO_Err,
+	FOC_CRIT_ENC_AB_Err,
 	FOC_CRIT_Err_Max = 32,
 }FOC_CritiCal_Ebit_t;
 

Applications/foc/limit.c

@@ -120,7 +120,7 @@ static u16 _motor_limit(void) {
 			curr_value = fclamp(curr_value, 0, prv_lim_value);
 			lim_value = (u16)(((float)gear->n_max_trq * curr_value) / 100.0f);
 			mc_set_motor_lim_level(i + 1);
-			err_add_record(FOC_CRIT_MOTOR_TEMP_Err, temp);
+			mc_crit_err_add_s16(FOC_CRIT_MOTOR_TEMP_Err, temp);
 			return lim_value;
 		}
 	}
@@ -158,7 +158,7 @@ static u16 _mos_limit(void) {
 			float curr_value = prv_lim_value - (float)(temp - lim->enter_pointer)/delta_tmp * delta_value;
 			curr_value = fclamp(curr_value, 0, prv_lim_value);
 			lim_value = (u16)(((float)gear->n_max_trq * curr_value) / 100.0f);
-			err_add_record(FOC_CRIT_MOS_TEMP_Err, temp);
+			mc_crit_err_add_s16(FOC_CRIT_MOS_TEMP_Err, temp);
 			mc_set_mos_lim_level(i + 1);
 			return lim_value;
 		}
@@ -197,7 +197,7 @@ u16 vbus_under_vol_limit(void) {
 		u16 lim_value = _vol_limiter(vol, lim);
 		if (lim_value != HW_LIMIT_NONE) {
 			mc_set_critical_error(FOC_CRIT_UN_Vol_Err);
-			err_add_record(FOC_CRIT_UN_Vol_Err, vol);
+			mc_crit_err_add_s16(FOC_CRIT_UN_Vol_Err, vol);
 			return lim_value;
 		}
 	}

Applications/foc/mc_error.c

@@ -1,31 +1,141 @@
-#include "foc/mc_error.h"
-#include "foc/core/PMSM_FOC_Core.h"
-#include "libs/logger.h"
-
-static err_record_t err[FOC_CRIT_Err_Max];
-
-void err_add_record(u8 err_code, s16 err_value) {
-	if (err_code >= FOC_CRIT_Err_Max) {
-		return;
-	}
-	err[err_code].err_value = err_value;
-	err[err_code].err_time = get_tick_ms();
-}
-
-int err_get_record(u8 index, int size, err_record_t *err_out) {
-	size = min(size, (FOC_CRIT_Err_Max -  index));
-	for (int i = index; i < index + size; i++) {
-		err_out[i].err_value = err[i].err_value;
-		err_out[i].err_time = err[i].err_time;
-	}
-	return size;
-}
-
-void err_code_log(void) {
-	for (int i = 0;  i < FOC_CRIT_Err_Max; i++) {
-		if (err[i].err_time == 0) {
-			continue;
-		}
-		sys_error("err:%d, time = %d\n", err[i].err_value, err[i].err_time);
-	}
-}
+#include "foc/mc_error.h"
+#include "libs/logger.h"
+#include "app/nv_storage.h"
+#include "libs/crc16.h"
+
+static u8 page_buff[one_page_size];
+static u8 page_buff_runtime[one_page_size];
+static err_nv_t *_e_nv = NULL;
+static runtime_nv_t *_r_nv = NULL;
+static bool _w_pending = false;
+static bool _w_pending_rt = false;
+#define ERR_MAX_SIZE 200
+#define RUNTIME_MAX_SIZE 20
+
+void mc_crit_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));
+	if (crc16 != _e_nv->crc) {
+		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);
+	}
+
+	_r_nv = (runtime_nv_t *)page_buff_runtime;
+	nv_read_runtime_block(page_buff_runtime, one_page_size);
+	crc16 = crc16_get((u8 *)_r_nv->node, one_page_size - sizeof(runtime_nv_t));
+	if (crc16 != _r_nv->crc) {
+		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 err p: 0x%x\n", _e_nv->node);
+}
+
+void mc_crit_err_add(u8 code, s16 value1, s16 value2) {
+	err_node_t *node = _e_nv->node;
+	node = node + _e_nv->tail;
+	node->idx = _e_nv->tail;
+	node->err_code = code;
+	node->value1 = value1;
+	node->value2 = value2;
+	_e_nv->tail = (_e_nv->tail + 1) % ERR_MAX_SIZE; //next add index
+	_e_nv->count++;
+	if (_e_nv->count > ERR_MAX_SIZE) {
+		_e_nv->count = ERR_MAX_SIZE;
+	}
+	_w_pending = true;
+}
+
+void mc_crit_err_add_s16(u8 code, s16 value) {
+	mc_crit_err_add(code, value, 0);
+}
+
+
+void mc_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));
+	node->idx = _r_nv->tail;
+	_r_nv->tail = (_r_nv->tail + 1) % RUNTIME_MAX_SIZE; //next add index
+	_r_nv->count++;
+	if (_r_nv->count > RUNTIME_MAX_SIZE) {
+		_r_nv->count = RUNTIME_MAX_SIZE;
+	}
+	_w_pending_rt = true;
+}
+
+
+void mc_crit_err_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);
+		_w_pending = false;
+	}
+	if (_w_pending_rt) {
+		_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);
+		_w_pending_rt = false;
+	}
+}
+
+int mc_crit_err_get(s16 offset, u8 *buff, int len) {
+	int count = len / sizeof(err_node_t);
+	int index = offset;
+	u8 *p_remain = buff++;
+	if (_e_nv->count == ERR_MAX_SIZE) {
+		index = (index + _e_nv->tail) % ERR_MAX_SIZE;
+	}
+	int remain = (offset > _e_nv->count)?0:(_e_nv->count - offset);
+	count = min(count, remain);
+	err_node_t *node = _e_nv->node;
+	len = 0;
+	*p_remain = count;
+	len ++;
+	while (count--) {
+		memcpy(buff, (u8 *)(node + index), sizeof(err_node_t));
+		buff += sizeof(err_node_t);
+		index = (index + 1) % ERR_MAX_SIZE;
+		len += sizeof(err_node_t);
+	}
+
+	return len;
+
+}
+
+
+int mc_runtime_get(s16 offset, u8 *buff, int len) {
+	int count = len / sizeof(runtime_node_t);
+	int index = offset;
+	u8 *p_remain = buff++;
+	if (_r_nv->count == RUNTIME_MAX_SIZE) {
+		index = (index + _r_nv->tail) % RUNTIME_MAX_SIZE;
+	}
+	int remain = (offset > _r_nv->count)?0:(_r_nv->count - offset);
+	count = min(count, remain);
+	runtime_node_t *node = _r_nv->node;
+	len = 0;
+	*p_remain = count;
+	len ++;
+	while (count--) {
+		memcpy(buff, (u8 *)(node + index), sizeof(runtime_node_t));
+		buff += sizeof(runtime_node_t);
+		index = (index + 1) % RUNTIME_MAX_SIZE;
+		len += sizeof(runtime_node_t);
+	}
+
+	return len;
+}
+
+
+void mc_err_code_log(void) {
+	if (_e_nv->count > 0) {
+		sys_debug("mc err count %d, tail %d\n", _e_nv->count, _e_nv->tail);
+	}
+	if (_r_nv->count > 0) {
+		sys_debug("mc rt count %d, tail %d\n", _r_nv->count, _r_nv->tail);
+	}
+}
+

Applications/foc/mc_error.h

@@ -2,14 +2,64 @@
 #define _MC_ERROR_H__
 #include "os/os_types.h"
 
-#pragma  pack (push,1)
 typedef struct {
 	s16 err_value;
 	u32 err_time;
 }err_record_t;
+
+#pragma  pack (push,1)
+typedef struct {
+	u8 idx;
+	u8 err_code;
+	s16 value1;
+	s16 value2;
+}err_node_t;
+
+typedef struct {
+	u8    idx;
+	s16   vbus_x10;
+	s16   ibus_x10;
+	s16   vacc_x10;
+	s16   id_x10;
+	s16   iq_x10;
+	s16   vd_x10;
+	s16   vq_x10;
+	s16   id_ref_x10;
+	s16   iq_ref_x10;
+	s16   torque_ref_x10;
+	u8    run_mode;
+	s16   rpm;
+	u8    b_sensorless;
+	u8    b_sensorless_stable;
+	s16   sensorless_rpm;
+	s16   mos_temp;
+	s16   mot_temp;
+	u8    enc_error;
+}runtime_node_t;
+
 #pragma pack(pop)
 
-void err_add_record(u8 err_code, s16 err_value);
+typedef struct{
+	u8 tail;
+	u8 count;
+	u16 crc;
+	err_node_t node[0];
+}err_nv_t;
+
+typedef struct{
+	u8 tail;
+	u8 count;
+	u16 crc;
+	runtime_node_t node[0];
+}runtime_nv_t;
+
+void mc_crit_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);
+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);
 
 #endif /*_MC_ERROR_H__ */
 

Applications/foc/motor/current_ics.c

@@ -123,11 +123,11 @@ bool phase_current_offset(void) {
 
 bool phase_curr_offset_check(void) {
 	if ((g_cs.adc_offset_b > ADC_FULL_MAX/2 + 100) || (g_cs.adc_offset_c > ADC_FULL_MAX/2 + 100)) {
-		err_add_record(FOC_CRIT_CURR_OFF_Err, MAX(g_cs.adc_offset_c, g_cs.adc_offset_b));
+		mc_crit_err_add_s16(FOC_CRIT_CURR_OFF_Err, MAX(g_cs.adc_offset_c, g_cs.adc_offset_b));
 		return true;
 	}
 	if ((g_cs.adc_offset_b < ADC_FULL_MAX/2 - 100) || (g_cs.adc_offset_c < ADC_FULL_MAX/2 - 100)) {
-		err_add_record(FOC_CRIT_CURR_OFF_Err, min(g_cs.adc_offset_c, g_cs.adc_offset_b));
+		mc_crit_err_add_s16(FOC_CRIT_CURR_OFF_Err, min(g_cs.adc_offset_c, g_cs.adc_offset_b));
 		return true;
 	}
 	return false;

Applications/foc/motor/encoder.c

@@ -80,7 +80,7 @@ void encoder_init_clear(void) {
 	g_encoder.b_cali_err = false;
 	g_encoder.produce_error = false;
 	g_encoder.last_delta_cnt = MAX_S16;
-	g_encoder.enc_maybe_err = false;
+	g_encoder.enc_maybe_err = ENCODER_NO_ERR;
 	g_encoder.start_dianostic_cnt = 0;
 	g_encoder.pwm_time_ms = get_tick_ms();
 	_init_pll();
@@ -146,11 +146,15 @@ static __INLINE float _eccentricity_compensation(int cnt) {
 #endif
 }
 
-#define CONFIG_ENC_DIANOSTIC_MIN_CNT (10.0F * ENC_MAX_RES * FOC_CTRL_US) //600rpm
-
+#define CONFIG_ENC_DETECT_ERR
+#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;
+s16 enc_delta_err2 = 0;
 static void encoder_detect_error(u32 cnt) {
 #ifdef CONFIG_ENC_DETECT_ERR
-	if (!g_encoder.enc_maybe_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) {
@@ -166,33 +170,51 @@ static void encoder_detect_error(u32 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) {
-			g_encoder.enc_maybe_err = true;
+			g_encoder.enc_maybe_err = ENCODER_PWM_ERR;
+			enc_pwm_err_ms = get_delta_ms(g_encoder.pwm_time_ms);
 		}
 		if (g_encoder.start_dianostic_cnt < 0xFFFF) {
 			g_encoder.start_dianostic_cnt ++;
 		}
-		if (!skip && (g_encoder.last_delta_cnt > CONFIG_ENC_DIANOSTIC_MIN_CNT) && (g_encoder.start_dianostic_cnt >= 1000)) {
+		if (!skip && ((g_encoder.last_delta_cnt > CONFIG_ENC_DIANOSTIC_MIN_CNT) || (_mayerr_cnt != 0)) && (g_encoder.start_dianostic_cnt >= 1000)) {
 			float last_delta = (float)g_encoder.last_delta_cnt;
 			float r = (float)delta_cnt / (last_delta + 0.0000001f);
 			r = ABS(r);
 			float r_thr;
-			if (g_encoder.last_delta_cnt <= CONFIG_ENC_DIANOSTIC_MIN_CNT * 2) {
+			u32   cnt_thr;
+			if (g_encoder.last_delta_cnt <= CONFIG_ENC_DIANOSTIC_MIN_CNT * 2) { //0.4个机械角度
 				r_thr = 0.3f;
-			}else if (g_encoder.last_delta_cnt <= CONFIG_ENC_DIANOSTIC_MIN_CNT * 4) {
+				cnt_thr = 4;
+			}else if (g_encoder.last_delta_cnt <= CONFIG_ENC_DIANOSTIC_MIN_CNT * 4) { //0.8个机械角度
 				r_thr = 0.5f;
-			}else if (g_encoder.last_delta_cnt <= CONFIG_ENC_DIANOSTIC_MIN_CNT * 6) {
+				cnt_thr = 3;
+			}else if (g_encoder.last_delta_cnt <= CONFIG_ENC_DIANOSTIC_MIN_CNT * 6) { //1.3个机械角度
 				r_thr = 0.6f;
+				cnt_thr = 2;
 			}else {
 				r_thr = 0.7f;
+				cnt_thr = 1;
+			}
+			if (r <= 0.01f) {
+				cnt_thr = 1;
 			}
 			if (r <= r_thr || r >= (2.0f - r_thr)) {
-				g_encoder.enc_maybe_err = true;
+				_mayerr_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;
+				}
+			}else {
+				_mayerr_cnt = 0;
 			}
+		}else {
+			_mayerr_cnt = 0;
 		}
 		g_encoder.last_delta_cnt = delta_cnt;
 	}
 #else
-	g_encoder.enc_maybe_err = false;
+	g_encoder.enc_maybe_err = ENCODER_NO_ERR;
 #endif
 }
 
@@ -242,11 +264,14 @@ void encoder_produce_error(bool error) {
 	g_encoder.produce_error = error;
 }
 
-bool encoder_may_error(void) {
+u8 encoder_may_error(void) {
 	return g_encoder.enc_maybe_err;
 }
 
 float encoder_get_speed(void) {
+	if (g_encoder.enc_maybe_err != ENCODER_NO_ERR) {
+		return 0;
+	}
 	return (g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f;
 }
 

Applications/foc/motor/encoder.h

@@ -31,10 +31,14 @@ typedef struct {
 	bool  produce_error;
 	s16   last_delta_cnt;
 	u16   start_dianostic_cnt;
-	bool  enc_maybe_err;
+	u8    enc_maybe_err;
 	u32   pwm_time_ms;
 }encoder_t;
 
+#define ENCODER_NO_ERR 0
+#define ENCODER_PWM_ERR 1
+#define ENCODER_AB_ERR  2
+
 void encoder_init(void);
 void encoder_init_clear(void);
 float encoder_get_theta(void);
@@ -52,7 +56,7 @@ u32 encoder_get_cnt_offset(void);
 bool encoder_get_cali_error(void);
 void encoder_epm_pll_band(bool epm);
 void encoder_produce_error(bool error);
-bool encoder_may_error(void);
+u8 encoder_may_error(void);
 
 #endif /* _Encoder_H__ */
 

Applications/foc/motor/motor.c

@@ -27,8 +27,6 @@ static bool mc_detect_hwbrake(void);
 static bool mc_is_gpio_mlock(void);
 static void _pwm_brake_prot_timer_handler(shark_timer_t *);
 static shark_timer_t _brake_prot_timer = TIMER_INIT(_brake_prot_timer, _pwm_brake_prot_timer_handler);
-static void _autohold_beep_timer_handler(shark_timer_t *);
-static shark_timer_t _autohold_beep_timer = TIMER_INIT(_autohold_beep_timer, _autohold_beep_timer_handler);
 static void _fan_det_timer_handler(shark_timer_t *);
 static shark_timer_t _fan_det_timer1 = TIMER_INIT(_fan_det_timer1, _fan_det_timer_handler);
 static shark_timer_t _fan_det_timer2 = TIMER_INIT(_fan_det_timer2, _fan_det_timer_handler);
@@ -54,7 +52,7 @@ static mc_gear_t sensorless_gear = {
 	.n_accl_time = 1500,
 	.n_torque = {100, 100, 100, 100, 0, 0, 0, 0, 0, 0},
 };
-static motor_err_t mc_error;
+static runtime_node_t mc_error;
 
 static void MC_Check_MosVbusThrottle(void) {
 	int count = 1000;
@@ -120,7 +118,7 @@ static void MC_Check_MosVbusThrottle(void) {
 
 static u32 _self_check_task(void *p) {
 	if (ENC_Check_error()) {
-		err_add_record(FOC_CRIT_Encoder_Err, 0);
+		mc_crit_err_add_s16(FOC_CRIT_Encoder_Err, 0);
 		mc_set_critical_error(FOC_CRIT_Encoder_Err);
 	}
 	if (get_tick_ms() < 5000) { //启动后5s内检测锁电机线
@@ -913,7 +911,6 @@ void MC_Brake_IRQHandler(void) {
 
 static void _pwm_brake_prot_timer_handler(shark_timer_t *t){
 	pwm_brake_enable(true);
-	mc_err_block_write(motor.n_CritiCalErrMask, (u8 *)&mc_error, sizeof(mc_error));
 	sys_debug("MC protect error\n");
 }
 
@@ -935,10 +932,12 @@ void MC_Protect_IRQHandler(void){
 	mc_error.torque_ref_x10 = (s16)(PMSM_FOC_Get()->in.s_targetTorque * 10.0f);
 	mc_error.run_mode = PMSM_FOC_Get()->out.n_RunMode;
 	mc_error.rpm = (s16)motor_encoder_get_speed();
-	mc_error.b_smo_running = !foc_observer_is_encoder();
+	mc_error.b_sensorless = !foc_observer_is_encoder();
+	mc_error.b_sensorless_stable = foc_observer_sensorless_stable();
 	mc_error.mos_temp = get_mos_temp();
 	mc_error.mot_temp = get_motor_temp();
-	mc_error.enc_error = foc_observer_enc_errcount();
+	mc_error.enc_error = motor_encoder_may_error();
+	mc_error.sensorless_rpm = (s16)foc_observer_sensorless_speed();
 
 	mc_set_critical_error(FOC_CRIT_Phase_Err);
 	_mc_internal_init(CTRL_MODE_OPEN, false);
@@ -946,6 +945,7 @@ void MC_Protect_IRQHandler(void){
 	pwm_stop();
 	PMSM_FOC_Stop();
 	pwm_up_enable(true);
+	mc_runtime_add(&mc_error);
 }
 
 void motor_debug(void) {
@@ -954,62 +954,7 @@ void motor_debug(void) {
 	}
 	sys_debug("err1: %f, %f, %f, %d\n", (float)mc_error.vbus_x10/10.0f, (float)mc_error.id_ref_x10/10.0f, (float)mc_error.iq_ref_x10/10.0f, mc_error.run_mode);
 	sys_debug("err2: %f, %f, %f, %f\n", (float)mc_error.id_x10/10.0f, (float)mc_error.iq_x10/10.0f, (float)mc_error.vd_x10/10.0f, (float)mc_error.vq_x10/10.0f);
-	sys_debug("err3: %f, %d, %d, %d, %d\n", (float)mc_error.ibus_x10/10.0f, mc_error.b_smo_running, mc_error.mos_temp, mc_error.mot_temp, mc_error.enc_error);
-}
-
-int _mc_get_phase_errinfo(motor_err_t *mc_err, u8 *data, int dlen) {
-	int len = 0;
-	encode_s16(data, mc_err->vbus_x10);
-	len += 2;
-	encode_s16(data, mc_err->ibus_x10);
-	len += 2;
-	encode_s16(data, mc_err->vacc_x10);
-	len += 2;
-	encode_s16(data, mc_err->id_ref_x10);
-	len += 2;
-	encode_s16(data, mc_err->iq_ref_x10);
-	len += 2;
-	encode_s16(data, mc_err->id_x10);
-	len += 2;
-	encode_s16(data, mc_err->iq_x10);
-	len += 2;
-	encode_s16(data, mc_err->vd_x10);
-	len += 2;
-	encode_s16(data, mc_err->vq_x10);
-	len += 2;
-	encode_s16(data, mc_err->torque_ref_x10);
-	len += 2;
-	encode_s16(data, mc_err->rpm);
-	len += 2;
-	encode_u8(data, mc_err->run_mode);
-	len += 1;
-	encode_u8(data, mc_err->b_smo_running);
-	len += 1;
-	encode_s16(data, mc_err->mos_temp);
-	len += 2;
-	encode_s16(data, mc_err->mot_temp);
-	len += 2;
-	encode_s16(data, mc_err->enc_error);
-	len += 2;
-
-	return len;
-}
-
-int mc_get_phase_errinfo(u8 *data, int dlen) {
-	return _mc_get_phase_errinfo(&mc_error, data, dlen);
-}
-
-int mc_get_phase_errinfo_block(u8 *data, int dlen) {
-	motor_err_t mc_err;
-	u32 err_mask;
-	if (mc_error_curr_block_read(&err_mask, (u8 *)&mc_err, sizeof(mc_err)) < 0) {
-		encode_u32(data, motor.n_CritiCalErrMask);
-		data += 4;
-		return _mc_get_phase_errinfo(&mc_error, data, dlen - 4) + 4;
-	}
-	encode_u32(data, err_mask);
-	data += 4;
-	return _mc_get_phase_errinfo(&mc_err, data, dlen - 4) + 4;
+	sys_debug("err3: %f, %d, %d, %d, %d\n", (float)mc_error.ibus_x10/10.0f, mc_error.sensorless_rpm, mc_error.mos_temp, mc_error.mot_temp, mc_error.enc_error);
 }
 
 void TIMER_UP_IRQHandler(void){
@@ -1075,10 +1020,6 @@ static bool mc_run_stall_process(u8 run_mode) {
 	return false;
 }
 
-static void _autohold_beep_timer_handler(shark_timer_t *t) {
-	gpio_beep(60);
-}
-
 static void mc_autohold_process(void) {
 	if (nv_get_foc_params()->n_autoHold == 0) {
 		if (PMSM_FOC_AutoHoldding()) {
@@ -1103,7 +1044,6 @@ static void mc_autohold_process(void) {
 			if (get_delta_ms(motor.n_autohold_time) >= CONFIG_AUTOHOLD_DETECT_TIME) {
 				if (mc_auto_hold(true)) {
 					motor.b_wait_brk_release = true;
-					shark_timer_post(&_autohold_beep_timer, 0);
 				}
 			}
 		}
@@ -1246,6 +1186,8 @@ 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;
@@ -1282,7 +1224,13 @@ 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();
-		mc_set_critical_error(FOC_CRIT_Encoder_Err);
+		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) {
 		motor.b_limit_pending = true;

Applications/foc/motor/motor.h

@@ -53,25 +53,6 @@ typedef struct {
 	fan_t  fan[2];
 }motor_t;
 
-typedef struct {
-	s16 vbus_x10;
-	s16 ibus_x10;
-	s16 vacc_x10;
-	s16 id_x10;
-	s16 iq_x10;
-	s16 vd_x10;
-	s16 vq_x10;
-	s16 id_ref_x10;
-	s16 iq_ref_x10;
-	u8  run_mode;
-	s16 torque_ref_x10;
-	s16 rpm;
-	bool  b_smo_running;
-	s16    mos_temp;
-	s16    mot_temp;
-	u16    enc_error;
-}motor_err_t;
-
 motor_t * mc_params(void);
 void mc_init(void);
 bool mc_start(u8 mode);