void calculateShiftAmount(uint32_t timer, int64_t *cosAccumulator, int64_t *sinAccumulator, uint8_t *shiftAmt)
{
shiftAmt[timer] = 0;
uint16_t mask = 0xFFFF;
uint16_t cosAccumTopBits = (uint16_t)_Q15abs(*((_Q15 *)(cosAccumulator + timer) + 2));
uint16_t sinAccumTopBits = (uint16_t)_Q15abs(*((_Q15 *)(sinAccumulator + timer) + 2));
while(cosAccumTopBits != 0x0000 || sinAccumTopBits != 0x0000)
{
++shiftAmt[timer];
if (shiftAmt[timer] == 15)
{
break;
}
mask = mask << 1;
cosAccumTopBits = mask & cosAccumTopBits;
sinAccumTopBits = mask & sinAccumTopBits;
}
++shiftAmt[timer];
}
void DMA1_Channel1_IRQHandler(void)
{
/* Test DMA1 TC flag */
if((DMA_GetFlagStatus(DMA1_FLAG_TC1)) != RESET )
{
/* Clear DMA TC flag */
DMA_ClearFlag(DMA1_FLAG_TC1);
if(Motor_State==INIT)
{
ADCTemp_Init(ADC_Tab);
}
else
{
LED1_ON();
SensorlessFOCRUN();
LED1_OFF();
}
SVM_Angle=smc1.Theta;
//DAC_SetChannel1Data(DAC_Align_12b_R,SVM_Angle/16);
DAC_SetChannel1Data(DAC_Align_12b_R,ADC_Tab[IB_Channl]);
if(++T2ms_Temp>T2MSTEMP) //2ms
{
T2ms_Temp=0;
T2ms_Flag=1;
Error_OMEGA=smc1.Omega-OMEGA_Old;
OMEGA_Old=smc1.Omega;
if(uGF.bit.RunMotor&&(uGF.bit.OpenLoop==0)) //ÅжÏÕýÐþÕðµ´
{
if(_Q15abs(Error_OMEGA)>ERROROMEGAMIN)
{
uGF.bit.RunMotor = 0;
uGF.bit.MotorFail = 1;
}
}
}else{}
if(++T100ms_Temp>T100MSTEMP) //100ms
{
T100ms_Temp=0;
T100ms_Flag=1;
}else{}
}
}
void SMC_Position_Estimation (SMC *s)
{
PUSHCORCON();
CORCONbits.SATA = 1;
CORCONbits.SATB = 1;
CORCONbits.ACCSAT = 1;
CalcEstI();
CalcIError();
// Sliding control calculator
if (_Q15abs(s->IalphaError) < s->MaxSMCError)
{
// s->Zalpha = (s->Kslide * s->IalphaError) / s->MaxSMCError
// If we are in the linear range of the slide mode controller,
// then correction factor Zalpha will be proportional to the
// error (Ialpha - EstIalpha) and slide mode gain, Kslide.
CalcZalpha();
}
else if (s->IalphaError > 0)
s->Zalpha = s->Kslide;
else
s->Zalpha = -s->Kslide;
if (_Q15abs(s->IbetaError) < s->MaxSMCError)
{
// s->Zbeta = (s->Kslide * s->IbetaError) / s->MaxSMCError
// If we are in the linear range of the slide mode controller,
// then correction factor Zbeta will be proportional to the
// error (Ibeta - EstIbeta) and slide mode gain, Kslide.
CalcZbeta();
}
else if (s->IbetaError > 0)
s->Zbeta = s->Kslide;
else
s->Zbeta = -s->Kslide;
// Sliding control filter -> back EMF calculator
// s->Ealpha = s->Ealpha + s->Kslf * s->Zalpha -
// s->Kslf * s->Ealpha
// s->Ebeta = s->Ebeta + s->Kslf * s->Zbeta -
// s->Kslf * s->Ebeta
// s->EalphaFinal = s->EalphaFinal + s->KslfFinal * s->Ealpha
// - s->KslfFinal * s->EalphaFinal
// s->EbetaFinal = s->EbetaFinal + s->KslfFinal * s->Ebeta
// - s->KslfFinal * s->EbetaFinal
CalcBEMF();
// Rotor angle calculator -> Theta = atan(-EalphaFinal,EbetaFinal)
s->Theta = atan2CORDIC(-s->EalphaFinal,s->EbetaFinal);
AccumTheta += s->Theta - PrevTheta;
PrevTheta = s->Theta;
AccumThetaCnt++;
if (AccumThetaCnt == IRP_PERCALC)
{
s->Omega = AccumTheta;
AccumThetaCnt = 0;
AccumTheta = 0;
}
// Q15(Omega) * 60
// Speed RPMs = -----------------------------
// SpeedLoopTime * Motor Poles
// For example:
// Omega = 0.5
// SpeedLoopTime = 0.001
// Motor Poles (pole pairs * 2) = 10
// Then:
// Speed in RPMs is 3,000 RPMs
// s->OmegaFltred = s->OmegaFltred + FilterCoeff * s->Omega
// - FilterCoeff * s->OmegaFltred
CalcOmegaFltred();
// Adaptive filter coefficients calculation
// Cutoff frequency is defined as 2*_PI*electrical RPS
//
// Wc = 2*_PI*Fc.
// Kslf = Tpwm*2*_PI*Fc
//
// Fc is the cutoff frequency of our filter. We want the cutoff frequency
// be the frequency of the drive currents and voltages of the motor, which
// is the electrical revolutions per second, or eRPS.
//
// Fc = eRPS = RPM * Pole Pairs / 60
//
// Kslf is then calculated based on user parameters as follows:
// First of all, we have the following equation for RPMS:
//
// RPM = (Q15(Omega) * 60) / (SpeedLoopTime * Motor Poles)
// Let us use: Motor Poles = Pole Pairs * 2
// eRPS = RPM * Pole Pairs / 60), or
// eRPS = (Q15(Omega) * 60 * Pole Pairs) / (SpeedLoopTime * Pole Pairs * 2 * 60)
// Simplifying eRPS
// eRPS = Q15(Omega) / (SpeedLoopTime * 2)
// Using this equation to calculate Kslf
// Kslf = Tpwm*2*_PI*Q15(Omega) / (SpeedLoopTime * 2)
// Using diIrpPerCalc = SpeedLoopTime / Tpwm
// Kslf = Tpwm*2*Q15(Omega)*_PI / (diIrpPerCalc * Tpwm * 2)
// Simplifying:
// Kslf = Q15(Omega)*_PI/diIrpPerCalc
//
// We use a second filter to get a cleaner signal, with the same coefficient
//
// Kslf = KslfFinal = Q15(Omega)*_PI/diIrpPerCalc
//
// What this allows us at the end is a fixed phase delay for theta compensation
// in all speed range, since cutoff frequency is changing as the motor speeds up.
//
// Phase delay: Since cutoff frequency is the same as the input frequency, we can
// define phase delay as being constant of -45 DEG per filter. This is because
// the equation to calculate phase shift of this low pass filter is
// arctan(Fin/Fc), and Fin/Fc = 1 since they are equal, hence arctan(1) = 45 DEG.
// A total of -90 DEG after the two filters implemented (Kslf and KslfFinal).
s->Kslf = s->KslfFinal = FracMpy(s->OmegaFltred,Q15(_PI / IRP_PERCALC));
// Since filter coefficients are dynamic, we need to make sure we have a minimum
// so we define the lowest operation speed as the lowest filter coefficient
if (s->Kslf < Q15(OMEGA0 * _PI / IRP_PERCALC))
{
s->Kslf = Q15(OMEGA0 * _PI / IRP_PERCALC);
s->KslfFinal = Q15(OMEGA0 * _PI / IRP_PERCALC);
}
// Theta compensation for different speed ranges. These are defined
// based on Minimum and Maximum operating speed defined in UserParms.h
if (s->OmegaFltred < Q15(OMEGA0))
{
s->ThetaOffset = FracMpy(s->OmegaFltred,SLOPEFRAC0);
s->ThetaOffset += s->OmegaFltred * SLOPEINT0;
s->ThetaOffset += CONSTANT0;
}
else if (s->OmegaFltred < Q15(OMEGA1))
{
s->ThetaOffset = FracMpy(s->OmegaFltred,SLOPEFRAC1);
s->ThetaOffset += s->OmegaFltred * SLOPEINT1;
s->ThetaOffset += CONSTANT1;
}
else if (s->OmegaFltred < Q15(OMEGA2))
{
s->ThetaOffset = FracMpy(s->OmegaFltred,SLOPEFRAC2);
s->ThetaOffset += s->OmegaFltred * SLOPEINT2;
s->ThetaOffset += CONSTANT2;
}
else if (s->OmegaFltred < Q15(OMEGA3))
{
s->ThetaOffset = FracMpy(s->OmegaFltred,SLOPEFRAC3);
s->ThetaOffset += s->OmegaFltred * SLOPEINT3;
s->ThetaOffset += CONSTANT3;
}
else if (s->OmegaFltred < Q15(OMEGA4))
{
s->ThetaOffset = FracMpy(s->OmegaFltred,SLOPEFRAC4);
s->ThetaOffset += s->OmegaFltred * SLOPEINT4;
s->ThetaOffset += CONSTANT4;
}
else if (s->OmegaFltred < Q15(OMEGA5))
{
s->ThetaOffset = FracMpy(s->OmegaFltred,SLOPEFRAC5);
s->ThetaOffset += s->OmegaFltred * SLOPEINT5;
s->ThetaOffset += CONSTANT5;
}
else
s->ThetaOffset = DEFAULTCONSTANT;
s->Theta = s->Theta + s->ThetaOffset;
POPCORCON();
return;
}
