;===========================================================================
; File Name:	volt_cal.asm
;
; Module Name: 	PHASE_VOLTAGE_CALC
;
; Initialization Routine:  PHASE_VOLTAGE_CALC_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:	Calculates the 3 Phase Motor voltages and stationary 
;	 		dq-axis voltages based on the PWM modulating function 
;			& DC bus voltage measurement.
;	
;				    |~~~~~~~~~~~~~~~~~~~~|
;				    |		         |----->o  Vphase_A
;	Mfunc_V1	o---------->| 			 |----->o  Vphase_B
;	Mfunc_V2	o---------->| PHASE_VOLTAGE_CALC |----->o  Vphase_C
;	Mfunc_V3	o---------->|      Q15 / Q15     |
;	DC_bus		o---------->|			 |----->o  Vdirect
;				    |			 |----->o  Vquadra
;				    | 			 |
;				    |____________________|
;
; Target dependency:	C2xx core only
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release Rev 1.0
;===========================================================================
;(To use this Module, copy this section to main system file)
;		.ref	PHASE_VOLTAGE_CALC			;function call
;		.ref	PHASE_VOLTAGE_CALC_INIT			;function call
;		.ref	Mfunc_V1, Mfunc_V2			;Inputs
;		.ref	Mfunc_V3, DC_bus				;Inputs
;		.ref	Vphase_A,Vphase_B,Vphase_C		;Outputs
;		.ref	Vdirect,Vquadra				;Outputs
;===========================================================================
;Module definitions for external reference.
		.def	PHASE_VOLTAGE_CALC			;function call
		.def	PHASE_VOLTAGE_CALC_INIT			;function call
		.def	Mfunc_V1, Mfunc_V2			;Inputs
		.def	Mfunc_V3, DC_bus				;Inputs
		.def	Vphase_A,Vphase_B,Vphase_C		;Outputs
		.def	Vdirect,Vquadra				;Outputs
;===========================================================================
out_of_phase_			.set 	1		; set 1 for the out of phase correction if
										;  * Mfunc_V1 is out of phase with PWM1,
										;  * Mfunc_V2 is out of phase with PWM3,
										;  * Mfunc_V3 is out of phase with PWM5
										; otherwise, set 0 if their phases are correct.
			.include	x24x_app.h

Mfunc_V1		.usect	"volt_cal",1
Mfunc_V2		.usect	"volt_cal",1
Mfunc_V3		.usect	"volt_cal",1
DC_bus		.usect	"volt_cal",1
Vphase_A		.usect	"volt_cal",1
Vphase_B		.usect	"volt_cal",1
Vphase_C		.usect	"volt_cal",1
Vdirect		.usect	"volt_cal",1
Vquadra		.usect	"volt_cal",1
one_third		.usect	"volt_cal",1		; 1/3
sqrt3inv 		.usect	"volt_cal",1		; 1/sqrt(3)
tmp_volt		.usect	"volt_cal",1 		; temporary variable

;=========================
PHASE_VOLTAGE_CALC_INIT:
;=========================
; 1/3 = 0.33333 => 0.33333*2^15 = 10923 
		LDP	#one_third
		SPLK	#10923,one_third	;7FFFh x 0.33333...    (Q15)

; 1/sqrt(3) = 0.577350269 => 0.577350269*2^15 = 18919 
		SPLK #018919,sqrt3inv  	;7FFFh x 0.577350269...   (Q15)	
                                	
		RET

;=========================
PHASE_VOLTAGE_CALC:
;=========================
		SETC	SXM

	.if (out_of_phase_)
		LACC	Mfunc_V1
		NEG		
		SACL	Mfunc_V1
		LACC	Mfunc_V2
		NEG		
		SACL	Mfunc_V2		
		LACC	Mfunc_V3
		NEG		
		SACL	Mfunc_V3	
	.endif
	
