add	#1,(TC_shift+9-15+w_shift-6-16-1)
		rpt	#(TC_shift+9-15+w_shift-6-16-1)
		sfr			;2^(6+16)
		sdp	w,agl				
		add	agl,16		;2^(6+16)
		adds	agl_l
		sub	#360*64,15
		sub	#360*64,15
posrst?		bcnd	negrst?,geq
		add	#360*64,15
		add	#360*64,15
negrst?		bcnd	iwdt_end?,geq
		add	#360*64,15
		add	#360*64,15
iwdt_end?	sach	agl
		sacl	agl_l
		.endm
;=========================================================================
;===========================================
; sincos for 0-360度
; 待做:
;	DP自動切換
;------------------------------------------------------------
sincos	.macro	agl,agl_l,sinagl,cosagl,sect_tmp,temp1,temp2,temp3,temp4
	
	lacc	agl,12
	splk	#90*64,temp1
	rpt	#3
	subc	temp1
	sacl	sect_tmp
	sub	sect_tmp
	adds	agl_l	
	rpt	#3
	sfr
	sach	temp1		;for look up table
	sacl	temp2		;for interpolation
	
	lacl	temp1
	add	#sin
	tblr	temp4
	add	#1
	tblr	temp3
	lacc	temp3
	sub	temp4
	sacl	temp3	
	lt	temp3
	mpyu	temp2		;2^12,還應除以2^4
	pac			;2*16
	add	temp4,16
	add	#1,15
	sach	sinagl		;sin(0-90)

	lacc	#90*64,12
	sub	temp1,16
	subs	temp2
	sach	temp1
	sacl	temp2
	
	lacl	temp1
	add	#sin
	tblr	temp4
	add	#1
	tblr	temp3
	lacc	temp3
	sub	temp4
	sacl	temp3	
	lt	temp3
	mpyu	temp2		;2^12,還應除以2^4
	pac			;2*16
	add	temp4,16
	add	#1,15
	sach	temp1		;sin(0-90)
			
	lacc	sect_tmp,1				
	add	#bpart?
	bacc
bpart?	b	part0?
	b	part1?
	b	part2?
	b	part3?
	
part0?	bldd	temp1,#cosagl
	b	sincos_end?
	
part1?	lacl	sinagl
	neg
	sacl	cosagl
	bldd	temp1,#sinagl
	b	sincos_end?
	
part2?	lacl	sinagl
	neg
	sacl	sinagl
	lacl	temp1
	neg
	sacl	cosagl
	b	sincos_end?
	
part3?	bldd	sinagl,#cosagl
	lacl	temp1
	neg
	sacl	sinagl	
sincos_end?	nop	

	.endm
;=========================================================================
; 有待修改: 1.四舍五入
;-------------------------------------------------------------------------
vvvf		.macro	w,w_n,vsd,vsq,vn,temp1
		
		bldd	#vn,vsd
		splk	#0,vsq
		
		lacc	w
		abs
		sacl	temp1
		sub	w_n
		bcnd	norm,lt
			
abovenorm	nop			
		b	vvvf_end
		
norm		lacc	temp1,15
		rpt	#15
		subc	w_n
		sacl	temp1		;2^15
		
		lt	temp1
		mpy	vsd
		pac
		add	#1,14
		sach	vsd,1
		mpy	vsq
		pac
		add	#1,14
		sach	vsq,1
		
vvvf_end	nop	
		.endm
;=========================================================================
;===================================================================
;空間矢量模塊
;(VSAL_REF,VSBE_REF,VDC)--->(SVPWM發出)
;------------------------------------------------------------------
;專用變量: 	VSAL_REF,VSBE_REF,
;(全在DP=4)	Vref1,Vref2,Vref3,
;		VDC,VDCINVT,
;		X,Y,Z,SECTOR,
;		t1,t2,taon,tbon,tcon,
;	   	CMPR1_TMP,CMPR2_TMP,CMPR3_TMP	
;臨時變量: 	TEMP,TEMP1
;引用常量: 	PWMPRD,VDCINVT_kh,VDCINVT_kl
;編譯開關: 	Tc_short,SVPWM_load
;------------------------------------------------------------------
SVPWM		.MACRO 
;------------------------------------------------------------------			

		anb_abc	VSBE_REF,VSAL_REF,Vref1,Vref3,Vref2,TEMP
