;=====================================================================
; Filename:		svgen_dq.asm
; Module Name:	SVGEN_DQ
; Initialization Routine: SVGEN_DQ_INIT
; Description:	This module calculates the appropriate duty ratios needed 
;					to generate a given stator reference voltage using space 
;					vector PWM technique. The stator reference voltage is 
;					described by it's (a,b) components, Ualfa and Ubeta.
;
;					|~~~~~~~~~~~~~~~|
; Ualfa   o---->	|				|----->o  Ta
;					|   SVGEN_DQ   	|----->o  Tb
; Ubeta   o---->	|				|----->o  Tc
;					|_______________|
;=====================================================================
;---------------------------------------------------------------------
; Reference/Prototype
;---------------------------------------------------------------------
;		.ref	SVGEN_DQ,SVGEN_DQ_INIT			;function call
;		.ref	Ualfa,Ubeta						;Inputs
;		.ref	Ta,Tb,Tc						;Outputs

;---------------------------------------------------------------------
; Global Definitions
;---------------------------------------------------------------------
		.def	SVGEN_DQ,SVGEN_DQ_INIT			;function call
		.def	Ualfa,Ubeta						;Inputs
		.def	Ta,Tb,Tc						;Outputs

;---------------------------------------------------------------------
; Variables
;---------------------------------------------------------------------
Ualfa			.usect "svgen_dq",1
Ubeta			.usect "svgen_dq",1
Va			.usect "svgen_dq",1
Vb			.usect "svgen_dq",1
Vc			.usect "svgen_dq",1
Ta			.usect "svgen_dq",1
Tb			.usect "svgen_dq",1
Tc			.usect "svgen_dq",1
sector		.usect "svgen_dq",1			;SVPWM sector    
t1			.usect "svgen_dq",1			;SVPWM T1 
t2			.usect "svgen_dq",1			;SVPWM T2 
half_sqrt3	.usect "svgen_dq",1			;SQRT(3) * 0.5

;Alias Variable declaration (to conserve .bss locations)
X			.set	Va
Y			.set	Vb
Z			.set	Vc
SR_ADDR	.set	sector
;----------------------------------------------------------

;=============================
SVGEN_DQ_INIT:
;=============================
		ldp		#half_sqrt3
		SPLK 	#28378,half_sqrt3 			; Set constant sqrt(3)*0.5 in 
；Q15 format
		RET

;=============================
SVGEN_DQ:
;=============================
	;INV_CLARKE:	 
	;-----------
		SPM		1							; SPM set for Q15 multiplication 
		SETC	SXM							; Sign extension mode on

	;Va = Ubeta 	
		ldp		#Ubeta
		LACC	Ubeta						; ACC = Ubeta
		SACL	Va							; Va = Ubeta
        
	;Vb = (-Ubeta + sqrt(3) * Ualfa) / 2  
		LT		Ualfa				; TREG = Ualfa
		MPY		half_sqrt3			; PREG = Ualfa * half_sqrt3     
		PAC                     		; ACC high = Ualfa * half_sqrt3              
		SUB		Ubeta,15			; ACC high = Ualfa * half_sqrt3 +Ubeta/2 
		SACH	Vb           		; Vb = Ualfa * half_sqrt3 +Ubeta/2
        
	;Vc = (-Ubeta - sqrt(3) * Ualfa) / 2        
		PAC                     	; ACC high = Ualfa * half_sqrt3 
		NEG                     	; ACC high = - Ualfa * half_sqrt3
		SUB		Ubeta,15		; ACC high = - Ualfa * half_sqrt3 -Ubeta/2
		SACH	Vc             	; Vc = - Ualfa * half_sqrt3 -Ubeta/2

;----------------------------------------------------------
; 60 degrees sector determination 
; sector = r1 + 2*r2 + 4*r3
; r1=1 if Va>0
; r2=1 if Vb>0
; r3=1 if Vc>0
;----------------------------------------------------------
	  	SPLK	#0,sector
		LACC	Va
		BCND	vref1_neg,LEQ   ;If Va<0 do not set bit 1 of sector
		LACC	sector          	;
		OR		#1              	;
		SACL	sector          	;
vref1_neg  
		LACC	Vb
		BCND	vref2_neg,LEQ   ;If Vb<0 do not set bit 2 of sector
		LACC	sector          	;
		OR		#2              	;
		SACL	sector         	;
vref2_neg  
		LACC	Vc
		BCND	vref3_neg,LEQ   ;If Vc<0 do not set bit 3 of sector
		LACC	sector          	;
		OR		#4             	;
		SACL	sector          	;
vref3_neg  

;----------------------------------------------------------
;X,Y,Z calculation:   
;----------------------------------------------------------
XYZ_CALC:
	;X = Ubeta 	
		LACC	Ubeta
		SACL	X

	;Y = (0.5 * Ubeta) + (sqrt(3) * 0.5 * Ualfa) 
		LT		Ualfa			; TREG = Ualfa
		MPY		half_sqrt3		; PREG = Ualfa * half_sqrt3     
		PAC                    	 	; ACC high = Ualfa * half_sqrt3              
		ADD		Ubeta,15		; ACC high = Ualfa * half_sqrt3 + Ubeta/2 
		SACH	Y           		; Y = Ualfa * half_sqrt3 + Ubeta/2

	;Z = (0.5 * Ubeta) - (sqrt(3) * 0.5 * Ualfa)
		PAC                     	; ACC high = Ualfa * half_sqrt3 
		NEG                     	; ACC high = - Ualfa * half_sqrt3
		ADD		Ubeta,15		; ACC high = - Ualfa * half_sqrt3 + Ubeta/2
		SACH	Z             	; Z = - Ualfa * half_sqrt3 + Ubeta/2

