;=====================================================================
; Filename	: 	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_ip	o---->	|		    		|----->o  ipark_q
;						|_______________|
;
; Note: 0<theta_ip<7FFFh   (i.e. equivalent to 0<theta_ip<360 deg  )
;=====================================================================
;(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		; Sine table

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 REG=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