;------------------------------------------------------------------			
;計算vdcinvt
		LDP	#VDC/128
		lacl	VDC
		SFR
		ADD	#VDCINVT_kh*2,15
		or	#VDCINVT_kl
		RPT	#15
		SUBC	VDC		;2^5
		LDP	#VDCINVT/128
		sacl	VDCINVT		;2^2

;計算X,Y,Z
		LT	VDCINVT		;2^2
		mpy 	Vref1		;2^14
		pac			;2^16
		add	#1,15
		LDP	#X/128
		sach 	X
		
		mpy	Vref3
		pac
		neg
		add	#1,15
		LDP	#Y/128
		sach	Y
		
		mpy	Vref2
		pac
		neg
		add	#1,15
		LDP	#Z/128
		sach	Z
		
;------------------------------------------------------------------
;60 degrees SECTOR determination
;------------------------------------------------------------------
		clrc	SXM
		LACC	Vref3,4
		AND	#8000H,4
		ADD	Vref2,3
		AND	#0C000H,4
		ADD	Vref1,2
		XOR	#0F000H,4		
		AND	#0E000H,4
		sach	SECTOR
		setc	SXM
;------------------------------------------------------------------
;t1 and t2 calculation depending on the SECTOR number
;------------------------------------------------------------------

		lacl	SECTOR
		add	#(bsector-2)
		bacc
bsector		b	sector1
		b	sector2
		b	sector3
		b	sector4
		b	sector5
		b	sector6

sector1		bldd	#Z,t1
		bldd	#Y,t2
		b 	t1t2out
		
sector2		bldd	#Y,t1
		lacc 	X
		neg
		sacl 	t2	
		b 	t1t2out
		
sector3		lacc 	Z
		neg
		sacl 	t1
		bldd	#X,t2
		b 	t1t2out
		
sector4		lacc 	X
		neg
		sacl 	t1
		bldd	#Z,t2
		b 	t1t2out
		
sector5		bldd	#X,t1
		lacc 	Y
		neg	
		sacl	t2
		b 	t1t2out
		
sector6		lacc 	Y
		neg
		sacl 	t1
		lacc 	Z
		neg
		sacl 	t2		
t1t2out		nop
;------------------------------------------------------------------
;END t1 and t2 calculation
;------------------------------------------------------------------
		lacl 	t1 		;if t1+t2>PWMPRD we have to saturate
		add 	t2
		sacl 	TEMP
		sub	#PWMPRD
		bcnd 	satend,LEQ
		
sat		lacc 	#PWMPRD,15 	;divide PWMPRD by (t1+t2)
		rpt 	#15
		subc 	TEMP
		sacl 	TEMP
		lt 	TEMP
		mpy 	t1 		;t1 (saturated)=t1*(PWMPRD/(t1+t2))
		pac
		sach 	t1,1
		mpy 	t2 		;t2 (saturated)=t2*(PWMPRD/(t1+t2))
		pac
		sach 	t2,1
satend		nop
;------------------------------------------------------------------
;taon,tbon and tcon calculation
		
		lacc 	#PWMPRD
		sub 	t1
		sub 	t2		;taon=(PWMPRD-t1-t2)/2
		add	#1
		sfr
		sacl 	taon
		add 	t1		;tbon=taon+t1
		sacl 	tbon
		add 	t2		;tcon=tbon+t2
		sacl 	tcon
;------------------------------------------------------------------
;SECTOR switching
		lacl	SECTOR					
		add	#(bpush-2)
		bacc
bpush		b	push1
		b	push2
		b	push3
		b	push4
		b	push5
		b	push6