;-------------------------------------------------------------------
;Sector calculations ("case statement")
;-------------------------------------------------------------------
		LACC	#SECTOR_TBL
		ADD		sector
		TBLR	SR_ADDR
		LACC	SR_ADDR
		BACC

SECTOR_SR1:
;----------
	;sector 1:	t1=Z and t2=Y, (abc --> Tb, Ta, Tc)
		lacc	Z
		sacl  	t1
		lacc  	Y
		sacl 	t2

		lacc 	#7FFFh			;Load 1 (Q15)
		sub   	t1              
		sub   	t2              	;taon=(1-t1-t2)/2
		sfr                     
		sacl  	Tb	            
		add   	t1              	;tbon=taon+t1
		sacl  	Ta            
		add   	t2              	;tcon=tbon+t2
		sacl  	Tc            
		B		SV_END

SECTOR_SR2:
;----------
	;sector 2:	t1=Y and t2=-X, (abc --> Ta, Tc, Tb)	
		lacc	Y
		sacl  	t1
		lacc 	X
		neg
		sacl  	t2

		lacc  	#7FFFh			;Load 1 (Q15)
		sub   	t1              
		sub   	t2              	;taon=(1-t1-t2)/2
		sfr                    		;
		sacl  	Ta
		add   	t1              	;tbon=taon+t1
		sacl 	Tc
		add   	t2              	;tcon=tbon+t2
		sacl  	Tb
		B		SV_END

SECTOR_SR3:
;----------
	;sector 3:	t1=-Z and t2=X, (abc --> Ta, Tb, Tc)
		lacc	Z
		neg
		sacl  	t1
		lacc  	X
		sacl  	t2

		lacc  	#7FFFh			;Load 1 (Q15)
		sub   	t1              
		sub   	t2              	;taon=(1-t1-t2)/2
		sfr                     	;
		sacl 	Ta
		add   	t1              	;tbon=taon+t1
		sacl  	Tb
		add   	t2              	;tcon=tbon+t2
		sacl 	Tc
		B		SV_END

SECTOR_SR4:
;----------
	;sector 4: t1=-X and t2=Z, (abc --> Tc, Tb, Ta)
		lacc	X
		neg
		sacl  	t1
		lacc  	Z
		sacl 	t2

		lacc 	#7FFFh			;Load 1 (Q15)
		sub   	t1              
		sub   	t2              	;taon=(1-t1-t2)/2
		sfr                     	;
		sacl  	Tc
		add   	t1              	;tbon=taon+t1
		sacl  	Tb
		add   	t2              	;tcon=tbon+t2
		sacl  	Ta
		B		SV_END

SECTOR_SR5:
;----------
	;sector 5:	t1=X and t2=-Y, (abc --> Tb, Tc, Ta)
		lacc	X
		sacl  	t1
		lacc  	Y
		neg
		sacl  	t2

		lacc  	#7FFFh			;Load 1 (Q15)
		sub   	t1              
		sub   	t2              	;taon=(1-t1-t2)/2
		sfr                     	;
		sacl  	Tb
		add   	t1              	;tbon=taon+t1
		sacl  	Tc
		add   	t2              	;tcon=tbon+t2
		sacl  	Ta
		B		SV_END

SECTOR_SR6:
;----------
	;sector 6:	t1=-Y and t2=-Z, (abc --> Tb, Tc, Ta)
	    	lacc	Y
		neg
		sacl  	t1
		lacc  	Z
		neg
		sacl  	t2

		lacc  	#7FFFh			;Load 1 (Q15)
		sub   	t1              
		sub   	t2              	;taon=(1-t1-t2)/2
		sfr                     	;
		sacl  	Tc
		add   	t1              	;tbon=taon+t1
		sacl  	Ta
		add   	t2              	;tcon=tbon+t2
		sacl 	Tb

SV_END:
	;Multiply Ta by 2 & offset by 1/2
		LACC	Ta
		SUB		#3FFFh
		SACL	Ta,1			;mpy by 2

	;Multiply Tb by 2 & offset by 1/2
		LACC	Tb
		SUB		#3FFFh
		SACL	Tb,1			;mpy by 2

	;Multiply Tc by 2 & offset by 1/2
		LACC	Tc
		SUB		#3FFFh
		SACL	Tc,1			;mpy by 2

DUMMY		
SPM		0	            	; SPM reset
		RET

;-------------------------------------------------------
;SVPWM Sector routine jump table - used with BACC inst.
;-------------------------------------------------------
SECTOR_TBL:
SR00		.word	DUMMY
SR0		.word	SECTOR_SR1
SR1		.word	SECTOR_SR2
SR2		.word	SECTOR_SR3
SR3		.word	SECTOR_SR4
SR4		.word	SECTOR_SR5
SR5		.word	SECTOR_SR6