PMSM_FOC_GetSpeed 判断无感模式使用不同的速度

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

Applications/foc/core/PMSM_FOC_Core.c

@@ -1135,7 +1135,17 @@ void PMSM_FOC_Get_TgtIDQ(DQ_t * dq) {
 }
 
 float PMSM_FOC_GetSpeed(void) {
-	return gFoc_Ctrl.in.s_motVelocity;
+	float speed = gFoc_Ctrl.in.s_motVelocity;
+	if (!gFoc_Ctrl.in.b_motEnable || foc_observer_is_encoder()) {
+		speed = motor_encoder_get_speed();
+	}else {
+		if (foc_observer_sensorless_stable()) {
+			speed = foc_observer_sensorless_speed();
+		}else {
+			speed = 0;
+		}
+	}
+	return speed;
 }
 
 

Applications/foc/motor/motor.c

@@ -289,7 +289,7 @@ bool mc_start(u8 mode) {
 		PMSM_FOC_SetErrCode(FOC_Param_Err);
 		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;
 	}
@@ -342,7 +342,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;
 	}

Applications/prot/can_foc_msg.c

@@ -7,19 +7,6 @@
 #include "foc/core/PMSM_FOC_Core.h"
 #include "foc/core/foc_observer.h"
 
-static float can_get_speed(void) {
-	float speed = PMSM_FOC_GetSpeed();
-	if (!PMSM_FOC_Is_Start() || foc_observer_is_encoder()) {
-		speed = motor_encoder_get_speed();
-	}else {
-		if (foc_observer_sensorless_stable()) {
-			speed = foc_observer_sensorless_speed();
-		}else {
-			speed = 0;
-		}
-	}
-	return speed;
-}
 
 void can_report_speed(u8 can, s16 rpm) {
 	u8 data[6];
@@ -30,7 +17,7 @@ void can_report_speed(u8 can, s16 rpm) {
 
 void can_report_power(u8 can) {
 	u8 data[8];
-	s16 rpm = (s16)can_get_speed();
+	s16 rpm = (s16)PMSM_FOC_GetSpeed();
 	float vDC = get_vbus_float();
 	float iDC = PMSM_FOC_GetVbusCurrent();	
 	s16 v = S16Q5(vDC);
@@ -111,7 +98,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)(can_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();
@@ -142,7 +129,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)(can_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();