;===========================================================================
; File Name:	Svgen_mf.asm					    
;
; Module Name: 	SVGEN_MF
;
; Initialization Routine: SVGEN_MF_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 magnitude and frequency.
;
;			|~~~~~~~~~~~~~~~~~|
;	sv_gain	o------>|		  |----->o  Ta
;	sv_offseto----->|    SVGEN_MF     |----->o  Tb
;	sv_freqo------->|		  |----->o  Tc
;			|_________________|
;
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release Rev 1.0		
;===========================================================================
;(To use this Module, copy this section to main system file)
;		.ref	SVGEN_MF, SVGEN_MF_INIT				;function call
;		.ref	sv_gain, sv_offset, sv_freq			;Inputs
;		.ref	Ta, Tb, Tc					;Outputs
;===========================================================================
;Module definitions for external reference.
		.def	SVGEN_MF, SVGEN_MF_INIT				;function call
		.def	sv_gain, sv_offset, sv_freq			;Inputs
		.def	Ta, Tb, Tc					;Outputs
		.def	sv_freq_max
;===========================================================================
STEP_ANGLE_SV_MAX	.set	2359	;corresponds to 120Hz frequency.
					;(7FFFh = 120Hz)

		.include	x24x_app.h

		
ALPHA_SV	.usect	"svgen_mf",1
STEP_ANGLE_SV	.usect	"svgen_mf",1
ENTRY_NEW	.usect	"svgen_mf",1
ENTRY_OLD	.usect	"svgen_mf",1
SR_ADDR		.usect	"svgen_mf",1
SECTOR_PTR	.usect	"svgen_mf",1
dx		.usect	"svgen_mf",1
dy		.usect	"svgen_mf",1
T		.usect	"svgen_mf",1
Ta		.usect	"svgen_mf",1
Tb		.usect	"svgen_mf",1
Tc		.usect	"svgen_mf",1
sv_gain		.usect	"svgen_mf",1
sv_offset	.usect	"svgen_mf",1
sv_freq		.usect	"svgen_mf",1
sv_freq_max	.usect	"svgen_mf",1
;======================================================================
SVGEN_MF_INIT:
;======================================================================
		ldp	#ALPHA_SV
		SPLK	#7FFFh, T		;T = 100%
		SPLK	#0, ALPHA_SV 	;Start at 0 deg
		SPLK	#0, ENTRY_NEW	;Clear Sine Table Pointer
		SPLK	#0, SECTOR_PTR	;Clear Sector Pointer

		SPLK	#STEP_ANGLE_SV_MAX, sv_freq_max

		SPLK	#3FFFh, sv_gain	 	;Init amplitude to 0.5
		SPLK	#0h, sv_offset 		;Init offset to 0
		SPLK	#3FFFh, sv_freq		;Init freq to 50%

		RET

;======================================================================
SVGEN_MF:
;======================================================================
	;Normalise the freq input to appropriate step angle
		ldp	#sv_freq	
		LT	sv_freq		;sv_freq is in Q15
		MPY	sv_freq_max		;sv_freq_max is in Q0
		PAC				;P = Q0 x Q15 = Q15 (in 32bit word)
		SACH	STEP_ANGLE_SV,1	;shift 1 to restore Q0 format
	
	;Calculate new angle ALPHA
		LACC	ENTRY_NEW
		SACL	ENTRY_OLD
		LACC	ALPHA_SV
		ADD	STEP_ANGLE_SV	;Inc angle.
		SACL	ALPHA_SV
		LACC	ALPHA_SV,8
		SACH	ENTRY_NEW
		LACC	#STABLE60
		ADD	ENTRY_NEW
		TBLR	dy			;dy=Sin(ALPHA)

		LACC	#0FFh			;ACC=60 deg
		SUB	ENTRY_NEW
		ADD	#STABLE60
		TBLR	dx			;dx=Sin(60-ALPHA)

	;Determine which Sector
		LACC	ENTRY_NEW
		SUB	ENTRY_OLD
		BCND	BRNCH_SR, GEQ	;If negative need to change Sector
						;If positive continue
MODIFY_SEC_PTR:
		LACC	SECTOR_PTR	
		SUB	#05h			;Check if at last sector (S6)
		BCND	PISR1,EQ		;If yes, re-init AR1= 1st Sector (S1)
		LACC	SECTOR_PTR		;If no, select next Sector (Sn->Sn+1)
		ADD	#01h
		SACL	SECTOR_PTR		;i.e. inc SECTOR_PTR
		B	BRNCH_SR
PISR1		SPLK	#00, SECTOR_PTR 	;Reset Sector pointer to 0

BRNCH_SR:
		LACC	#SECTOR_TBL
		ADD	SECTOR_PTR
		TBLR	SR_ADDR
		LACC	SR_ADDR
		BACC

