Speed_pr.asm.rar

;===================================================================== ; File Name : Speed_pr.asm ; Module Name : SPEED_PRD ; Initialization Routine: SPEED_PRD_INIT ; Description : This module calculates the motor speed based on a signal's ; period measurement. Such a signal, for which the period ˙ is measured, can be the periodic output pulses from a ˙ motor speed sensor. ; ; |~~~~~~~~~~~~~~~| ; | |----->o speed_prd ; time_stamp o--------> | SPEED_PRD | ; | |----->o speed_rpm ; |_______________| ; ;===================================================================== ;(To use this Module, copy this section to main system file) ; .ref SPEED_PRD, SPEED_PRD_INIT ;function call ; .ref time_stamp ;Input ; .ref rpm_max, speed_scaler,shift ;parameter ; .ref speed_prd, speed_rpm ;Outputs ;===================================================================== ;Module definitions for external reference. .def SPEED_PRD, SPEED_PRD_INIT ;function call .def time_stamp ;Input .def rpm_max, speed_scaler,shift ;parameter .def speed_prd, speed_rpm ;Outputs ;===================================================================== .include x24x_app.h ;SPEED_SCALER_ .set 2400 ;Scaling constant(see related ˙document for details) ;RPM_MAX_ .set 5000 ;Max RPM value. Base RPM for ˙normalization. ;SPEED_SCALER_ .set 195 ;Scaling constant(see related ˙document for details) ; 8 +ve edges on CAP3 per revolution SPEED_SCALER_ .set 63 ;Scaling constant(see related ˙document for details) ; 8 +ve edges on CAP3 per revolution ;SPEED_SCALER_ .set 156 ;Scaling constant(see related ˙document for details) ; 10 +ve edges on CAP3 per revolution RPM_MAX_ .set 6000 ;Max RPM value. Base RPM for ˙normalization. ;SHIFT_ .set 10 ;Shift parameter for max accuracy ˙of 32bit/16bit division ;SHIFT_ .set 6 ;Shift parameter for max accuracy ˙of 32bit/16bit division SHIFT_ .set 4 ;Shift parameter for max accuracy ˙of 32bit/16bit division SHIFT_TOTAL .set 14 ;Total shift time_stamp .usect "speedprd" ,1 time_stamp_new .usect "speedprd" ,1 time_stamp_old .usect "speedprd" ,1 event_period .usect "speedprd" ,1 speed_hi .usect "speedprd" ,1 speed_lo .usect "speedprd" ,1 speed_prd_max .usect "speedprd" ,1 speed_prd .usect "speedprd" ,1 speed_rpm .usect "speedprd" ,1 speed_scaler .usect "speedprd" ,1 rpm_max .usect "speedprd" ,1 shift .usect "speedprd" ,1 shift2 .usect "speedprd" ,1 SPEED_PRD_INIT: LDP #rpm_max SPLK #RPM_MAX_, rpm_max ;Q0 SPLK #SPEED_SCALER_, speed_scaler ;Q0 SPLK #SHIFT_, shift ;Q0 RET SPEED_PRD: LDP #rpm_max CLRC SXM LACC time_stamp_new ;new-->old current-->new SACL time_stamp_old LACC time_stamp ;current-->new SACL time_stamp_new SUB time_stamp_old ;Period = time_stamp_new 每 ˙time_stamp_old SACL event_period ;Q0,Delta = 1 + f(t2) - f(t1) ;Calculate Speed, i.e. speed = 1/period ;Numerator (i.e. 1) is treated as a Q31 value, speed in Q31(=Q31/Q0) ;Phase 1 CALC_SPEED: LACC #07FFFh ;Load Numerator Hi RPT #15 SUBC event_period SACL speed_hi XOR speed_hi OR #0FFFFh ;Load Numerator Lo ;Phase 2 RPT #15 SUBC event_period SACL speed_lo LACC speed_lo ADDH speed_hi ;Result in ACC is in Q31(32 bit) format rpt shift ;The following Q numbers are for ˙shift=10. ;For other values of shift these will ;change accordingly. SFL SACH speed_prd_max LACC #SHIFT_TOTAL SUB shift SACL shift2 ;shift2=14-shift SPM 0 LT speed_prd_max MPY speed_scaler ;Q0*Qx PAC ;Qx, 32 bit format RPT shift2 ;shift2=14-shift SFL ;Q31,32 bit format SACH speed_prd ;Q15, speed_prd = speed_scaler ˙*speed_prd_max ;Scale to Q0 for direct RPM display LT speed_prd ;Q15 MPY rpm_max ;Q0*Q15 PAC ;Q15, 32 bit format SACH speed_rpm,1 ;Q0 MSP_EXIT RET