push1		bldd	tbon,#CMPR1_TMP
		bldd	taon,#CMPR2_TMP
		bldd	tcon,#CMPR3_TMP
		
		.if	(SVPWM_load==1)
		bldd	tbon,#CMPR1
		bldd	taon,#CMPR2		
		bldd	tcon,#CMPR3
		.if	(ENSCMP==1)
		bldd	tbon,#SCMPR1	;(this line only for zd)
		bldd	taon,#SCMPR2	;(this line only for zd)
		bldd	tcon,#SCMPR3	;(this line only for zd)
		.endif
		.endif
		B 	push_end
		
push2		bldd	taon,#CMPR1_TMP
		bldd	tcon,#CMPR2_TMP
		bldd	tbon,#CMPR3_TMP
		
		.if	(SVPWM_load==1)
		bldd	taon,#CMPR1		
		bldd	tcon,#CMPR2		
		bldd	tbon,#CMPR3
		.if	(ENSCMP==1)
		bldd	taon,#SCMPR1	;(this line only for zd)
		bldd	tcon,#SCMPR2	;(this line only for zd)
		bldd	tbon,#SCMPR3	;(this line only for zd)
		.endif
		.endif
		b 	push_end
		
push3		bldd	taon,#CMPR1_TMP
		bldd	tbon,#CMPR2_TMP
		bldd	tcon,#CMPR3_TMP
		
		.if	(SVPWM_load==1)
		bldd	taon,#CMPR1				
		bldd	tbon,#CMPR2				
		bldd	tcon,#CMPR3
		.if	(ENSCMP==1)
		bldd	taon,#SCMPR1	;(this line only for zd)
		bldd	tbon,#SCMPR2	;(this line only for zd)
		bldd	tcon,#SCMPR3	;(this line only for zd)
		.endif
		.endif
		b 	push_end
		
push4		bldd	tcon,#CMPR1_TMP
		bldd 	tbon,#CMPR2_TMP
		bldd	taon,#CMPR3_TMP
		
		.if	(SVPWM_load==1)
		bldd	tcon,#CMPR1				
		bldd 	tbon,#CMPR2				
		bldd	taon,#CMPR3
		.if	(ENSCMP==1)
		bldd	tcon,#SCMPR1	;(this line only for zd)
		bldd 	tbon,#SCMPR2	;(this line only for zd)
		bldd	taon,#SCMPR3	;(this line only for zd)
		.endif
		.endif
		b 	push_end
		
push5		bldd	tcon,#CMPR1_TMP
		bldd	taon,#CMPR2_TMP
		bldd	tbon,#CMPR3_TMP
		
		.if	(SVPWM_load==1)
		bldd	tcon,#CMPR1				
		bldd	taon,#CMPR2				
		bldd	tbon,#CMPR3
		.if	(ENSCMP==1)
		bldd	tcon,#SCMPR1	;(this line only for zd)
		bldd	taon,#SCMPR2	;(this line only for zd)
		bldd	tbon,#SCMPR3	;(this line only for zd)
		.endif
		.endif
				
		b 	push_end
		
push6		bldd	tbon,#CMPR1_TMP
		bldd	tcon,#CMPR2_TMP
		bldd	taon,#CMPR3_TMP
		
		.if	(SVPWM_load==1)
		bldd	tbon,#CMPR1				
		bldd	tcon,#CMPR2				
		bldd	taon,#CMPR3
		.if	(ENSCMP==1)
		bldd	tbon,#SCMPR1	;(this line only for zd)
		bldd	tcon,#SCMPR2	;(this line only for zd)
		bldd	taon,#SCMPR3	;(this line only for zd)
		.endif
		.endif
push_end
		.if	(DEADCON==0)
		LDP	#CMPR1_TMP/128
		LACC	CMPR1_TMP,15
		LDP	#CMPR1/128
		SACH	CMPR1
		
		LDP	#CMPR2_TMP/128
		LACC	CMPR2_TMP,15
		LDP	#CMPR2/128
		SACH	CMPR2
		
		LDP	#CMPR3_TMP/128
		LACC	CMPR3_TMP,15
		LDP	#CMPR3/128
		SACH	CMPR3
		.endif
		.ENDM
;---------------------------------------------------------		
;空間矢量模塊結束
;==========================================================

