;=====================================================================
; Filename	: 	PARK.asm
; Module Name	: 	PARK					      
;
； Initialization Routine: PARK_INIT
; 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)
;=====================================================================
;(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
;=====================================================================
;Option
;--------
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