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; }