堵转保护开始计时点Q轴电流大于阈值并且速度小于1km，在3s内不超过0.2圈认为堵转

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

Applications/foc/motor/encoder.c

@@ -63,6 +63,7 @@ void encoder_init_clear(s8 diretcion) {
 	g_encoder.pwm_angle = 0.0f;
 	g_encoder.est_angle_counts = 0;
 	g_encoder.est_vel_counts = 0;
+	g_encoder.position = 0.0f;
 	g_encoder.interpolation = 0.0f;
 	g_encoder.cali_angle = INVALID_ANGLE;
 }
@@ -167,7 +168,7 @@ float encoder_get_theta(void) {
 	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;
 }
 
@@ -264,6 +265,10 @@ float encoder_get_vel_count(void) {
 	return g_encoder.est_vel_counts;
 }
 
+float encoder_get_position(void) {
+	return g_encoder.position;
+}
+
 float encoder_zero_phase_detect(void) {
 	float phase = g_encoder.pwm_angle;
 	float total_ph = phase;

Applications/foc/motor/encoder.h

@@ -13,7 +13,7 @@ typedef struct {
 	float interpolation;
 	float pll_bandwidth_shadow;
 	float pll_bandwidth;
-	float postion;
+	float position;
 	PLL_t est_pll;
 	bool  b_timer_ov;
 	bool  b_lock_pos;
@@ -42,5 +42,7 @@ float encoder_get_pwm_angle(void);
 float encoder_zero_phase_detect(void);
 bool ENC_Check_error(void);
 float encoder_get_abi_angle(void);
+float encoder_get_position(void);
+
 #endif /* _Encoder_H__ */
 

Applications/foc/motor/motor.c

@@ -552,6 +552,7 @@ static bool mc_can_stop_foc(void) {
 	return false;
 }
 #endif
+#if 0
 static bool mc_run_stall_process(u8 run_mode) {
 	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !PMSM_FOC_AutoHoldding()) {
 		//堵转判断
@@ -578,7 +579,40 @@ static bool mc_run_stall_process(u8 run_mode) {
 	}
 	return false;
 }
+#else
+static bool mc_run_stall_process(u8 run_mode) {
+	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !PMSM_FOC_AutoHoldding()) {
+		//堵转判断
+		if (motor.b_runStall) {
+			if (!mc_throttle_released()) {
+				return true;
+			}
+			motor.runStall_time = 0;
+			motor.b_runStall = false; //转把释放，清除堵转标志
+		}else if (PMSM_FOC_Get()->out.s_RealIdq.q >= CONFIG_STALL_MAX_CURRENT){
+			if (ABS(PMSM_FOC_GetSpeed()) < 1.0f && (motor.runStall_time == 0)) {
+				motor.runStall_time = get_tick_ms();
+				motor.runStall_pos = motor_encoder_get_position();
+			}
+			if (motor.runStall_time > 0) {
+				if (get_delta_ms(motor.runStall_time) >= CONFIG_STALL_MAX_TIME) {
+					motor.b_runStall = true;
+					motor.runStall_time = 0;
+					torque_speed_target(run_mode, 0.0f);
+					return true;
+				}
+				if (ABS(motor.runStall_pos - motor_encoder_get_position()) >= 0.2f) {
+					motor.runStall_time = 0;
+				}
+			}
+		}else {
+			motor.runStall_time = 0;
+		}
+	}
+	return false;
+}
 
+#endif
 static void mc_autohold_process(void) {
 	if (PMSM_FOC_AutoHoldding() && !mc_throttle_released()) {
 		mc_auto_hold(false);

Applications/foc/motor/motor.h

@@ -20,6 +20,7 @@ typedef struct {
 	float  lim_rpm;
 	float  lim_phase_curr;
 	u32    runStall_time;
+	float  runStall_pos;
 	s16    s_testAngle;
 	s32    s_targetFix;
 	s8     s_direction;
@@ -91,6 +92,17 @@ static __INLINE float motor_encoder_get_vel_count(void) {
 #endif
 }
 
+static __INLINE float motor_encoder_get_position(void) {
+#ifdef USE_ENCODER_HALL
+	return 0;
+#elif defined (USE_ENCODER_ABI)
+	return encoder_get_position();
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
+
 static __INLINE void motor_encoder_init(void) {
 #ifdef USE_ENCODER_HALL
 		hall_sensor_init();