|
@@ -367,8 +367,8 @@ static __INLINE bool PMSM_FOC_Update_Input(void) {
|
|
|
gFoc_Ctrl.in.s_motAngle = enc_angle;
|
|
gFoc_Ctrl.in.s_motAngle = enc_angle;
|
|
|
}
|
|
}
|
|
|
gFoc_Ctrl.in.s_motVelocity = enc_vel;
|
|
gFoc_Ctrl.in.s_motVelocity = enc_vel;
|
|
|
- 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;
|
|
|
|
|
|
|
+ LowPass_Filter(gFoc_Ctrl.in.s_motVelocityFiltered, gFoc_Ctrl.in.s_motVelocity, 0.01f);
|
|
|
|
|
+ gFoc_Ctrl.in.s_motVelDegreePers = gFoc_Ctrl.in.s_motVelocityFiltered / 30.0f * PI * gFoc_Ctrl.params.n_poles;
|
|
|
#ifdef CONFIG_DQ_STEP_RESPONSE
|
|
#ifdef CONFIG_DQ_STEP_RESPONSE
|
|
|
gFoc_Ctrl.in.s_motAngle = 0;
|
|
gFoc_Ctrl.in.s_motAngle = 0;
|
|
|
#endif
|
|
#endif
|
|
@@ -384,7 +384,7 @@ 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.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);
|
|
LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, 0.004f);
|
|
|
#ifdef Volvec_Delay_Comp
|
|
#ifdef Volvec_Delay_Comp
|
|
|
- if (gFoc_Ctrl.in.s_motRPMFilted >= Volvec_Delay_Comp_Start_Vel) {
|
|
|
|
|
|
|
+ if (gFoc_Ctrl.in.s_motVelocityFiltered >= 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);
|
|
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);
|
|
rand_angle(next_angle);
|
|
|
SinCos_Lut(next_angle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
|
|
SinCos_Lut(next_angle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
|