;=======================================================================
; File Name:	speed_fr.asm			      
;
; Module Name: 	SPEED_FRQ	
;
; Initialization Routine: SPEED_FRQ_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:  Speed Calculation based on electrical angle 	    	
;
;
;			          |~~~~~~~~~~~~~~~~~~~~|
;	                  |                 Q15|----->o speed_frq 
;theta_elec_fr o----->|Q15    SPEED_FRQ	 Q0|----->o speed_frq_rpm
;	         	      |____________________|
;
; Target dependency:	C2xx core only
;
;===========================================================================
; History:
;---------------------------------------------------------------------------
; 08-26-2002	Release	Rev 1.0	
;===========================================================================
;(To use this Module, copy this section to main system file)
;		.ref	SPEED_FRQ, SPEED_FRQ_INIT		;function call
;		.ref	theta_elec_fr          			;Inputs
;		.ref	speed_frq, speed_frq_rpm	    ;Outputs
;===========================================================================
;Module definitions for external reference.
		.def	SPEED_FRQ, SPEED_FRQ_INIT		;function call
		.def	theta_elec_fr          			;Inputs
		.def	speed_frq, speed_frq_rpm	    ;Outputs
;===========================================================================
K1_fr_		.set	6400		; for K1_fr (Q6) 
K2_fr_		.set	32162		; for K2_fr (Q15)	
K3_fr_		.set	606 	    ; for K3_fr (Q15)

BASE_RPM_FR_ 	 .set 	6000		; Base rpm speed (Q0)	
DIFF_MAX_LIMIT   .set   7333h       ; The maximum limit to differentiate theta_r_se (Q15)
DIFF_MIN_LIMIT   .set   0CCDh       ; The minimum limit to differentiate theta_r_se (Q15)
         
		.include	x24x_app.h
            
theta_elec_fr   .usect	"speed_fr",1 	; Electrical angle                      (Q15)
speed_frq       .usect	"speed_fr",1 	; Estimated rotor speed                 (Q15)
speed_frq_rpm   .usect	"speed_fr",1    ; Estimated rotor speed in rpm          (Q0)
K1_fr			.usect	"speed_fr",1	; Constant using in differentiator      (Q6)
K2_fr			.usect	"speed_fr",1	; Constant using in low-pass filter     (Q15)
K3_fr			.usect	"speed_fr",1	; Constant using in low-pass filter     (Q15)
rpm_max_fr      .usect	"speed_fr",1    ; Base speed in rpm                        (Q0)
theta_old       .usect	"speed_fr",1	; Previous rotor flux angle               (Q15)
min_theta_fr    .usect	"speed_fr",1    ; Minimum limit for diffirential rotor flux angle (Q15)
max_theta_fr    .usect	"speed_fr",1    ; Maximum limit for diffirential rotor flux angle (Q15)
tmp1_fr			.usect	"speed_fr",1	; Temporary variable

;=========================
SPEED_FRQ_INIT:
;=========================    
	LDP		#K1_fr    
	SPLK	#K1_fr_,K1_fr	     	; K1 = 1/(fb*T)                (Q6)
  	SPLK	#K2_fr_,K2_fr       	; K2 = Tau/(Tau+T)             (Q15)
  	SPLK	#K3_fr_,K3_fr       	; K3 = T/(Tau+T)               (Q15)
	SPLK	#BASE_RPM_FR_,rpm_max_fr  ; Initialize the base speed  (Q0)	
    SPLK	#DIFF_MAX_LIMIT,max_theta_fr ; Initialize the max limit of rotor flux angle  (Q15)	
    SPLK	#DIFF_MIN_LIMIT,min_theta_fr ; Initialize the min limit of rotor flux angle  (Q15)    
    SPLK	#0h,theta_old           ; Initialize the rotor flux angle  (Q15)
 
    RET
                   
;=========================
SPEED_FRQ:
;=========================    
   		SPM		0 				; Reset product mode 
		SETC	SXM				; Set sign extension mode
		SETC	OVM				; Set overflow mode

		LDP		#max_theta_fr
		
; Differentiator
; Check theta_r_se is within the differentiable range (min_theta,max_theta)
		LACC	max_theta_fr
		SUB		theta_elec_fr
		BCND	LESS_MAX_FR,GT		; Branch to LESS_MAX if max_theta > theta_elec 
        B		SKIP_DIFF_FR
LESS_MAX_FR
    	LACC	min_theta_fr
    	SUB		theta_elec_fr
    	BCND	MORE_MIN_FR,LT    ; Branch to MORE_MIN if min_theta < theta_elec 
        B		SKIP_DIFF_FR
MORE_MIN_FR	
		; Differentiating the rotor flux angle 
		; if min_theta < theta_elec < max_theta
		LT		K1_fr			; TREG = K1                (Q6)
		MPY		theta_elec_fr   ; PREG = K1*theta(k)     (Q21)
		PAC						; ACC = K1*theta(k)      (Q21)
		MPY		theta_old       ; PREG = K1*theta(k-1)   (Q21)
		SPAC					; ACC = K1*theta(k)-K1*theta(k-1)   (Q21)
		SFL               		; ACC = K1*theta(k)-K1*theta(k-1)   (Q22)
		SFL               		; ACC = K1*theta(k)-K1*theta(k-1)   (Q23)
		SFL               		; ACC = K1*theta(k)-K1*theta(k-1)   (Q24)
		SACH	tmp1_fr,7       ; tmp1_fr = K1*theta(k)-K1*theta(k-1)   (Q15)
		; Low-pass filter
		LT		K2_fr			; TREG = K2                (Q15)
		MPY		speed_frq       ; PREG = K2*speed_frq(k-1)   (Q30)
        PAC						; ACC =  K2*w_psi_r(k-1)     (Q30)
        LT		K3_fr			; TREG = K3                  (Q15)
        MPY		tmp1_fr         ; PREG = K3*tmp1_fr          (Q30)
        APAC					; ACC = K2*speed_frq(k-1)+K3*tmp1_fr           (Q30)
        SACH	speed_frq,1     ; speed_frq(k) = K2*speed_frq(k-1)+K3*tmp1_fr  (Q15)
SKIP_DIFF_FR

; Update the rotor flux angle
		LACC	theta_elec_fr	; ACC = theta_elec             (Q15)
		SACL	theta_old       ; theta_old = theta_elec      (Q15)
; Change motor speed from pu value to rpm value (Q15 -> Q0)
		LT		rpm_max_fr   	; TREG = rpm_max_fr             (Q0)
		MPY	    speed_frq    	; PREG = rpm_max_fr*speed_frq    (Q15)
		PAC						; ACC = rpm_max_fr*speed_frq     (Q15)	
		SACH	speed_frq_rpm,1	; speed_frq_rpm = rpm_max_fr*speed_frq  (Q0)
       
	    RET