;===========================================================
FM		.macro	in
		ldp	#in/128
		LACC	in		
		ldp 	#0E8H
		SACL	T1CMPR	
		.endm
;----------------------------------------------------------
;===========================================================
;瞬時電流死區補償算法
;-----------------------------------------------------------
indb		.macro	IA,IB,IC			
		
		.if	(immediate==1)
		ldp	#COMCON/128
		LACL	COMCON
		XOR	#0110000001100000B
		rpt	#1
		SACL	COMCON		;中末<->立即
		.endif
		
		LDP	#IA/128
		BIT 	IA,BIT15
		sdp	IA,CMPR1_TMP
		BCND	IA_ADD,TC		
		LACL	CMPR1_TMP
		sdp	CMPR1_TMP,DT_COM
		SUB	DT_COM
		BCND	SUBAEND,GEQ
		ZAC		
SUBAEND		LDP	#0E8H
		SFR			;CMPR1_TMP/2->CMPR1
		SACL	CMPR1
		LDP	#lyh/128
		SACL	lyh
		.if	(ENSCMP==1)
		SACL	SCMPR1
		.endif
		B	DC_IB
		
IA_ADD		LACL	CMPR1_TMP
		sdp	CMPR1_TMP,DT_COM			
		add     DT_COM
		SUB	#PWMPRD
		BCND	ADDAEND,LT
		ZAC		
ADDAEND		ADD	#PWMPRD
		LDP	#0E8H
		SFR			;CMPR1_TMP/2->CMPR1
		sacl	CMPR1
		LDP	#lyh/128
		SACL	lyh
		.if	(ENSCMP==1)
		SACL	SCMPR1
		.endif
		
DC_IB		LDP	#IB/128
		BIT 	IB,BIT15
		sdp	IB,CMPR2_TMP
		BCND	IB_ADD,TC			
		LACL	CMPR2_TMP
		sdp	CMPR2_TMP,DT_COM
		SUB	DT_COM
		BCND	SUBBEND,GEQ
		ZAC		
SUBBEND		LDP	#0E8H
		SFR			;CMPR2_TMP/2->CMPR2
		SACL	CMPR2
		.if	(ENSCMP==1)
		SACL	SCMPR2
		.endif
		B	DC_IC
		
IB_ADD		LACL	CMPR2_TMP
		sdp	CMPR2_TMP,DT_COM			
		add     DT_COM
		SUB	#PWMPRD
		BCND	ADDBEND,LT
		ZAC		
ADDBEND		ADD	#PWMPRD
		LDP	#0E8H
		SFR			;CMPR2_TEM/2->CMPR2
		sacl	CMPR2
		.if	(ENSCMP==1)
		SACL	SCMPR2
		.endif	
		
DC_IC		LDP	#IC/128
		BIT 	IC,BIT15
		sdp	IC,CMPR3_TMP
		BCND	IC_ADD,TC				
		LACL	CMPR3_TMP
		sdp	CMPR3_TMP,DT_COM
		SUB	DT_COM
		BCND	SUBCEND,GEQ
		ZAC		
SUBCEND		LDP	#0E8H
		SFR			;CMPR3_TMP/2->CMPR3
		SACL	CMPR3
		.if	(ENSCMP==1)
		SACL	SCMPR3
		.endif
		
		B	DC_END
		
IC_ADD		LACL	CMPR3_TMP
		sdp	CMPR3_TMP,DT_COM
		add     DT_COM
		SUB	#PWMPRD
		BCND	ADDCEND,LT
		ZAC		
ADDCEND		LDP	#0E8H
		ADD	#PWMPRD
		SFR			;CMPR3_TMP/2->CMPR3
		sacl	CMPR3
		.if	(ENSCMP==1)
		SACL	SCMPR3
		.endif
DC_END		nop

		.if	(immediate==1)
		ldp	#COMCON/128
		LACL	COMCON
		XOR	#0110000001100000B
		rpt	#1
		SACL	COMCON		;中末<->立即
		.endif
				
		.endm
;===========================================================