;===========================================================================
; File Name:	park.asm
;
; Module Name: 	PARK					      
;
; Initialization Routine: PARK_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:	This transformation converts vectors in balanced 2-phase 
;		orthogonal stationary system into orthogonal rotating 
;		reference frame.
;
;			id = ialfa * cos_teta + ibeta * sin_teta
;			iq = -ialfa *sin_teta + ibeta * cos_teta
;
;			|~~~~~~~~~~~~~~~~~|
;	park_d	o------>|		  |----->o  park_D
;	park_q	o------>|      PARK	  |
;	theta_p	o------>|		  |----->o  park_Q
;			|_________________|
;
;	Note:	0 < theta_p < 7FFFh   (i.e. equivalent to   0 < theta_p < 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	PARK, PARK_INIT					;function call
;		.ref	park_d, park_q, theta_p				;Inputs
;		.ref	park_D, park_Q					;Outputs
;===========================================================================
;Module definitions for external reference.
		.def	PARK, PARK_INIT					;function call
		.def	park_d, park_q, theta_p				;Inputs
		.def	park_D, park_Q					;Outputs
;===========================================================================
;Options
;--------
High_precision	.set	0	;Set to 1 for High prec / Set to 0 for low prec

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


park_d		.usect "park",1
park_q		.usect "park",1
theta_p		.usect "park",1
park_D		.usect "park",1
park_Q		.usect "park",1
t_ptr		.usect "park",1
ip_val		.usect "park",1
cos_theta	.usect "park",1
sin_theta	.usect "park",1
nxt_entry	.usect "park",1
delta_angle	.usect "park",1
GPR0_park	.usect "park",1

;=====================================================================
PARK_INIT: 
;=====================================================================
		RET


;=====================================================================
PARK: 
;=====================================================================
;Calculate Cos(theta_p)
;--- High_precision option -------
	.if (High_precision)
	;Higher precision using look-up + interpolation method

		ldp	#theta_p
		LACC	theta_p
		ADD	#8192		;add 90 deg, i.e. COS(A)=SIN(A+90)
		AND	#07FFFh 	;Force positive wrap-around
		SACL	GPR0_park		;here 90 deg = 7FFFh/4
		LACC	GPR0_park,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_p
		LACC	theta_p
		ADD	#8192		;add 90 deg, i.e. COS(A)=SIN(A+90)
		AND	#07FFFh 	;Force positive wrap-around
		SACL	GPR0_park		;here 90 deg = 7FFFh/4
		LACC	GPR0_park,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_p,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_p,9
		SACH	t_ptr
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	sin_theta	;sin_theta = Sin(theta_p) in Q15
	.endif
;-----------------------------------

;Calculate the Park transform
		SPM	1		; SPM set for Q15 multiplication        
		ZAC			; Reset accumulator
		LT	park_q     	; TREG = ibeta
		MPY	sin_theta   	; PREG = ibeta * sin_teta
		LTA	park_d     	; ACC = ibeta * sin_teta and TREG = ialfa 
		MPY	cos_theta   	; PREG = ialfa * cos_teta
		MPYA	sin_theta   	; ACC = ibeta*sin_teta + ialfa*cos_teta and PREG=ialfa*sin_teta
		SACH	park_D		; id = ialfa * cos_teta + ibeta * sin_teta        
	
		LACC	#0          	; Clear ACC
		LT	park_q     	; TREG = ibeta
		MPYS	cos_theta   	; ACC = -ialfa*sin_teta and PREG = ibeta*cos_teta
		APAC              	; ACC = -ialfa *sin_teta + ibeta * cos_teta
		SACH	park_Q      ; iq = -ialfa *sin_teta + ibeta * cos_teta
		SPM	0           ; SPM reset
		RET