;=====================================================================
; Filename	:	clarke.asm
; Module Name	: 	CLARKE
; Initialization Routine: CLARKE_INIT
; Description	:	Converts balanced three phase quantities into balanced 
;					two phase quadrature quantities.
;					(a,b,c) -> (d,q) Transformation
;					clark_d = clark_a
;					clark_q = (2 * clark_b + clark_b) / sqrt(3)
;
;					|~~~~~~~~~~~~~~~|
; clark_a  o----->|		     	|----->o  clark_d
; clark_b  o----->|     CLARKE    	|
; clark_c* o----->|		     	|----->o  clark_q
;					|_______________|
;
;=====================================================================
;(To use this Module, copy this section to main system file)
;		.ref	CLARKE, CLARKE_INIT			; function call
;		.ref	clark_a, clark_b			; Inputs
;		.ref	clark_d, clark_q			; Outputs
;=====================================================================
;Module definitions for external reference.
		.def	CLARKE, CLARKE_INIT		; function call
		.def	clark_a, clark_b			; Inputs
		.def	clark_d, clark_q			; Outputs
;=====================================================================
			.include "x24x_app.h"

; Variables     
clark_a	.usect "clarke",1
clark_b	.usect "clarke",1
clark_d	.usect "clarke",1
clark_q	.usect "clarke",1
sqrt3inv	.usect "clarke",1		; 1/sqrt(3)
clk_temp	.usect "clarke",1
 
;=========================
CLARKE_INIT:	  
;=========================
; sqrt3inv = (1/sqrt(3))=0.577350269
		ldp		#sqrt3inv
		SPLK 	#018830,sqrt3inv	; 1/sqrt(3) (Q15)
		RET
   
;=========================
CLARKE:	  
;=========================
		SPM	1						; Set SPM for Q15 math
		SETC	SXM					; Sign extension mode on

;clark_d = clark_a
		ldp		#clark_a
		LACC	clark_a				; ACC = clark_a
		SACL	clark_d				; clark_d = clark_a
                                    
;clark_q = (2 * clark_b + clark_a) / sqrt(3)
		SFR							; ACC = clark_a/2
		ADD		clark_b				; ACC = clark_a/2 + clark_b
		SACL	clk_temp			; clk_temp = clark_a/2 + clark_b
		LT		clk_temp			; TREG = clark_a/2 + clark_b
		MPY		sqrt3inv			; PREG = (clark_a/2 + 
；clark_b)*(1/sqrt(3))
		PAC							; ACC =  (clark_a/2 + 
；clark_b)*(1/sqrt(3))
    	SFL							; ACC =  (clark_a + 
；clark_b*2)*(1/sqrt(3))
		SACH	clark_q				; clark_q = (clark_a + 
；clark_b*2)*(1/sqrt(3))
    	SPM		0					; SPM reset
		RET