;scale the incomming Modulation functions with the DC_bus value:
	;Input 1
		LDP	#Mfunc_V1
		LT	Mfunc_V1		;Mfunc_V1 is in Q15
		MPY	DC_bus		;DC_bus is in Q15
		PAC				;P = Mfunc_V1 * DC_bus 
		SACH	Mfunc_V1,1		;shift 1 to restore Q15 format
	;Input 2
		LT	Mfunc_V2		;Mfunc_V1 is in Q15
		MPY	DC_bus		;DC_bus is in Q15
		PAC				;P = Mfunc_V1 * DC_bus 
		SACH	Mfunc_V2,1		;shift 1 to restore Q15 format
	;Input 3
		LT	Mfunc_V3		;Mfunc_V1 is in Q15
		MPY	DC_bus		;DC_bus is in Q15
		PAC				;P = Mfunc_V1 * DC_bus 
		SACH	Mfunc_V3,1		;shift 1 to restore Q15 format

;Calculate the 3 Phase voltages:
		SPM	1	
	;Phase A
		LT	one_third		; TREG = one_third           (Q15)
		MPY	Mfunc_V1		; PREG = one_third*Mfunc_V1  (Q15)
		PAC				; ACC = one_third*Mfunc_V1   (Q15)
		SFL			   	; ACC = 2*one_third*Mfunc_V1 (Q15)
		MPY	Mfunc_V2		; PREG = one_third*Mfunc_V2  (Q15)
		SPAC				; ACC = (2/3)*Mfunc_V1-(1/3)*Mfunc_V2  (Q15)   
		MPY	Mfunc_V3		; PREG = one_third*Mfunc_V3  (Q15)
		SPAC				; ACC = (2/3)*Mfunc_V1-(1/3)*Mfunc_V2-(1/3)*Mfunc_V3 (Q15)   
		SACH	Vphase_A

	;Phase B
		MPY	Mfunc_V2		; PREG = one_third*Mfunc_V2  (Q15)
		PAC				; ACC = one_third*Mfunc_V2   (Q15)
		SFL			   	; ACC = 2*one_third*Mfunc_V2 (Q15)
		MPY	Mfunc_V1		; PREG = one_third*Mfunc_V1  (Q15)
		SPAC				; ACC = (2/3)*Mfunc_V2-(1/3)*Mfunc_V1  (Q15)   
		MPY	Mfunc_V3		; PREG = one_third*Mfunc_V3  (Q15)
		SPAC				; ACC = (2/3)*Mfunc_V2-(1/3)*Mfunc_V1-(1/3)*Mfunc_V3 (Q15)   
		SACH	Vphase_B

	;Phase C
		MPY	Mfunc_V3		; PREG = one_third*Mfunc_V3  (Q15)
		PAC				; ACC = one_third*Mfunc_V3   (Q15)
		SFL			   	; ACC = 2*one_third*Mfunc_V3 (Q15)
		MPY	Mfunc_V1		; PREG = one_third*Mfunc_V1  (Q15)
		SPAC				; ACC = (2/3)*Mfunc_V3-(1/3)*Mfunc_V1  (Q15)   
		MPY	Mfunc_V2		; PREG = one_third*Mfunc_V2  (Q15)
		SPAC				; ACC = (2/3)*Mfunc_V3-(1/3)*Mfunc_V1-(1/3)*Mfunc_V2 (Q15)   
		SACH	Vphase_C

; Voltage transformation (a,b,c)  ->  (Direct,Quadrature)
; Direct-axis 
		LACC	Vphase_A		; ACC = Vphase_A             	(Q15)
		SACL	Vdirect    	      ; Vdirect = Vphase_A   		(Q15)

; Quadrature-axis
		SFR	                  ; ACC = Vphase_A/2     		(Q15)
		ADD	Vphase_B		; ACC = Vphase_A/2 + Vphase_B (Q15)
		SACL	tmp_volt			; tmp_volt = Vphase_A/2 + Vphase_B (Q15)
		LT	tmp_volt			; TREG = Vphase_A/2 + Vphase_B (Q15)
		MPY	sqrt3inv		; PREG = (Vphase_A/2 + Vphase_B)*(1/sqrt(3))  (Q30)
		PAC				; ACC = (Vphase_A/2 + Vphase_B)*(1/sqrt(3))   (Q30)
		SFL				; ACC = (Vphase_A/2 + Vphase_B)*(1/sqrt(3))   (Q31)
		SACH	Vquadra	    	; Vquadra = (Vphase_A + 2*Vphase_B)*(1/sqrt(3)) (Q15)

		SPM	0
		CLRC	SXM
		
		RET