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