;-------------------------------------------------------------------
;Sector 1 calculations - a,b,c --> a,b,c
;-------------------------------------------------------------------
SECTOR_SR1:
		LACC	T		;Acc = T
		SUB	dx		;Acc = T-dx
		SUB	dy		;Acc = T-dx-dy
		SFR			;Acc = Ta = 1/2(T-dx-dy)  <A>
		SACL	Ta

		ADD	dx		;Acc = Tb = dx+Ta	   <B>
		SACL	Tb

		LACC	T		;ACC = T
		SUB	Ta		;ACC = T-Ta
		SACL	Tc		;ACC = Tc = T-Ta	  <C>
		B	SV_END

;-------------------------------------------------------------------
;Sector 2 calculations - a,b,c --> b,a,c  &  dx <--> dy
;-------------------------------------------------------------------
SECTOR_SR2:
		LACC	T		;Acc = T
		SUB	dx		;Acc = T-dx
		SUB	dy		;Acc = T-dx-dy
		SFR			;Acc = Tb = 1/2(T-dx-dy)  <A>
		SACL	Tb

		ADD	dy		;Acc = Ta = dy+Tb	   <B>
		SACL	Ta

		LACC	T		;ACC = T
		SUB	Tb		;ACC = T-Tb
		SACL	Tc		;ACC = Tc = T-Tb	  <C>
		B	SV_END

;-------------------------------------------------------------------
;Sector 3 calculations - a,b,c --> c,a,b
;-------------------------------------------------------------------
SECTOR_SR3:
		LACC	T		;Acc = T
		SUB	dx		;Acc = T-dx
		SUB	dy		;Acc = T-dx-dy
		SFR			;Acc = Tc = 1/2(T-dx-dy)  <A>
		SACL	Tb

		ADD	dx		;Acc = Ta = dx+Tc	   <B>
		SACL	Tc

		LACC	T		;ACC = T
		SUB	Tb		;ACC = T-Tc
		SACL	Ta		;ACC = Tb = T-Tc	  <C>
		B	SV_END

;-------------------------------------------------------------------
;Sector 4 calculations - a,b,c --> c,b,a  &  dx <--> dy
;-------------------------------------------------------------------
SECTOR_SR4:
		LACC	T		;Acc = T
		SUB	dx		;Acc = T-dx
		SUB	dy		;Acc = T-dx-dy
		SFR			;Acc = Tc = 1/2(T-dx-dy)  <A>
		SACL	Tc

		ADD	dy		;Acc = Tb = dx+Ta	   <B>
		SACL	Tb

		LACC	T		;ACC = T
		SUB	Tc		;ACC = T-Tc
		SACL	Ta		;ACC = Ta = T-Tc	  <C>
		B	SV_END

;-------------------------------------------------------------------
;Sector 5 calculations - a,b,c --> b,c,a
;-------------------------------------------------------------------
SECTOR_SR5:
		LACC	T		;Acc = T
		SUB	dx		;Acc = T-dx
		SUB	dy		;Acc = T-dx-dy
		SFR			;Acc = Tb = 1/2(T-dx-dy)  <A>
		SACL	Tc

		ADD	dx		;Acc = Tc = dx+Ta	   <B>
		SACL	Ta

		LACC	T		;ACC = T
		SUB	Tc		;ACC = T-Tb
		SACL	Tb		;ACC = Ta = T-Tb	  <C>
		B	SV_END

;-------------------------------------------------------------------
;Sector 6 calculations - a,b,c --> a,c,b  &  dx <--> dy
;-------------------------------------------------------------------
SECTOR_SR6:
		LACC	T		;Acc = T
		SUB	dx		;Acc = T-dx
		SUB	dy		;Acc = T-dx-dy
		SFR			;Acc = Ta = 1/2(T-dx-dy)  <A>
		SACL	Ta

		ADD	dy		;Acc = Tc = dx+Ta	   <B>
		SACL	Tc

		LACC	T		;ACC = T
		SUB	Ta		;ACC = T-Ta
		SACL	Tb		;ACC = Tb = T-Ta	  <C>

SV_END:
	;Multiply by 2 & modify Ta output with input gain & offset
		LACC	Ta
		SUB	#3FFFh
		SACL	Ta,1
		LT	Ta			;Ta is in Q15
		MPY	sv_gain		;sv_gain is in Q15
		PAC				;P = sg_gain * Ta
		SACH	Ta,1			;shift 1 to restore Q15 format
	;add offset value to Ta
		LACC	Ta
		ADD	sv_offset
		SACL	Ta

	;Multiply by 2 & modify Tb output with input gain & offset
		LACC	Tb
		SUB	#3FFFh
		SACL	Tb,1
		LT	Tb			;Tb is in Q15
		MPY	sv_gain		;sv_gain is in Q15
		PAC				;P = sg_gain * Tb
		SACH	Tb,1			;shift 1 to restore Q15 format
	;add offset value to Tb
		LACC	Tb
		ADD	sv_offset
		SACL	Tb

	;Multiply by 2 & modify Tc output with input gain & offset
		LACC	Tc
		SUB	#3FFFh
		SACL	Tc,1
		LT	Tc			;Tc is in Q15
		MPY	sv_gain		;sv_gain is in Q15
		PAC				;P = sg_gain * Tc
		SACH	Tc,1			;shift 1 to restore Q15 format
	;add offset value to Tb
		LACC	Tc
		ADD	sv_offset
		SACL	Tc

		RET