;===========================================================================
; File Name:	Speed_pr.asm
;
; Module Name: 	SPEED_PRD			     
;
; Initialization Routine: SPEED_PRD_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;	
; 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
;				|_________________|
;
;
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release Rev 1.0	
;===========================================================================
;(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	156	;Scaling constant(see related document for details)
RPM_MAX_	.set	3600	;Max RPM value. Base RPM for normalization.
SHIFT_		.set	6	;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
;		BCND	NEG_DELTA, LT		;If Period is negative, allow "wrapping"

;POS_DELTA	SACL	event_period		;Q0,Delta = f(t2) - f(t1)
;		B	CALC_SPEED

;NEG_DELTA	ADD	#7FFFh			;Add 1 to Delta
		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				;Q42
		SACH	speed_prd_max		;Q26

		LACC	#SHIFT_TOTAL  
		SUB	shift
		SACL	shift2			;shift2=14-shift

		SPM	0
		LT    	speed_prd_max		;Q26
		MPY	speed_scaler		;Q0*Q26 
		PAC   				;Q26, 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