编码器监测放到encoder.c中，低速通过pwm的频率检测，中高速通过上一次的cnt和当前cnt的比较来检测

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

Applications/app/app.c

@@ -140,7 +140,6 @@ static u32 _app_report_task(void *p) {
 		//sys_debug("throttle %f\n", get_throttle_float());
 		//sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());
 		//sys_debug("dead time %d\n", get_deadtime());
-		//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();
@@ -149,7 +148,7 @@ static u32 _app_report_task(void *p) {
 		PMSM_FOC_LogDebug();
 		//F_debug();
 		//eCtrl_debug_log();
-		sys_debug("enc err %d\n", foc_observer_enc_errcount());
+		sys_debug("enc err %d, %d\n", foc_observer_enc_errcount(), foc_observer_sensorless_stable());
 		sys_debug("=====\n");
 		//err_code_log();
 	}
@@ -157,7 +156,7 @@ static u32 _app_report_task(void *p) {
 }
 int plot_type = 0;
 static void plot_smo_angle(void) {
-#if 1
+#if 0
 	float smo_angle = foc_observer_sensorless_angle();
 	float delta = smo_angle - PMSM_FOC_Get()->in.s_motAngle;
 	float s, c;

Applications/foc/commands.c

@@ -546,6 +546,8 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Use_SensorLess_Angle:
 		{
 			bool sensorless = decode_u8((u8 *)command->data)?true:false;
+			sys_debug("use smo %d\n", sensorless);
+#if 0
 			if (sensorless && mc_is_start() && PMSM_FOC_GetSpeed() >= foc_observer_sensorless_working_speed()) {
 				sys_debug("use smo %d\n", sensorless);
 				foc_observer_use_sensorless(sensorless);
@@ -553,6 +555,13 @@ static void process_foc_command(foc_cmd_body_t *command) {
 				sys_debug("unuse smo\n");
 				foc_observer_use_sensorless(false);
 			}
+#else
+			if (sensorless && mc_is_start()){
+				motor_encoder_produce_error(sensorless);
+			}else {
+				motor_encoder_produce_error(false);
+			}
+#endif
 			break;
 		}
 		case Foc_Set_Limiter_Config:

Applications/foc/core/PMSM_FOC_Core.c

@@ -338,9 +338,8 @@ static __INLINE void PMSM_FOC_Calc_iDC_Fast(void) {
 
 //#define UPDATE_Lq_By_iq   /* Q轴电感 通过Iq电流补偿 */
 //#define Volvec_Delay_Comp /* 电压矢量角度补偿 */
-static __INLINE float PMSM_FOC_Update_Input(void) {
+static __INLINE bool PMSM_FOC_Update_Input(void) {
 	AB_t iAB;
-	bool count_down = PWM_Direction_Down();
 	float *iabc = gFoc_Ctrl.in.s_iABC;
 
 	phase_current_get(iabc);
@@ -351,13 +350,13 @@ static __INLINE float PMSM_FOC_Update_Input(void) {
 
 	foc_observer_update(gFoc_Ctrl.out.s_OutVAB.a * TWO_BY_THREE, gFoc_Ctrl.out.s_OutVAB.b * TWO_BY_THREE, iAB.a, iAB.b);
 
-	if (!count_down) {
-		return false;
-	}
 	float enc_angle = motor_encoder_get_angle();
 	float enc_vel = motor_encoder_get_speed();
 	if (!foc_observer_diagnostic(enc_angle, enc_vel)){
-		/* deltect encoder angle error, do something here */
+		/* detect encoder angle error, do something here */
+		if (!foc_observer_sensorless_stable()) {
+			return false;
+		}
 		enc_angle = foc_observer_sensorless_angle();
 		enc_vel = foc_observer_sensorless_speed();
 	}
@@ -440,12 +439,12 @@ static __INLINE float iq_feedforward(float eW) {
 #endif
 }
 
-void PMSM_FOC_Schedule(void) {
+bool PMSM_FOC_Schedule(void) {
 
 	gFoc_Ctrl.ctrl_count++;
 
 	if (!PMSM_FOC_Update_Input()){
-		return;
+		return false;
 	}
 	
 	if (gFoc_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
@@ -506,6 +505,8 @@ void PMSM_FOC_Schedule(void) {
 	
 	pwm_update_duty(gFoc_Ctrl.out.n_Duty[0], gFoc_Ctrl.out.n_Duty[1], gFoc_Ctrl.out.n_Duty[2]);
 	pwm_update_sample(gFoc_Ctrl.out.n_Sample1, gFoc_Ctrl.out.n_Sample2, gFoc_Ctrl.out.n_CPhases);
+
+	return true;
 }
 
 void PMSM_FOC_LogDebug(void) {
@@ -973,6 +974,9 @@ void PMSM_FOC_SetEbrkTorque(float phase_curr, float dc_curr) {
 }
 
 float PMSM_FOC_GetEbrkTorque(void) {
+	if (!foc_observer_is_encoder()) {
+		return 0; //无感运行关闭能量回收
+	}
 	return gFoc_Ctrl.userLim.s_TorqueBrkLim;
 }
 

Applications/foc/core/PMSM_FOC_Core.h

@@ -55,8 +55,8 @@ typedef enum {
 	FOC_CRIT_ACC_OV_Err,
 	FOC_CRIT_ACC_Un_Err,
 	FOC_CRIT_Phase_Err,
-	FOC_CRIT_Encoder_Err,
-	FOC_CRIT_Brake_Err,
+	FOC_CRIT_Encoder_Err, /* 编码器错误，可能还是可以骑行，取决无感是否稳定 */
+	FOC_CRIT_Angle_Err, /* FOC 角度错误，一般发生在编码器错误，同时无感没有稳定的情况下，必须要停机 */
 	FOC_CRIT_CURR_OFF_Err,
 	FOC_CRIT_H_MOS_Err,
 	FOC_CRIT_L_MOS_Err,
@@ -66,7 +66,7 @@ typedef enum {
 	FOC_CRIT_Fan_Err,
 	FOC_CRIT_IDC_OV,
 	FOC_CRIT_THRO_Err,
-	FOC_CRIT_Err_Max = 32,	
+	FOC_CRIT_Err_Max = 32,
 }FOC_CritiCal_Ebit_t;
 
 #define FOC_Cri_Err_Mask(err) (1<<(err))
@@ -246,7 +246,7 @@ typedef enum {
 
 PMSM_FOC_Ctrl *PMSM_FOC_Get(void);
 void PMSM_FOC_CoreInit(void);
-void PMSM_FOC_Schedule(void);
+bool PMSM_FOC_Schedule(void);
 u8 PMSM_FOC_CtrlMode(void);
 void PMSM_FOC_idqCalc(void);
 void PMSM_FOC_Start(u8 nCtrlMode);

Applications/foc/core/foc_observer.c

@@ -16,6 +16,9 @@ void foc_observer_init(void) {
 	foc_obser.sensorless_angle = INVALID_ANGLE;
 	foc_obser.sensorless_est_angle = INVALID_ANGLE;
 	foc_obser.sensorless_speed = 0;
+	foc_obser.is_sensorless_stable = false;
+	foc_obser.sensorless_stable_cnt = 0;
+	foc_obser.sensorless_unstable_cnt = 0;
 #ifdef CONFIG_SMO_OBSERVER
 	smo_observer_init(CONFIG_SMO_PLL_BANDWITH, CONFIG_SMO_LFP_WC, CONFIG_SMO_GAIN_K, CONFIG_SMO_SIGMOID_MAX);
 	foc_obser.is_sensorless_enable = true;
@@ -42,32 +45,41 @@ 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 <= (foc_observer_sensorless_working_speed() - 100.0f)) {
+		if (!foc_obser.is_sensorless_running) {
+			if (motor_encoder_may_error()) {
+				foc_obser.enc_err_cnt++;
+				foc_obser.is_sensorless_running = true;
+			}
+		}
 		foc_obser.enc_angle = enc_angle;
 		foc_obser.enc_speed = ABS(enc_vel);
-		return true;
+		if (!foc_obser.is_sensorless_running) {
+			return true;
+		}
+		if (foc_obser.is_sensorless_stable) {
+			if (foc_obser.sensorless_speed <= foc_observer_sensorless_working_speed()/2.0f) {
+				foc_obser.is_sensorless_stable = false;
+			}
+		}
+		return false;
 	}
 	if (!foc_obser.is_sensorless_running) {
-		float est_diff = RPM_2_degree(foc_obser.enc_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 {
-			ration_thrl = 0.3f;
+		/* 判断无感算法是否已经稳定跟踪速度 */
+		if (!foc_obser.is_sensorless_stable) {
+			float vel_diff_abs = ABS(enc_vel - foc_obser.sensorless_speed);
+			if (vel_diff_abs < 50) {
+				foc_obser.sensorless_stable_cnt++;
+			}else {
+				foc_obser.sensorless_stable_cnt = 0;
+			}
+			if (foc_obser.sensorless_stable_cnt >= 1000) {
+				foc_obser.is_sensorless_stable = true;
+			}
 		}
-		if (ration > ration_thrl) {
+		if (motor_encoder_may_error()) {
 			foc_obser.enc_err_cnt++;
+			foc_obser.is_sensorless_running = true;
 		}
-		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);
@@ -81,6 +93,7 @@ float foc_observer_speed(void) {
 bool  foc_observer_is_encoder(void) {
 	return !foc_obser.is_sensorless_running;
 }
+
 void  foc_observer_use_sensorless(bool use_smo) {
 	if (foc_obser.is_sensorless_enable) {
 		foc_obser.is_sensorless_running = use_smo;
@@ -112,6 +125,10 @@ float foc_observer_sensorless_ration(void) {
 	return foc_obser.real_est_ration;
 }
 
+bool foc_observer_sensorless_stable(void) {
+	return foc_obser.is_sensorless_stable;
+}
+
 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,9 @@ typedef struct {
 	float sensorless_est_angle; //通过smo速度，估计当前角度
 	bool  is_sensorless_enable;
 	bool  is_sensorless_running;
+	bool  is_sensorless_stable;
+	int   sensorless_stable_cnt;
+	int   sensorless_unstable_cnt;
 	float real_est_diff;
 	float real_est_ration;
 	u16   enc_err_cnt;  //融合系数
@@ -30,6 +33,7 @@ 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);
+bool foc_observer_sensorless_stable(void);
 
 #endif /*_FOC_OBSERVER_H__*/
 

Applications/foc/foc_config.h

@@ -36,6 +36,7 @@
 #define CONFIG_FOC_VDQ_RAMP_TS 100
 #define CONFIG_FOC_VDQ_RAMP_FINAL_TIME 1000
 #define CONFIG_ZERO_SPEED_RPM  10
+#define CONFIG_LOCK_MOTOR_MIN_RPM 600
 #define CONFIG_ZERO_SPEED_RAMP_RMP 60 //从0速启动到结束的速度
 /* 电子刹车，动能回收，加速 */
 #define CONFIG_eCTRL_STEP_TS CONFIG_SPD_CTRL_MS     /* 斜率给定的step的时间值，单位 ms */

Applications/foc/motor/encoder.c

@@ -18,8 +18,6 @@
 #include "encoder_off5.h"
 #endif
 #endif
-static void encoder_do_offset_calibrate(void) ;
-static void _detect_off_finished(void);
 
 /* 磁编码器使用一对极的磁铁，所以编码器获取的角度和机械角度相同需要转为电角度*/
 
@@ -58,7 +56,7 @@ void encoder_init(void) {
 
 void encoder_set_direction(s8 direction) {
 	g_encoder.direction = direction;
-	g_encoder.cali_angle = INVALID_ANGLE;
+	//g_encoder.cali_angle = INVALID_ANGLE;
 }
 
 void encoder_set_bandwidth(float bandwidth) {
@@ -77,9 +75,14 @@ void encoder_init_clear(void) {
 	g_encoder.est_vel_counts = 0;
 	g_encoder.position = 0.0f;
 	g_encoder.interpolation = 0.0f;
-	g_encoder.cali_angle = INVALID_ANGLE;
+	//g_encoder.cali_angle = INVALID_ANGLE;
 	g_encoder.enc_count_off = 0xFFFFFFFF;
 	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.start_dianostic_cnt = 0;
+	g_encoder.pwm_time_ms = get_tick_ms();
 	_init_pll();
 }
 
@@ -143,6 +146,56 @@ static __INLINE float _eccentricity_compensation(int cnt) {
 #endif
 }
 
+#define CONFIG_ENC_DIANOSTIC_MIN_CNT (10.0F * ENC_MAX_RES * FOC_CTRL_US) //600rpm
+
+static void encoder_detect_error(u32 cnt) {
+#ifdef CONFIG_ENC_DETECT_ERR
+	if (!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;
+		}
+#ifdef CONFIG_ENC_ERR_TEST
+		if (g_encoder.produce_error) {
+			delta_cnt = 0;
+		}
+#endif
+		if (g_encoder.last_delta_cnt == MAX_S16) {
+			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) {
+			g_encoder.enc_maybe_err = true;
+		}
+		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)) {
+			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) {
+				r_thr = 0.3f;
+			}else if (g_encoder.last_delta_cnt <= CONFIG_ENC_DIANOSTIC_MIN_CNT * 4) {
+				r_thr = 0.5f;
+			}else if (g_encoder.last_delta_cnt <= CONFIG_ENC_DIANOSTIC_MIN_CNT * 6) {
+				r_thr = 0.6f;
+			}else {
+				r_thr = 0.7f;
+			}
+			if (r <= r_thr || r >= (2.0f - r_thr)) {
+				g_encoder.enc_maybe_err = true;
+			}
+		}
+		g_encoder.last_delta_cnt = delta_cnt;
+	}
+#else
+	g_encoder.enc_maybe_err = false;
+#endif
+}
+
 float encoder_get_theta(void) {
 	if (!g_encoder.b_index_found) {
 		return g_encoder.pwm_angle;
@@ -154,6 +207,8 @@ float encoder_get_theta(void) {
 		g_encoder.b_timer_ov = true;
 		ENC_ClearUpFlags();
 	}
+
+	encoder_detect_error(cnt);
 	
     bool snap_to_zero_vel = encoder_run_pll((float)(cnt));
 
@@ -171,111 +226,28 @@ float encoder_get_theta(void) {
 			g_encoder.interpolation = 0.0f;
 		}
 	}
-	if (g_encoder.cali_angle != INVALID_ANGLE) {
-		g_encoder.interpolation = 0.0f;
-	}
+
 	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle((float)cnt + g_encoder.interpolation) * g_encoder.motor_poles + g_encoder.enc_offset;
 	g_encoder.abi_angle += _eccentricity_compensation(cnt);
 	rand_angle(g_encoder.abi_angle);
 	g_encoder.last_cnt = cnt;
 	g_encoder.last_us = task_get_usecond();
 
-	if (g_encoder.cali_angle != INVALID_ANGLE) {
-		encoder_do_offset_calibrate();
-	}
 	g_encoder.position += (g_encoder.est_vel_counts/g_encoder.cpr) * FOC_CTRL_US;
 	return g_encoder.abi_angle;
 }
 
-float encoder_get_speed(void) {
-	return (g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f;
-}
-
-void _encoder_caliberate_init(void) {
-	if (g_encoder.encoder_off_map != NULL) {
-		return;
-	}
-	u32 mask = cpu_enter_critical();
-	g_encoder.encoder_off_map = (s16 *)os_alloc(g_encoder.cpr * sizeof(s16));
-	g_encoder.encoder_off_count = (u8 *)os_alloc(g_encoder.cpr);
-	
-	for (int i = 0; i < g_encoder.cpr; i++) {
-		g_encoder.encoder_off_map[i] = 0;
-		g_encoder.encoder_off_count[i] = 0;
-	}
-	cpu_exit_critical(mask);
-}
-
-void _encoder_caliberate_deinit(void) {
-	if (g_encoder.encoder_off_map != NULL) {
-		os_free(g_encoder.encoder_off_map);
-		os_free(g_encoder.encoder_off_count);
-	}
-	g_encoder.encoder_off_map = NULL;
-	g_encoder.encoder_off_count = NULL;
-}
-
-#define MIN_OFF_COUNT 5
-void encoder_detect_offset(float angle){
-#if 1
-	_encoder_caliberate_init();
-	g_encoder.cali_angle = angle;
-#else
-	plot_2data16((s16)angle, (s16)g_encoder.abi_angle);
-#endif
-}
-
-static void encoder_do_offset_calibrate(void) {
-	float delta = (g_encoder.abi_angle - g_encoder.cali_angle);
-	if (delta > 200) {
-		delta = delta - 360;
-	}
-	if (delta < -200) {
-		delta = delta + 360;
-	}
-	if (g_encoder.direction == POSITIVE) {
-		if ((g_encoder.encoder_off_count[g_encoder.last_cnt] & 0xF) <= MIN_OFF_COUNT) {
-			g_encoder.encoder_off_map[g_encoder.last_cnt] += (s16)(delta*100.0f);
-			g_encoder.encoder_off_count[g_encoder.last_cnt] += 0x01;
-		}
-	}else {
-		if (((g_encoder.encoder_off_count[g_encoder.last_cnt] >> 4) & 0xF) <= MIN_OFF_COUNT) {
-			g_encoder.encoder_off_map[g_encoder.last_cnt] += (s16)(delta*100.0f);
-			g_encoder.encoder_off_count[g_encoder.last_cnt] += 0x10;
-		}
-	}
-}
 
-bool encoder_detect_finish(void) {
-	u8 off_count = 0;
-	for (int i = 0; i < 1024; i++) {
-		if (g_encoder.direction == POSITIVE) {
-			off_count = g_encoder.encoder_off_count[i] & 0xF;
-		}else {
-			off_count = (g_encoder.encoder_off_count[i] >> 4)& 0xF;
-		}
-		if (off_count <= MIN_OFF_COUNT) {
-			return false;
-		}
-	}
-	if (g_encoder.direction == NEGATIVE) {
-		g_encoder.cali_angle = INVALID_ANGLE;
-	}
-	return true;
+void encoder_produce_error(bool error) {
+	g_encoder.produce_error = error;
 }
 
-void encoder_detect_upload(void) {
-	_detect_off_finished();//output data to PC tools, and use Matlab do FIR filter
-	_encoder_caliberate_deinit();
+bool encoder_may_error(void) {
+	return g_encoder.enc_maybe_err;
 }
 
-static void _detect_off_finished(void) {
-	for (int i = 0; i < 1024; i++) {
-		float angle_off = g_encoder.encoder_off_map[i] / (((g_encoder.encoder_off_count[i] >> 4)&0xF) + (g_encoder.encoder_off_count[i]&0xF));
-		plot_1data16((s16)angle_off);
-		delay_ms(30);
-		wdog_reload();
-	}	
+float encoder_get_speed(void) {
+	return (g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f;
 }
 
 float encoder_get_vel_count(void) {
@@ -334,12 +306,15 @@ bool encoder_get_cali_error(void) {
 }
 
 static void encoder_sync_pwm_abs(void) {
+	u32 mask = cpu_enter_critical();
 	ENC_COUNT = g_encoder.pwm_count;
 	g_encoder.last_cnt = g_encoder.pwm_count;
 	g_encoder.est_pll.observer = (float)g_encoder.pwm_count;
 	g_encoder.abi_angle = g_encoder.pwm_angle;
 	g_encoder.b_index_found = true;
+	g_encoder.last_delta_cnt = MAX_S16;
 	PLL_Reset(&g_encoder.est_pll, (float)_abi_count());
+	cpu_exit_critical(mask);
 }
 
 /*I 信号的中断处理，一圈一个中断*/
@@ -386,11 +361,16 @@ void ENC_PWM_Duty_Handler(float t, float d) {
 		encoder_sync_pwm_abs();
 	}
 	pwm_check_count ++;
+#ifdef CONFIG_ENC_ERR_TEST
+	if (!g_encoder.produce_error) {
+		g_encoder.pwm_time_ms = get_tick_ms();
+	}
+#endif
 }
 static u32 _check_time = 0;
 bool ENC_Check_error(void) {
 	bool error = false;
-	if (get_delta_ms(_check_time) > 1000) {
+	if (get_delta_ms(_check_time) > 200) {
 		if (pwm_check_count == 0) {
 			error = true;
 		}
@@ -418,5 +398,5 @@ 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\n", enc_get_pwm_freq());
+	sys_debug("pwm freq %f, err %d\n", enc_get_pwm_freq(), g_encoder.enc_maybe_err);
 }

Applications/foc/motor/encoder.h

@@ -19,36 +19,40 @@ typedef struct {
 	bool  b_lock_pos;
 	bool  b_cali_err;
 	u32   cpr;
-	u32   last_cnt;
+	s16   last_cnt;
 	u32   b_index_cnt;
 	u32   last_us;
 	s8    direction; //motor running dir, 逆时针 正，顺时针负
 	float est_vel_counts; //每秒多少个计数
 	float est_angle_counts;
 	float rpm;
-	u8   *encoder_off_count;
-	s16  *encoder_off_map;
+	//u8   *encoder_off_count;
+	//s16  *encoder_off_map;
+	bool  produce_error;
+	s16   last_delta_cnt;
+	u16   start_dianostic_cnt;
+	bool  enc_maybe_err;
+	u32   pwm_time_ms;
 }encoder_t;
 
 void encoder_init(void);
 void encoder_init_clear(void);
 float encoder_get_theta(void);
 float encoder_get_speed(void);
-void encoder_detect_offset(float angle);
 void encoder_set_direction(s8 direction);
 float encoder_get_vel_count(void);
 void encoder_lock_position(bool enable);
-bool encoder_detect_finish(void);
 float encoder_get_pwm_angle(void);
 float encoder_zero_phase_detect(float *enc_off);
 bool ENC_Check_error(void);
 float encoder_get_abi_angle(void);
 float encoder_get_position(void);
-void encoder_detect_upload(void);
 void encoder_clear_cnt_offset(void);
 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);
 
 #endif /* _Encoder_H__ */
 

Applications/foc/motor/motor.c

@@ -45,6 +45,15 @@ static motor_t motor = {
 	.motor_lim = 0,
 	.idc_user_lim = IDC_USER_LIMIT_NONE,
 };
+/* 无感运行的挡位，限制速度，母线电流，最大扭矩 */
+static mc_gear_t sensorless_gear = {
+	.n_max_speed = CONFIG_SENSORLESS_MAX_SPEED,
+	.n_max_trq = CONFIG_SENSORLESS_MAX_TORQUE,
+	.n_max_idc = CONFIG_SENSORLESS_MAX_IDC,
+	.n_zero_accl = 1500,
+	.n_accl_time = 1500,
+	.n_torque = {100, 100, 100, 100, 0, 0, 0, 0, 0, 0},
+};
 static motor_err_t mc_error;
 
 static void MC_Check_MosVbusThrottle(void) {
@@ -174,14 +183,16 @@ static void _led_off_timer_handler(shark_timer_t *t) {
 
 static void mc_gear_vmode_changed(void) {
 	mc_gear_t *gears = mc_get_gear_config();
-	
-	//sys_debug("limit %d-%d-%d, mode = %s\n", gears->n_max_speed, gears->n_max_idc, gears->n_max_trq, motor.b_is96Mode?"96V":"48V");
+
 	PMSM_FOC_SpeedLimit(gears->n_max_speed);
 	PMSM_FOC_DCCurrLimit(min(gears->n_max_idc, motor.idc_user_lim));
 	PMSM_FOC_TorqueLimit(gears->n_max_trq);
 }
 
 static s16 mc_get_gear_idc_limit(void) {
+	if (!foc_observer_is_encoder()) {
+		return sensorless_gear.n_max_idc;
+	}
 	if (motor.b_is96Mode) {
 		return nv_get_gear_configs()->gears_96[motor.n_gear].n_max_idc;
 	}else {
@@ -215,7 +226,9 @@ motor_t * mc_params(void) {
 
 mc_gear_t *mc_get_gear_config_by_gear(u8 n_gear) {
 	mc_gear_t *gears;
-
+	if (!foc_observer_is_encoder()) { //无感模式，受限运行
+		return &sensorless_gear;
+	}
 	if (motor.b_is96Mode) {
 		gears = &nv_get_gear_configs()->gears_96[0];
 	}else {
@@ -229,7 +242,7 @@ mc_gear_t *mc_get_gear_config(void) {
 }
 
 bool mc_critical_can_not_run(void) {
-	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_IDC_OV) | FOC_Cri_Err_Mask(FOC_CRIT_MOTOR_TEMP_Err) | FOC_Cri_Err_Mask(FOC_CRIT_MOS_TEMP_Err);
+	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_IDC_OV) | FOC_Cri_Err_Mask(FOC_CRIT_MOTOR_TEMP_Err) | FOC_Cri_Err_Mask(FOC_CRIT_MOS_TEMP_Err) | FOC_Cri_Err_Mask(FOC_CRIT_Angle_Err);
 	u32 err = motor.n_CritiCalErrMask & mask;
 	return (err != 0);
 }
@@ -326,7 +339,7 @@ bool mc_stop(void) {
 		return false;
 	}
 
-	if (motor_encoder_get_speed() > CONFIG_ZERO_SPEED_RPM) {
+	if (PMSM_FOC_GetSpeed() > CONFIG_ZERO_SPEED_RPM) {
 		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 		return false;
 	}
@@ -654,63 +667,6 @@ void mc_force_run_open(s16 vd, s16 vq) {
 	motor.b_force_run = true;
 }
 
-void mc_encoder_off_calibrate(s16 vd) {
-	if (motor.b_start || motor.b_calibrate) {
-		return;
-	}
-	motor.b_calibrate = true;
-	pwm_up_enable(false);
-	pwm_turn_on_low_side();
-	task_udelay(500);
-	PMSM_FOC_Start(CTRL_MODE_OPEN);
-	phase_current_offset_calibrate();
-	pwm_start();
-	adc_start_convert();
-	phase_current_calibrate_wait();
-	PMSM_FOC_Set_MotAngle(0);
-	PMSM_FOC_SetOpenVdq(vd, 0);
-	delay_ms(2000);
-	motor_encoder_set_direction(POSITIVE);
-	for (int i = 0; i < 200; i++) {
-		for (float angle = 0; angle < 360; angle++) {
-			PMSM_FOC_Set_MotAngle(angle);
-			delay_ms(1);
-			if (i > 20) {
-				motor_encoder_offset(angle);
-			}
-		}
-		wdog_reload();
-		if (motor_encoder_offset_is_finish()) {
-			break;
-		}
-	}
-	motor_encoder_set_direction(NEGATIVE);
-	delay_ms(100);
-	for (int i = 0; i < 200; i++) {
-		for (float angle = 360; angle > 0; angle--) {
-			PMSM_FOC_Set_MotAngle(angle);
-			delay_ms(1);
-			if (i > 10) {
-				motor_encoder_offset(angle);
-			}
-		}
-		wdog_reload();
-		if (motor_encoder_offset_is_finish()) {
-			break;
-		}
-	}
-	delay_ms(500);
-	PMSM_FOC_SetOpenVdq(0, 0);
-	delay_ms(500);
-	wdog_reload();
-	adc_stop_convert();
-	pwm_stop();
-	PMSM_FOC_Stop();
-	pwm_up_enable(true);
-	motor_encoder_data_upload();
-	motor.b_calibrate = false;
-}
-
 
 static void _encoder_zero_off_timer_handler(shark_timer_t *t){
 	if (!motor.b_calibrate) {
@@ -792,7 +748,7 @@ bool mc_lock_motor(bool lock) {
 		ret = false;
 		goto ml_ex_cri;
 	}
-	if (lock && (motor_encoder_get_speed() > CONFIG_ZERO_SPEED_RPM)) {
+	if (lock && (motor_encoder_get_speed() >= CONFIG_LOCK_MOTOR_MIN_RPM)) {
 		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 		ret = false;
 		goto ml_ex_cri;
@@ -1077,7 +1033,11 @@ void ADC_IRQHandler(void) {
 		return;
 	}
 	TIME_MEATURE_START();
-	PMSM_FOC_Schedule();
+	if (!PMSM_FOC_Schedule()) {/* FOC 角度错误，立即停机 */
+		mc_set_critical_error(FOC_CRIT_Angle_Err);
+		PMSM_FOC_Stop();
+		pwm_disable_channel();
+	}
 	TIME_MEATURE_END();
 }
 
@@ -1249,15 +1209,15 @@ static bool mc_can_stop_foc(void) {
 			return true;
 		}
 	}
-	/* 启用无感观测器，同时速度低于观测器允许的最小速度，关闭输出，滑行 */
-	if (!foc_observer_is_encoder() && PMSM_FOC_GetSpeed() < foc_observer_sensorless_working_speed()) {
+	/* 启用无感观测器，但是观测器未稳定，关闭输出，滑行 */
+	if (!foc_observer_is_encoder() && !foc_observer_sensorless_stable()) {
 		return true;
 	}
 	return false;
 }
 
 static bool mc_can_restart_foc(void) {
-	return (!PMSM_FOC_Is_Start()) && (!mc_throttle_released() || (motor.epm_dir != EPM_Dir_None)) && (!mc_unsafe_critical_error());
+	return (!PMSM_FOC_Is_Start()) && (!mc_throttle_released() || (motor.epm_dir != EPM_Dir_None)) && (!mc_critical_can_not_run());
 }
 
 #endif
@@ -1288,6 +1248,7 @@ measure_time_t g_meas_MCTask;
 #define mc_TaskEnd time_measure_end(&g_meas_MCTask)
 void Sched_MC_mTask(void) {
 	static int vbus_err_cnt = 0;
+	static bool _sensorless_run = false;
 	mc_TaskStart;
 
 	adc_vref_filter();
@@ -1318,13 +1279,16 @@ void Sched_MC_mTask(void) {
 		mc_TaskEnd;
 		return;
 	}
-
-	if (mc_detect_vbus_mode() || (limted == FOC_LIM_CHANGE_L)) {
+	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);
 		motor.b_limit_pending = false;
 	}else if (limted == FOC_LIM_CHANGE_H) {
 		motor.b_limit_pending = true;
 	}
+	_sensorless_run = sensor_less;
+
 	/* 如果取消高温，欠压等限流需要释放转把后才生效，确保不会突然加速 */
 	if (motor.b_limit_pending && mc_throttle_released()) {
 		motor.b_limit_pending = false;

Applications/foc/motor/motor.h

@@ -122,6 +122,16 @@ static __INLINE float motor_encoder_get_angle(void) {
 #endif
 }
 
+static __INLINE bool motor_encoder_may_error(void) {
+#ifdef USE_ENCODER_HALL
+	return false;
+#elif defined (USE_ENCODER_ABI)
+	return encoder_may_error();
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
 static __INLINE void motor_encoder_update(void) {
 #ifdef USE_ENCODER_HALL
 	hall_sensor_get_theta();
@@ -165,9 +175,9 @@ static __INLINE float motor_encoder_get_position(void) {
 
 static __INLINE void motor_encoder_init(void) {
 #ifdef USE_ENCODER_HALL
-		hall_sensor_init();
+	hall_sensor_init();
 #elif defined (USE_ENCODER_ABI)
-		encoder_init();
+	encoder_init();
 #else
 	#error "Postion sensor ERROR"
 #endif
@@ -175,55 +185,14 @@ static __INLINE void motor_encoder_init(void) {
 
 static __INLINE void motor_encoder_start(bool start) {
 #ifdef USE_ENCODER_HALL
-		hall_sensor_clear(direction);
+	hall_sensor_clear(direction);
 #elif defined (USE_ENCODER_ABI)
-		encoder_init_clear();
+	encoder_init_clear();
 #else
 	#error "Postion sensor ERROR"
 #endif
 }
 
-static __INLINE void motor_encoder_offset(float angle) {
-#ifdef USE_ENCODER_HALL
-		hall_detect_offset(angle);
-#elif defined (USE_ENCODER_ABI)
-		encoder_detect_offset(angle);
-#else
-	#error "Postion sensor ERROR"
-#endif
-}
-
-static __INLINE void motor_encoder_offset_finish(void) {
-#ifdef USE_ENCODER_HALL
-		hall_detect_offset_finish();
-#elif defined (USE_ENCODER_ABI)
-		encoder_detect_finish();
-#else
-	#error "Postion sensor ERROR"
-#endif
-}
-
-static __INLINE bool motor_encoder_offset_is_finish(void) {
-#ifdef USE_ENCODER_HALL
-	return false;
-#elif defined (USE_ENCODER_ABI)
-	return encoder_detect_finish();
-#else
-	#error "Postion sensor ERROR"
-#endif
-}
-
-static __INLINE void motor_encoder_data_upload(void) {
-#ifdef USE_ENCODER_HALL
-	
-#elif defined (USE_ENCODER_ABI)
-	encoder_detect_upload();
-#else
-	#error "Postion sensor ERROR"
-#endif
-}
-
-
 static __INLINE float motor_encoder_zero_phase_detect(float *enc_off){
 #ifdef USE_ENCODER_HALL
 	return 0.0f;
@@ -236,9 +205,9 @@ static __INLINE float motor_encoder_zero_phase_detect(float *enc_off){
 
 static __INLINE void motor_encoder_set_direction(s8 dir) {
 #ifdef USE_ENCODER_HALL
-			hall_set_direction(dir);
+	hall_set_direction(dir);
 #elif defined (USE_ENCODER_ABI)
-			encoder_set_direction(dir);
+	encoder_set_direction(dir);
 #else
 	#error "Postion sensor ERROR"
 #endif
@@ -248,7 +217,7 @@ static __INLINE void motor_encoder_lock_pos(bool lock) {
 #ifdef USE_ENCODER_HALL
 		
 #elif defined (USE_ENCODER_ABI)
-		encoder_lock_position(lock);
+	encoder_lock_position(lock);
 #else
 	#error "Postion sensor ERROR"
 #endif
@@ -258,12 +227,21 @@ static __INLINE void motor_encoder_band_epm(bool epm) {
 #ifdef USE_ENCODER_HALL
 		
 #elif defined (USE_ENCODER_ABI)
-		encoder_epm_pll_band(epm);
+	encoder_epm_pll_band(epm);
 #else
 	#error "Postion sensor ERROR"
 #endif
 }
 
+static __INLINE void motor_encoder_produce_error(bool error) {
+#ifdef USE_ENCODER_HALL
+
+#elif defined (USE_ENCODER_ABI)
+	encoder_produce_error(error);
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
 
 #endif /* _MOTOR_H__ */
 

Applications/foc/motor/motor_param.h

@@ -76,6 +76,10 @@ float motor_get_ebreak_toruqe(float rpm);
 #define CONFIG_DEFAULT_PHASE_CURR_LIM 500
 #define CONFIG_MAX_FW_D_CURR     300.0F //d轴最大的退磁电流
 #define CONFIG_CURRENT_LOOP_DECOUPE    //电流环解耦
+#define CONFIG_SENSORLESS_MAX_IDC 30
+#define CONFIG_SENSORLESS_MAX_TORQUE 80
+#define CONFIG_SENSORLESS_MAX_SPEED 3000
+
 #elif CONFIG_MOT_TYPE==MOTOR_BLUESHARK_NEW2
 #define MOTOR_R   0.013f
 #define MOTOR_Ld (0.000140f*0.5f)
@@ -116,6 +120,10 @@ float motor_get_ebreak_toruqe(float rpm);
 #define CONFIG_DEFAULT_PHASE_CURR_LIM 200
 #define CONFIG_MAX_FW_D_CURR     100.0F //d轴最大的退磁电流
 
+#define CONFIG_SENSORLESS_MAX_IDC 30
+#define CONFIG_SENSORLESS_MAX_TORQUE 80
+#define CONFIG_SENSORLESS_MAX_SPEED 3000
+
 #elif CONFIG_MOT_TYPE==MOTOR_BLUESHARK_OLD
 #define MOTOR_R 0.012f
 #define MOTOR_Ld (0.000143f*0.5f)

Applications/os/os_types.h

@@ -51,6 +51,8 @@ typedef unsigned int	uint32;
 #define rand_angle(a) {while (a >= PHASE_360_DEGREE) a-=PHASE_360_DEGREE;while (a < 0) a +=PHASE_360_DEGREE;};
 #define ABS(x) 					( (x)>0?(x):-(x) )
 
+#define MAX_S16 (0x7FFF)
+
 extern void *pvPortMalloc( size_t xWantedSize );
 #define os_alloc pvPortMalloc
 extern void vPortFree(void *);

Applications/prot/can_foc_msg.c

@@ -15,7 +15,7 @@ void can_report_speed(u8 can, s16 rpm) {
 
 void can_report_power(u8 can) {
 	u8 data[8];
-	s16 rpm = (s16)motor_encoder_get_speed();
+	s16 rpm = (s16)PMSM_FOC_GetSpeed();
 	float vDC = get_vbus_float();
 	float iDC = PMSM_FOC_GetVbusCurrent();	
 	s16 v = S16Q5(vDC);
@@ -96,7 +96,7 @@ void can_report_foc_status(u8 can) {
 void can_mcast_foc_status2(void) {
 	u8 data[8];
 	encode_u16(data, mc_get_running_status2());
-	encode_u16(data + 2, ABS((s16)(motor_encoder_get_speed())));
+	encode_u16(data + 2, ABS((s16)(PMSM_FOC_GetSpeed())));
 	float vDC = get_vbus_float();
 	encode_s16(data + 4, (s16)(vDC*10));
 	float iDC = PMSM_FOC_GetVbusCurrent();
@@ -127,7 +127,7 @@ void can_report_ext_status(u8 can) {
 	data[1] = mc_is_start()?0:1;
 	data[1] |= (mc_is_cruise_enabled()?1:0) << 3;
 	data[1] |= mc_get_gear() << 6;
-	encode_s16(data + 2, ABS((s16)(motor_encoder_get_speed()/4.0f)));
+	encode_s16(data + 2, ABS((s16)(PMSM_FOC_GetSpeed()/4.0f)));
 	float vDC = get_vbus_float();
 	encode_s16(data + 4, (s16)(vDC*10));
 	float iDC = PMSM_FOC_GetVbusCurrent();