;===========================================================================
; 文件名:	Speed_pr.asm
;
; 模塊名: 	SPEED_PRD			     
;
; 初始化程序: SPEED_PRD_INIT
;
; 公司:	達(dá)盛科技
;	
; 功能描述:  同過(guò)測(cè)量傳感器輸出的信號(hào)周期來(lái)計(jì)算電動(dòng)機(jī)的轉(zhuǎn)速
;			
;							|~~~~~~~~~~~~~~~~~|
;							|		  		  |----->o  speed_prd
;	   time_stamp o-------->|  SPEED_PRD 	  |
;							|		  		  |----->o  speed_rpm
;							|_________________|
;
;===========================================================================
; 更改記錄:
;---------------------------------------------------------------------------
; 2005.08.21	版本 Ver 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
;===========================================================================
;模塊外部變量聲明
		.def	SPEED_PRD, SPEED_PRD_INIT		;函數(shù)調(diào)用
		.def	time_stamp						;輸入
		.def	rpm_max, speed_scaler,shift		;參數(shù)
		.def	speed_prd, speed_rpm			;輸出
;===========================================================================
		.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