默认开启电压相角补偿

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

Applications/foc/core/PMSM_FOC_Core.c

@@ -336,7 +336,8 @@ static __INLINE void PMSM_FOC_Calc_iDC_Fast(void) {
 }
 
 //#define UPDATE_Lq_By_iq   /* Q轴电感 通过Iq电流补偿 */
-//#define Volvec_Delay_Comp /* 电压矢量角度补偿 */
+#define Volvec_Delay_Comp /* 电压矢量角度补偿 */
+#define Volvec_Delay_Comp_Start_Vel 500 // rpm
 static __INLINE bool PMSM_FOC_Update_Input(void) {
 	AB_t iAB;
 	float *iabc = gFoc_Ctrl.in.s_iABC;
@@ -366,8 +367,8 @@ static __INLINE bool PMSM_FOC_Update_Input(void) {
 		gFoc_Ctrl.in.s_motAngle = enc_angle;
 	}
 	gFoc_Ctrl.in.s_motVelocity = enc_vel;
-	LowPass_Filter(gFoc_Ctrl.in.s_motRPMFilted, gFoc_Ctrl.in.s_motVelocity, 0.005f);
-	gFoc_Ctrl.in.s_motVelDegreePers = gFoc_Ctrl.in.s_motVelocity / 30.0f * PI * gFoc_Ctrl.params.n_poles;
+	LowPass_Filter(gFoc_Ctrl.in.s_motRPMFilted, gFoc_Ctrl.in.s_motVelocity, 0.01f);
+	gFoc_Ctrl.in.s_motVelDegreePers = gFoc_Ctrl.in.s_motRPMFilted / 30.0f * PI * gFoc_Ctrl.params.n_poles;
 #ifdef CONFIG_DQ_STEP_RESPONSE
 	gFoc_Ctrl.in.s_motAngle = 0;
 #endif
@@ -383,9 +384,11 @@ static __INLINE bool PMSM_FOC_Update_Input(void) {
 	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, 0.004f);
 	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, 0.004f);
 #ifdef Volvec_Delay_Comp
-	float next_angle = gFoc_Ctrl.in.s_motAngle + gFoc_Ctrl.in.s_motVelDegreePers / PI * 180.0f * (FOC_CTRL_US - 2e-6f);
-	rand_angle(next_angle);
-	SinCos_Lut(next_angle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
+	if (gFoc_Ctrl.in.s_motRPMFilted >= Volvec_Delay_Comp_Start_Vel) {
+		float next_angle = gFoc_Ctrl.in.s_motAngle + gFoc_Ctrl.in.s_motVelDegreePers / PI * 180.0f * (FOC_CTRL_US * 0.8f);
+		rand_angle(next_angle);
+		SinCos_Lut(next_angle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
+	}
 #endif
 	return true;
 }