无感和encoder切换，未调试

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

Applications/foc/core/PMSM_FOC_Core.c

@@ -290,6 +290,8 @@ static __INLINE float PMSM_FOC_Update_Input(void) {
 	float enc_vel = motor_encoder_get_speed();
 	if (!foc_observer_diagnostic(enc_angle, enc_vel)){
 		/* deltect encoder angle error, do something here */
+		enc_angle = foc_observer_sensorless_angle();
+		enc_vel = foc_observer_sensorless_speed();
 	}
 	
 	if (!gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {

Applications/foc/core/foc_observer.c

@@ -45,35 +45,37 @@ bool foc_observer_diagnostic(float enc_angle, float enc_vel) {
 		foc_obser.enc_speed = ABS(enc_vel);
 		return true;
 	}
-	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;
-	}else {
-		thro = 0.4f;
-	}
-	if (r >= thro) {
-		if (foc_obser.enc_err_cnt < 65535) {
-			foc_obser.enc_err_cnt ++;
+	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;
+		}else {
+			thro = 0.4f;
+		}
+		if (r >= thro) {
+			if (foc_obser.enc_err_cnt < 65535) {
+				foc_obser.enc_err_cnt ++;
+			}
 		}
 	}
 	foc_obser.enc_angle = enc_angle;
 	foc_obser.enc_speed = ABS(enc_vel);
-	return true;
+	return !foc_obser.is_sensorless_running;
 }
 
 float foc_observer_speed(void) {