Uint16 CalcSpeed(void) { Uint16 DirectionQep; Uint32 tmp,tmp1; DirectionQep=EQep1Regs.QEPSTS.bit.QDF; // Motor direction: 0=CCW/reverse, 1=CW/forward // Check an index occurrence if (EQep1Regs.QFLG.bit.IEL == 1) { EQep1Regs.QCLR.bit.IEL=1; // Clear interrupt flag } if(EQep1Regs.QFLG.bit.UTO==1) // If unit timeout (one 100Hz period) { /** Differentiator **/ newp=(unsigned long)EQep1Regs.QPOSLAT; // Latched POSCNT value if(DirectionQep==0) { if(newp > oldp) tmp=4294967295-newp +oldp; else tmp=oldp - newp; } else if(DirectionQep==1) { if(newp > oldp) tmp=newp - oldp; else tmp=4294967295+newp-oldp; } SpeedRpm_fr=0.6*tmp; oldp=newp; EQep1Regs.QCLR.bit.UTO=1; // Clear interrupt flag } //low speed if(EQep1Regs.QEPSTS.bit.UPEVNT==1) // Unit position event { if(EQep1Regs.QEPSTS.bit.COEF==0) // No Capture overflow tmp1=(unsigned int)EQep1Regs.QCPRDLAT; // temp1 = t2-t1 else // Capture overflow, saturate the result tmp1=0xFFFF; SpeedRpm_pr=225000/tmp1; EQep1Regs.QEPSTS.all=0x88; // Clear Unit position event flag // Clear overflow error flag } } //=========================================================================== // No more. //===========================================================================