;=====================================================================
; File Name	:	speed_fr.asm			      
; Module Name	: 	SPEED_FRQ	
; Initialization Routine: SPEED_FRQ_INIT
; 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
;						|_______________|
;===================================================================== 
;(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