;===========================================================================
; File Name:	I_park.asm				
;
; Module Name: 	I_PARK	
;
; Initialization Routine: I_PARK_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; 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
;
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0	
;===========================================================================
;(To use this Module, copy this section to main system file)
;		.ref	I_PARK, I_PARK_INIT				;function call
;		.ref	ipark_D, ipark_Q, theta_ip			;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, theta_ip			;Inputs
		.def	ipark_d, ipark_q					;Outputs
;===========================================================================
;Options
;--------
High_precision	.set	0	;Set to 1 for High prec / Set to 0 for low prec

;===========================================================================
		
		.ref	SINTAB_360


		
ipark_d		.usect "I_park",1
ipark_q		.usect "I_park",1
theta_ip	.usect "I_park",1
ipark_D		.usect "I_park",1
ipark_Q		.usect "I_park",1

t_ptr		.usect "I_park",1
ip_val		.usect "I_park",1
cos_theta	.usect "I_park",1
sin_theta	.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)
;--- High_precision option -------
	.if (High_precision)
	;Higher precision using look-up + interpolation method
		ldp	#theta_ip
		LACC	theta_ip
		ADD	#8192		;add 90 deg, i.e. COS(A)=SIN(A+90)
		AND	#07FFFh 	;Force positive wrap-around
		SACL	GPR0_ipark		;here 90 deg = 7FFFh/4
		LACC	GPR0_ipark,9
		SACH	t_ptr		;Table pointer
		SFR			;Convert Interpolation value(ip_val) to Q15
		AND	#07FFFh 	;Force ip_val to a positive number
		SACL	ip_val
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	cos_theta	;cos_theta = Cos(theta) in Q15
		ADD	#1h		;Inc Table pointer
		TBLR	nxt_entry	;Get next entry i.e. (Entry + 1)
		LACC	nxt_entry
		SUB	cos_theta	;Find Delta of 2 points
		SACL	delta_angle
		LT	delta_angle
		MPY	ip_val		;ip_val = interpolation value
		PAC
		SACH	ip_val,1
		LACC	ip_val
		ADD	cos_theta
		SACL	cos_theta	;cos_theta = Final interpolated value
	.endif
;-----------------------------------

;--- Normal precision option -------
	.if (High_precision != 1)
	;Normal precision with simple 256 word look-up
		ldp	#theta_ip
		LACC	theta_ip
		ADD	#8192		;add 90 deg, i.e. COS(A)=SIN(A+90)
		AND	#07FFFh 	;Force positive wrap-around
		SACL	GPR0_ipark	;here 90 deg = 7FFFh/4
		LACC	GPR0_ipark,9
		SACH	t_ptr
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	cos_theta	;cos_theta = Cos(theta_p) in Q15
	.endif
;-----------------------------------

;Calculate Sin(theta_p)
;--- High_precision option -------
	.if (High_precision)
	;Higher precision using look-up + interpolation method
		LACC	theta_ip,9
		SACH	t_ptr		;Table pointer
		SFR			;Convert Interpolation value(ip_val) to Q15
		AND	#07FFFh 	;Force ip_val to a positive number
		SACL	ip_val
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	sin_theta	;sin_theta = Sin(theta) in Q15
		ADD	#1h		;Inc Table pointer
		TBLR	nxt_entry	;Get next entry i.e. (Entry + 1)
		LACC	nxt_entry
		SUB	sin_theta	;Find Delta of 2 points
		SACL	delta_angle
		LT	delta_angle
		MPY	ip_val		;ip_val = interpolation value
		PAC
		SACH	ip_val,1
		LACC	ip_val
		ADD	sin_theta
		SACL	sin_theta		;sin_theta = Final interpolated value
	.endif
;-----------------------------------

;--- Normal precision option -------
	.if (High_precision != 1)
	;Lower precision simple 256 word look-up
		LACC	theta_ip,9
		SACH	t_ptr
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	sin_theta		;sin_theta = Sin(theta_p) in Q15
	.endif
;-----------------------------------


;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_theta		; PREG = Udref * sin_theta
		LTA	ipark_Q			; ACC = Udref*sin_theta and TREG=Uqref
		MPY	cos_theta		; PREG = Uqref * cos_teta
		MPYA	sin_theta		; ACC = Uqref*cos_theta + Udref*sin_theta and TREG=Uqref*sin_theta 
		SACH	ipark_q			; 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_theta		; ACC = -Uqref*sin_theta and PREG = Udref*cos_theta 
		APAC				; ACC = -Uqref*sin_theta + Udref*cos_theta 
		SACH	ipark_d			; Ualfa = -Uqref*sin_theta + Udref*cos_theta
		
		SPM	0			; SPM reset
		RET