1. 当相错误的时候，记录mc_error到flash中

2. ladrc观测器切换的时候，通过观测器的速度判断角度误差（编码器的速度带宽较低）
3. 修改空转时候的限速和速度环参数

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

Applications/app/app.c

@@ -127,9 +127,9 @@ static u32 _app_report_task(void *p) {
 	can_report_phase_current(0x45);
 	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("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());
 		//sys_debug("throttle %f\n", get_throttle_float());
 		//sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());
@@ -137,30 +137,35 @@ static u32 _app_report_task(void *p) {
 		//sys_debug("Sensorless err %d\n", foc_observer_enc_errcount());
 		//thro_torque_log();
 		//sys_debug("_>%f, %f, %f\n", ladrc_observer_get()->ld, ladrc_observer_get()->lq, ladrc_observer_get()->poles);
-		//encoder_log();
-		motor_debug();
-		sample_log();
+		encoder_log();
+		//motor_debug();
+		//sample_log();
 		PMSM_FOC_LogDebug();
 		//F_debug();
 		//eCtrl_debug_log();
+		sys_debug("enc err %d\n", foc_observer_enc_errcount());
 		sys_debug("=====\n");
 		//err_code_log();
 	}
 	return 200;
 }
-int plot_type = 1;
+int plot_type = 0;
 static void plot_smo_angle(void) {
+#if 0
 	float smo_angle = foc_observer_sensorless_angle();
 	float delta = smo_angle - PMSM_FOC_Get()->in.s_motAngle;
 	float s, c;
 	arm_sin_cos_f32(delta, &s, &c);
 	delta = fast_atan2(s, c)/PI*180.0f;
 	can_plot3(PMSM_FOC_Get()->in.s_motAngle, smo_angle, delta);
+#else
+	can_plot2((s16)(foc_observer_sensorless_diff() * 100.0f), (s16)(foc_observer_sensorless_ration() * 100.0f));
+#endif
 }
 static u32 _app_plot_task(void * args) {
 	if (plot_type == 1) {
-		//can_plot2((s16)foc_observer_sensorless_speed(), (s16)PMSM_FOC_GetSpeed());
-		can_plot3((s16)PMSM_FOC_GetSpeed() + 1000, (s16)PMSM_FOC_GetSpeed(), (s16)PMSM_FOC_GetSpeed() - 1000);
+		can_plot2((s16)foc_observer_sensorless_speed(), (s16)PMSM_FOC_GetSpeed());
+		//can_plot3((s16)PMSM_FOC_GetSpeed() + 1000, (s16)PMSM_FOC_GetSpeed(), (s16)PMSM_FOC_GetSpeed() - 1000);
 	}else if (plot_type == 2) {
 		can_plot2(eCtrl_get_FinalTorque(), PMSM_FOC_Get()->in.s_targetTorque);
 	}else if (plot_type == 3) {

Applications/app/nv_storage.c

@@ -5,6 +5,26 @@
 #include "foc/motor/motor_param.h"
 #include "foc/foc_config.h"
 
+#define motorParam_idx_0 (sn_page_index + 1)
+#define motorParam_idx_1 (motorParam_idx_0 + 1)
+#define focParam_idx_0 (motorParam_idx_1 + 1)
+#define focParam_idx_1 (focParam_idx_0 + 1)
+
+#define trq_Tbl_size (4)
+#define trq_Tbl_idx0 (focParam_idx_1 + 1)
+#define trq_Tbl_idx1 (trq_Tbl_idx0 + trq_Tbl_size)
+
+#define gear_config_idx_0 (trq_Tbl_idx1 + trq_Tbl_size)
+#define gear_config_idx_1 (gear_config_idx_0 + 1)
+
+#define sn_idx_back       (gear_config_idx_1 + 1)
+
+#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)
+
 static motor_params_t m_params;
 static foc_params_t   foc_params;
 static mc_gear_config_t gear_config;
@@ -475,7 +495,93 @@ 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();
@@ -503,6 +609,8 @@ void nv_storage_init(void) {
 #if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1
 	m_params.offset = 0.0f; //编码器做了零位置校准
 #endif
-	sys_debug("%f -- %f, flux: %f, %d\n", foc_params.n_currentBand, m_params.ld, m_params.flux_linkage, sizeof(m_params));
+	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

@@ -103,23 +103,13 @@ typedef struct {
 	u16 crc;
 }mc_sn_t;
 
-
-#define motorParam_idx_0 3
-#define motorParam_idx_1 (motorParam_idx_0 + 1)
-#define focParam_idx_0 (motorParam_idx_1 + 1)
-#define focParam_idx_1 (focParam_idx_0 + 1)
-
-#define trq_Tbl_size (4)
-#define trq_Tbl_idx0 (focParam_idx_1 + 1)
-#define trq_Tbl_idx1 (trq_Tbl_idx0 + trq_Tbl_size)
-
-#define gear_config_idx_0 (trq_Tbl_idx1 + trq_Tbl_size)
-#define gear_config_idx_1 (gear_config_idx_0 + 1)
-
-#define sn_idx_back       (gear_config_idx_1 + 1)
-
-#define limiter_cfg_idx_0 (sn_idx_back + 1)
-#define limiter_cfg_idx_1 (limiter_cfg_idx_0 + 1)
+typedef struct {
+	u8 index;
+	u16 crc;
+	u16 len;
+	u32 err_mask;
+	u8 data[0]; //just a tag, no memeory alloc
+}mc_err_red_t;
 
 void nv_storage_init(void);
 motor_params_t *nv_get_motor_params(void);
@@ -141,10 +131,12 @@ int nv_read_sn(u8 *data, int len);
 bool nv_set_limit_config(u8 *config, int len);
 void nv_save_limit_config(void);
 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);
 
 #endif /* _NV_Storage_H__ */
 

Applications/bsp/gd32/fmc_flash.c

@@ -39,6 +39,11 @@ void fmc_read_data(int index, uint8_t *data, int len){
 	_fmc_read_data(_data_addr(index), data, len);
 }
 
+void fmc_read_data_with_offset(int index, u32 off, uint8_t *data, int len) {
+	uint32_t addr = _data_addr(index) + off;
+	_fmc_read_data(addr, data, len);
+}
+
 static __inline__ void _fmc_flag_clear(void) {
 	fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_WPERR | FMC_FLAG_END);
 }

Applications/bsp/gd32/fmc_flash.h

@@ -4,8 +4,6 @@
 
 #define one_page_size 2048
 
-#define data_bk_page_index 4
-#define data_page_index 3
 #define sn_page_index 2
 #define magic_page_index 1 //must is the last page in 256K eara
 
@@ -20,6 +18,7 @@ uint32_t fmc_get_addr(int page);
 void fmc_write_trq_table_begin(int addr);
 void fmc_write_trq_table_continue(const u8 *data, int len);
 void fmc_write_trq_table_end(void);
+void fmc_read_data_with_offset(int index, u32 off, uint8_t *data, int len);
 
 #endif /* _FMC_FLASH_H__ */
 

Applications/foc/core/foc_observer.c

@@ -40,38 +40,33 @@ float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta){
 }
 
 bool foc_observer_diagnostic(float enc_angle, float enc_vel) {
-	if (enc_vel <= 50.0f) {
+	if (enc_vel <= (foc_observer_sensorless_working_speed() - 100.0f)) {
 		foc_obser.enc_angle = enc_angle;
 		foc_obser.enc_speed = ABS(enc_vel);
 		return true;
 	}
 	if (!foc_obser.is_sensorless_running) {
-		float delta_angle_est = RPM_2_degree((foc_obser.enc_speed + ABS(enc_vel))/2.0f);
-		float delta_angle_enc = enc_angle - foc_obser.enc_angle;
-		delta_angle_enc = ABS(delta_angle_enc);
-		if (delta_angle_enc > 180.0f) {
-			delta_angle_enc = 360.0f - delta_angle_enc;
-		}
-		float r = ABS(delta_angle_enc - delta_angle_est)/(delta_angle_est + 1e-20f);
-		float thro = 2.0f;
-		if (delta_angle_est < 0.5f) {
-			thro = 6.0f;
-		}else if (delta_angle_est < 2.0f) {
-			thro = 2.0f;
-		}else if (delta_angle_est < 4.0f) {
-			thro = 1.0f;
-		}else if (delta_angle_est < 6.0f) {
-			thro = 0.7f;
-		}else if (delta_angle_est < 10.0f) {
-			thro = 0.5f;
+		float est_diff = RPM_2_degree(foc_obser.sensorless_speed);
+		float enc_angle_est = est_diff + foc_obser.enc_angle;
+		rand_angle(enc_angle_est);
+		float real_est_diff = enc_angle - enc_angle_est;
+		float s, c;
+		arm_sin_cos_f32(real_est_diff, &s, &c);
+		real_est_diff = fast_atan2(s, c)/PI*180.0f;
+		float ration = ABS(real_est_diff)/est_diff;
+		float ration_thrl = 1.0f;
+		if (est_diff < 3.0f) {
+			ration_thrl = 0.5f;
+		}else if (est_diff < 6.0f) {
+			ration_thrl = 0.4;
 		}else {
-			thro = 0.4f;
+			ration_thrl = 0.3f;
 		}
-		if (r >= thro) {
-			if (foc_obser.enc_err_cnt < 65535) {
-				foc_obser.enc_err_cnt ++;
-			}
+		if (ration > ration_thrl) {
+			foc_obser.enc_err_cnt++;
 		}
+		foc_obser.real_est_diff = real_est_diff;
+		foc_obser.real_est_ration = ration;
 	}
 	foc_obser.enc_angle = enc_angle;
 	foc_obser.enc_speed = ABS(enc_vel);
@@ -108,6 +103,14 @@ u16 foc_observer_enc_errcount(void) {
 	return foc_obser.enc_err_cnt;
 }
 
+float foc_observer_sensorless_diff(void) {
+	return foc_obser.real_est_diff;
+}
+
+float foc_observer_sensorless_ration(void) {
+	return foc_obser.real_est_ration;
+}
+
 float foc_observer_sensorless_working_speed(void) {
 #ifdef CONFIG_SMO_OBSERVER
 	return CONFIG_SMO_MIN_SPEED;

Applications/foc/core/foc_observer.h

@@ -13,6 +13,8 @@ typedef struct {
 	float sensorless_est_angle; //通过smo速度，估计当前角度
 	bool  is_sensorless_enable;
 	bool  is_sensorless_running;
+	float real_est_diff;
+	float real_est_ration;
 	u16   enc_err_cnt;  //融合系数
 }foc_observer_t;
 void foc_observer_init(void);
@@ -26,6 +28,8 @@ float foc_observer_sensorless_angle(void);
 float foc_observer_sensorless_speed(void);
 float foc_observer_sensorless_working_speed(void);
 u16 foc_observer_enc_errcount(void);
+float foc_observer_sensorless_diff(void);
+float foc_observer_sensorless_ration(void);
 
 #endif /*_FOC_OBSERVER_H__*/
 

Applications/foc/foc_config.h

@@ -70,8 +70,8 @@
 	#define CONFIG_LADRC_Wcv 5.0F
 	#define CONFIG_LADRC_B0  2500.0F
 
-	#define CONFIG_LADRC_NOLOAD_Wcv 3.0F
-	#define CONFIG_LADRC_NOLOAD_B0  2500.0F
+	#define CONFIG_LADRC_NOLOAD_Wcv 2.5F
+	#define CONFIG_LADRC_NOLOAD_B0  3000.0F
 
 	#define CONFIG_LADRC_EPM_Wcv 3.0F
 	#define CONFIG_LADRC_EPMBK_B0	 500.0F

Applications/foc/motor/motor.c

@@ -945,6 +945,7 @@ 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");
 }
 
@@ -988,41 +989,61 @@ void motor_debug(void) {
 	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(u8 *data, int dlen) {
+int _mc_get_phase_errinfo(motor_err_t *mc_err, u8 *data, int dlen) {
 	int len = 0;
-	encode_s16(data, mc_error.vbus_x10);
+	encode_s16(data, mc_err->vbus_x10);
 	len += 2;
-	encode_s16(data, mc_error.ibus_x10);
+	encode_s16(data, mc_err->ibus_x10);
 	len += 2;
-	encode_s16(data, mc_error.vacc_x10);
+	encode_s16(data, mc_err->vacc_x10);
 	len += 2;
-	encode_s16(data, mc_error.id_ref_x10);
+	encode_s16(data, mc_err->id_ref_x10);
 	len += 2;
-	encode_s16(data, mc_error.iq_ref_x10);
+	encode_s16(data, mc_err->iq_ref_x10);
 	len += 2;
-	encode_s16(data, mc_error.id_x10);
+	encode_s16(data, mc_err->id_x10);
 	len += 2;
-	encode_s16(data, mc_error.iq_x10);
+	encode_s16(data, mc_err->iq_x10);
 	len += 2;
-	encode_s16(data, mc_error.vd_x10);
+	encode_s16(data, mc_err->vd_x10);
 	len += 2;
-	encode_s16(data, mc_error.vq_x10);
+	encode_s16(data, mc_err->vq_x10);
 	len += 2;
-	encode_s16(data, mc_error.torque_ref_x10);
+	encode_s16(data, mc_err->torque_ref_x10);
 	len += 2;
-	encode_s16(data, mc_error.rpm);
+	encode_s16(data, mc_err->rpm);
 	len += 2;
-	encode_u8(data, mc_error.run_mode);
+	encode_u8(data, mc_err->run_mode);
 	len += 1;
-	encode_u8(data, mc_error.b_smo_running);
+	encode_u8(data, mc_err->b_smo_running);
 	len += 1;
-	encode_s16(data, mc_error.mos_temp);
+	encode_s16(data, mc_err->mos_temp);
+	len += 2;
+	encode_s16(data, mc_err->mot_temp);
 	len += 2;
-	encode_s16(data, mc_error.mot_temp);
+	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;
+}
+
 void TIMER_UP_IRQHandler(void){
 	if (!motor.b_start && !PMSM_FOC_Is_Start()) {
 		motor_encoder_update();

Applications/foc/motor/motor.h

@@ -107,10 +107,10 @@ bool mc_set_cruise_speed(bool rpm_abs, float target_rpm);
 void mc_set_idc_limit(s16 limit);
 mc_gear_t *mc_get_gear_config(void);
 mc_gear_t *mc_get_gear_config_by_gear(u8 n_gear);
-int mc_get_phase_errinfo(u8 *data, int dlen);
 void mc_set_motor_lim_level(u8 l);
 void mc_set_mos_lim_level(u8 l);
 void motor_debug(void);
+int mc_get_phase_errinfo_block(u8 *data, int dlen);
 
 static __INLINE float motor_encoder_get_angle(void) {
 #ifdef USE_ENCODER_HALL