空转检测调试

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

Applications/foc/core/F_Calc.c

@@ -30,7 +30,7 @@ void F_all_Calc(void) {
 	}
 	float delta_mph = (kmph - mot_vel) * 1000.0f / 3600.0f;
 	float diff = delta_mph / (CONFIG_SPD_CTRL_MS / 1000.0f);
-	LowPass_Filter(mot_accl, diff, 0.03f);
+	LowPass_Filter(mot_accl, diff, 0.01f);
 
 	F_accl = M * mot_accl;
 	F_Air = SQ(kmph) * 0.02f;

Applications/foc/core/PMSM_FOC_Core.c

@@ -504,6 +504,7 @@ void PMSM_FOC_Schedule(void) {
 
 void PMSM_FOC_LogDebug(void) {
 	sys_debug("DC curr %f --- %f\n", gFoc_Ctrl.out.s_CalciDC, gFoc_Ctrl.out.s_CalciDC2);
+	sys_debug("%s\n", gFoc_Ctrl.out.empty_load?"NoLoad Running":"Load Runing");
 }
 
 /*called in media task */
@@ -596,17 +597,51 @@ static void crosszero_step_towards(float *value, float target) {
 static __INLINE void PMSM_FOC_VelCtrl_Decide(void) {
 	static int change_cnt = 0;
 	static bool change_done = false;
+	static u32  change_time = 0xFFFFFFFF;
 	float f_te = F_get_Te();
+	float f_accl = F_get_accl();
 	if (gFoc_Ctrl.in.s_motVelocity == 0.0f || gFoc_Ctrl.out.n_RunMode == CTRL_MODE_OPEN) {
 		change_cnt = 0;
+		change_time = 0xFFFFFFFF;
 		change_done = false;
+		gFoc_Ctrl.out.empty_load = false;
 		return;
 	}
 	if (f_te <= 0.0f) {
 		change_cnt = 0;
+		change_time = 0xFFFFFFFF;
+		gFoc_Ctrl.out.empty_load = false;
 		return;
 	}
-	float f_accl = F_get_accl();
+
+	if (change_done) { 
+		/* 误判空转，发现电机给定的N大于空气阻力，说明不是空转 */
+		if (gFoc_Ctrl.out.empty_load) {
+			float f_air = F_get_air();
+			if ((f_accl > 0) && (f_te > (f_air + f_accl))) {
+				change_cnt ++;
+			}else {
+				change_cnt = 0;
+			}
+			if (change_cnt >= 50) {
+				gFoc_Ctrl.out.empty_load = false;
+#ifdef CONFIG_SPEED_LADRC
+				ladrc_change_b0(&gFoc_Ctrl.vel_lim_adrc, nv_get_foc_params()->f_adrc_vel_lim_B0);
+				ladrc_change_K(&gFoc_Ctrl.vel_lim_adrc, nv_get_foc_params()->f_adrc_vel_lim_Wcv);
+				ladrc_change_b0(&gFoc_Ctrl.vel_adrc, nv_get_foc_params()->f_adrc_vel_B0);
+				ladrc_change_K(&gFoc_Ctrl.vel_adrc, nv_get_foc_params()->f_adrc_vel_Wcv);
+#endif
+			}
+		}
+		return;
+	}
+	if (change_time == 0xFFFFFFFF) {
+		change_time = get_tick_ms();
+	}else { //起步3s内检测是否空转
+		if (get_delta_ms(change_time) > 3000) {
+			return;
+		}
+	}
 
 	if ((f_accl > 200.0f) && (f_accl/f_te > 1.5f )) {
 		change_cnt++;
@@ -614,16 +649,26 @@ static __INLINE void PMSM_FOC_VelCtrl_Decide(void) {
 		change_cnt = CHANGE_MAX_CNT;
 	}
 	else {
-		change_cnt = 0;
+		if ((f_te > 50) && (f_accl > 0) && (f_te > f_accl)) {
+			change_cnt --;
+		}else {
+			change_cnt = 0;
+		}
 	}
 	if (!change_done && (change_cnt >= CHANGE_MAX_CNT)) {
 		change_done = true;
+		change_cnt = 0;
+		gFoc_Ctrl.out.empty_load = change_done;
 #ifdef CONFIG_SPEED_LADRC
 		ladrc_change_b0(&gFoc_Ctrl.vel_lim_adrc, 2500.0f);
 		ladrc_change_K(&gFoc_Ctrl.vel_lim_adrc, 3);
 		ladrc_change_b0(&gFoc_Ctrl.vel_adrc, 2500.0f);
 		ladrc_change_K(&gFoc_Ctrl.vel_adrc, 3);
 #endif
+	}else if (!change_done && (change_cnt <= -20)) {
+		change_done = true;
+		change_cnt = 0;
+		gFoc_Ctrl.out.empty_load = false;
 	}
 }
 

Applications/foc/core/PMSM_FOC_Core.h

@@ -166,12 +166,8 @@ typedef struct {
 	s16   test_sample;
 	float sin;
 	float cos;
-	float pre_sin;
-	float pre_cos;
-	float s_preAngle;
 	u8    n_Error;
-	u32   n_CritiCalErrMask;
-	u32   n_CritiCalErrPrev;
+	bool  empty_load; //空载运行
 }FOC_OutP;
 
 typedef struct {