;===========================================================================
; File Name:	speed_fr1.asm			      
;
; Module Name: 	SPEED_FRQ	
;
; Initialization Routine: SPEED_FRQ_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description: This module calculates motor speed based on measurement 
;		of frequency of the signal generated by a speed sensor.
;		The frequency of the speed sensor signal is the number 
;		of pulses generated per sec, which is again proportional 
;		to the angular displacement of the sensor disk and hence 
;		that of the rotor. Therefore, this module gets it's input 
;		as rotor shaft displacements (shaft_angle) for a known 
;		time interval and then uses these information to calculate
;		the motor speed. Samples the position once every 
;		(SPEED_LP_MAX x Speed loop sampling time period) sec.
;
;				|~~~~~~~~~~~~~~~|
;	shaft_angle	o------>|		|----->o  speed_frq
;	  			| SPEED_FRQ	|
;	  direction o---------->|  		|----->o  speed_frq_rpm
;				|_______________|
; 
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0	
;===========================================================================
;(To use this Module, copy this section to main system file)
;		.ref	SPEED_FRQ, SPEED_FRQ_INIT		;function call
;		.ref	shaft_angle, direction			;Inputs
;		.ref	speed_frq, speed_frq_rpm		;Outputs
;===========================================================================
;Module definitions for external reference.
		.def	SPEED_FRQ, SPEED_FRQ_INIT		;function call
		.def	shaft_angle, direction			;Inputs
		.def	speed_frq, speed_frq_rpm		;Outputs
		.def	SPEED_LP_MAX, rpm_scaler
;===========================================================================

		.include	x24x_app.h

SPEED_LP_MAX_	.set	200		;200 x 50uS = 0.010 sec
RPM_SCALER_	.set	6000


shaft_angle	.usect	"speed_fr",1
s_angle_cur	.usect	"speed_fr",1
s_angle_old	.usect	"speed_fr",1
direction	.usect	"speed_fr",1
speed_frq	.usect	"speed_fr",1
speed_frq_rpm	.usect	"speed_fr",1
rpm_scaler	.usect	"speed_fr",1
speed_skp_cnt	.usect	"speed_fr",1
SPEED_LP_MAX	.usect	"speed_fr",1

;===========================================================================
SPEED_FRQ_INIT:
;===========================================================================
		ldp	#rpm_scaler
		SPLK	#RPM_SCALER_, rpm_scaler
		splk	#SPEED_LP_MAX_, SPEED_LP_MAX
		RET

;===========================================================================
SPEED_FRQ:
;===========================================================================
		ldp	#speed_skp_cnt
		LACC	speed_skp_cnt
		ADD	#1
		SACL	speed_skp_cnt
		SUB	SPEED_LP_MAX
		BCND	MS_FRQ_END, LT		;Skip the Speed meas.

MSF_1		LACC	s_angle_cur
		SACL	s_angle_old
		LACC	shaft_angle
		SACL	s_angle_cur
		LACC	direction
		BCND	UP_CNT, NEQ		;If not=0, then Up-count

;---------------------------
DWN_CNT:
;---------------------------
		LACC	s_angle_cur
		SUB	s_angle_old
		BCND	DNEG_DELTA, LT		;If negative

DPOS_DELTA:
		SUB	#7FFFh			;Sub 1 from Delta
		SACL	speed_frq		;Delta = f(t2) - f(t1) - 1
		B	SM_SCALE

DNEG_DELTA:
		SACL	speed_frq		;Delta = f(t2) - f(t1)
		B	SM_SCALE


;---------------------------
UP_CNT:
;---------------------------
		LACC	s_angle_cur
		SUB	s_angle_old
		BCND	UNEG_DELTA, LT		;If negative

UPOS_DELTA	SACL	speed_frq		;Delta = f(t2) - f(t1)
		B	SM_SCALE

UNEG_DELTA	ADD	#7FFFh			;Add 1 to Delta
		SACL	speed_frq		;Delta = 1 + f(t2) - f(t1)


SM_SCALE:
		LT	speed_frq
		MPY	rpm_scaler
		PAC
		SACH	speed_frq_rpm,1

		SPLK	#0, speed_skp_cnt	;reset count		

MS_FRQ_END:
		RET