;===========================================================================
; File Name:	I_park.asm				
;
; Module Name: 	I_PARK	
;
; Initialization Routine: I_PARK_INIT
;
;
; Description:	This transformation projects vectors in orthogonal rotating 
;		reference frame into two phase orthogonal stationary frame.
;
; 			id = ialfa * cos_teta - ibeta * sin_teta
; 			iq = ialfa *sin_teta + ibeta * cos_teta
;
;			|~~~~~~~~~~~~~~~|
;	ipark_D	o------>|		|----->o  ipark_d
;	ipark_Q	o------>|      I_PARK   |
;	theta_ipo------>|		|----->o  ipark_q
;			|_______________|
;
;	Note:	0 < theta_ip < 7FFFh   (i.e. equivalent to   0 < theta_ip < 360 deg  )
;
;	
;
; Target dependency:	C2xx core only
;
;===========================================================================
;Module definitions for external reference.
;		.ref	I_PARK, I_PARK_INIT					;function call
;		.ref	ipark_D, ipark_Q,cos_theta,sin_theta	;Inputs
;		.ref	ipark_d, ipark_q						;Outputs
;===========================================================================
;Module definitions for external reference.
		.def	I_PARK, I_PARK_INIT					;function call
		.def	ipark_D, ipark_Q, sin_theta1,cos_theta1	;Inputs
		.def	ipark_d, ipark_q						;Outputs
        .def	theta_ip
        .ref	SINTAB_360
;===========================================================================
	  .include	x24x_app.h
		
ipark_d			.usect "I_park",1
ipark_q			.usect "I_park",1
ipark_D			.usect "I_park",1
ipark_Q			.usect "I_park",1
theta_ip		.usect "I_park",1
t_ptr1				.usect "I_park",1
ip_val			.usect "I_park",1
cos_theta1		.usect "I_park",1
sin_theta1		.usect "I_park",1
nxt_entry		.usect "I_park",1
delta_angle	.usect "I_park",1
GPR0_ipark	.usect "I_park",1
;=====================================================================
I_PARK_INIT: 
;=====================================================================
		ldp	#ipark_d
		SPLK	#3FFFh, ipark_D
		SPLK	#3FFFh, ipark_Q
		RET

;======================================================================
I_PARK: 
;======================================================================
;Calculate Cos(theta_p)
;Normal precision with simple 1024 word look-up
	

;Calculate the Inverse Park transform

		SETC	SXM					; Sign extension mode
		SPM	1							; SPM set for Q15 multiplication
 
;park_q = ipark_Q * cos_theta + ipark_D * sin_theta
 
		LACC	#0					; Clear ACC
		LT	ipark_D				; TREG = Udref
		MPY	sin_theta1			; PREG = Udref * sin_theta
		LTA	ipark_Q				; ACC = Udref*sin_theta and TREG=Uqref
		MPY	cos_theta1			; PREG = Uqref * cos_teta
		MPYS	sin_theta1		; ACC = Uqref*cos_theta + Udref*sin_theta and TREG=Uqref*sin_theta 
		SACH	ipark_d			; Ubeta = Uqref*cos_theta + Udref*sin_theta 
		 
;park_d = ipark_D * cos_theta - ipark_Q * sin_theta		
		LACC	#0					; Clear ACC
		LT	ipark_D				; TREG = Udref
		MPYS	cos_theta1		; ACC = -Uqref*sin_theta and PREG = Udref*cos_theta 
		SPAC							; ACC = -Uqref*sin_theta + Udref*cos_theta 
		SACH	ipark_q			; Ualfa = -Uqref*sin_theta + Udref*cos_theta
		
		SPM	0							; SPM reset
		RET