;===========================================================================
; Filename:	svgen_dq.asm
; 
; Module Name:	SVGEN_DQ
;
; Initialization Routine: SVGEN_DQ_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; 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
;		|_______________|
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0	
;===========================================================================

;---------------------------------------------------------------------------
; Reference/Prototype
;---------------------------------------------------------------------------
;		.ref	SVGEN_DQ,SVGEN_DQ_INIT			;function call
;		.ref	Ualfa,Ubeta					;Inputs
;		.ref	Ta,Tb,Tc					;Outputs

;---------------------------------------------------------------------------
; Select Processor and Define Related Peripherals
;---------------------------------------------------------------------------
		.include "x24x_app.h"

;---------------------------------------------------------------------------
; 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