com_trig.asm

TI公司24X系列DSP控制永磁同步电机PMSM

;====================================================================================
; Module Name: 	COMTN_TRIG
;
; File Name:	com_trig.asm
;
; Description:	
;				|~~~~~~~~~~~~~~~|
; 	cmtn_ptr_ct	o------>|		|
;	Va		o------>|   COMTN_TRIG	|
;	Vb		o------>|		|----->o  cmtn_trig
;	Vc		o------>|		|
;	v_timer		o------>|		|
;				|_______________|
;
; 	Note: v_timer (virtual timer) is a "global" resource
;
; Target dependency:	C2xx core only
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.00	
;====================================================================================
;(To use this Module, copy this section to main system file)
;		.ref	COMTN_TRIG, COMTN_TRIG_INIT			;function call
;		.ref	Va, Vb, Vc, cmtn_trig, cmtn_ptr_ct		;Inputs/output
;		.ref	debug_Bemf, NW_DYN_THOLD			
;		.ref	cdnw_delta, noise_window_max
;=====================================================================================
;Module definitions for external reference.
		.def	COMTN_TRIG, COMTN_TRIG_INIT			;function call
		.def	Va, Vb, Vc, cmtn_trig, cmtn_ptr_ct		;Inputs/output
		.def	debug_Bemf, NW_DYN_THOLD
		.def	cdnw_delta, noise_window_max
        .def	zc_trig, neutral, rev_period
;=====================================================================================
		.include	x24x_app.h
		
		
		.ref	v_timer
;=====================================================================================
;Variable Definitions for comtn_trig module
;-------------------------------------------------------------------------------------
cmtn_trig	.usect	"com_trig",1	;Commutation trigger output
cmtn_ptr_ct	.usect	"com_trig",1
zc_trig		.usect	"com_trig",1
dt_task_ptr	.usect	"com_trig",1
neutral		.usect	"com_trig",1
rev_period	.usect	"com_trig",1
Va		.usect	"com_trig",1
Vb		.usect	"com_trig",1		
Vc		.usect	"com_trig",1		
time_stamp_new	.usect	"com_trig",1
time_stamp_old	.usect	"com_trig",1
cmtn_delay	.usect	"com_trig",1
cmtn_delay_cntr	.usect	"com_trig",1
D30_done_flg	.usect	"com_trig",1

noise_window_cntr	.usect	"com_trig",1
noise_window_max	.usect	"com_trig",1

cdnw_delta	.usect	"com_trig",1
NW_DYN_THOLD	.usect	"com_trig",1
GPR0_COM_TRIG	.usect	"com_trig",1
GPR1_COM_TRIG	.usect	"com_trig",1
debug_Bemf	.usect	"com_trig",1

CMTN_STATE_TBL:
		.word	CT_STATE_ANB	;cmtn_ptr_ct = 0, de-energized phase = C
		.word	CT_STATE_ANC	;cmtn_ptr_ct = 1, de-energized phase = B
		.word	CT_STATE_BNC	;cmtn_ptr_ct = 2, de-energized phase = A
		.word	CT_STATE_BNA	;cmtn_ptr_ct = 3, de-energized phase = C
		.word	CT_STATE_CNA	;cmtn_ptr_ct = 4, de-energized phase = B
		.word	CT_STATE_CNB	;cmtn_ptr_ct = 5, de-energized phase = A

;=====================================================================================
COMTN_TRIG_INIT:
;=====================================================================================
		ldp	#dt_task_ptr
		SPLK	#CHK_TRIGGER, dt_task_ptr
		SPLK	#0, noise_window_cntr	
		RET


;=====================================================================================
COMTN_TRIG:
;=====================================================================================
		ldp	#cmtn_trig
		SPLK	#0,cmtn_trig		;always clear flag on entry

	;--------------------------
	;Neutral voltage calculation:
	;----------------------------
NEU_CALC:	
		SETC	SXM			; Sign extension mode on
		LACC	Va
		ADD	Vb
		ADD	Vc			;Acc=3*neutral point voltage
		SACL	neutral			;Store 3*neutral point voltage

	;--------------------------
	;Commutation State table Tasks
	;--------------------------
STATE_TASKS:
		SPLK	#0h,zc_trig		;always clear flag on entry
		LACC	#CMTN_STATE_TBL		;point to State table
		ADD	cmtn_ptr_ct
		TBLR	GPR0_COM_TRIG
		LACC	GPR0_COM_TRIG
		BACC

	;State 0 - ZC for phase C
	;-----------------------
CT_STATE_ANB:
		LACC	Vc,1			;Fetch Vc
		ADD	Vc			;ACC=3*(Bemf + Neutral)
		SUB	neutral			;ACC=3*Bemf
		SACL	debug_Bemf			
		BCND	CLR_NW_S0,GT		;BEMF Still positive?
		CALL	NOISE_WIN
		B	ST_END

CLR_NW_S0	SPLK	#0h,noise_window_cntr
		B	ST_END	

	;State 1 - ZC for phase B
	;-----------------------
CT_STATE_ANC:
		LACC	Vb,1			;Fetch Vb
		ADD	Vb			;ACC=3*(Bemf + Neutral)
		SUB	neutral			;ACC=3*Bemf
		SACL	debug_Bemf			
		BCND	CLR_NW_S1,LT		;BEMF Still positive?

		CALL	NOISE_WIN
		B	ST_END

CLR_NW_S1	SPLK	#0h,noise_window_cntr
		B	ST_END	



	;State 2 - ZC for phase A
	;-----------------------
CT_STATE_BNC:
		LACC	Va,1			;Fetch Va
		ADD	Va			;ACC=3*(Bemf + Neutral)
		SUB	neutral			;ACC=3*Bemf
		SACL	debug_Bemf			
		BCND	CLR_NW_S2,GT		;BEMF Still positive?

		CALL	NOISE_WIN
		B	ST_END

CLR_NW_S2	SPLK	#0h,noise_window_cntr
		B	ST_END	



	;State 3 - ZC for phase C
	;-----------------------
CT_STATE_BNA:
		LACC	Vc,1			;Fetch Vc
		ADD	Vc			;ACC=3*(Bemf + Neutral)
		SUB	neutral			;ACC=3*Bemf
		SACL	debug_Bemf		
		BCND	CLR_NW_S3,LT		;BEMF Still positive?

		CALL	NOISE_WIN
		B	ST_END

CLR_NW_S3	SPLK	#0h,noise_window_cntr
		B	ST_END	



	;State 4 - ZC for phase B
	;-----------------------
CT_STATE_CNA:
		SPLK	#0h, D30_done_flg	;clear flag for delay calc in State 5
		LACC	Vb,1			;Fetch Vb
		ADD	Vb			;ACC=3*(Bemf + Neutral)
		SUB	neutral			;ACC=3*Bemf
		SACL	debug_Bemf			
		BCND	CLR_NW_S4,GT		;BEMF Still positive?

		CALL	NOISE_WIN
		B	ST_END

CLR_NW_S4	SPLK	#0h,noise_window_cntr
		B	ST_END	



	;State 5 - ZC for phase A
	;---------------------------------
CT_STATE_CNB:
		LACC	Va,1			;Fetch Va
		ADD	Va			;ACC=3*(Bemf + Neutral)
		SUB	neutral			;ACC=3*Bemf
		SACL	debug_Bemf		
		BCND	CLR_NW_S5,LT		;BEMF Still positive?

		CALL	NOISE_WIN
		B	DELAY_30

CLR_NW_S5	SPLK	#0h,noise_window_cntr


	;Delay 30 deg calculator
	;---------------------------------
DELAY_30
		LACC	D30_done_flg
		BCND	ST_END, NEQ		;If gone through once, skip.

		LACC	time_stamp_new		;new-->old,  current-->new
		SACL	time_stamp_old	

		ldp	#v_timer
		LACC	v_timer			;current-->new

		ldp	#time_stamp_new
		SACL	time_stamp_new	
		SUB	time_stamp_old		;Period = time_stamp_new - time_stamp_old
		BCND	NEG_DELTA, LT		;If Period is negative, allow "wrapping"

POS_DELTA	SACL	rev_period		;Delta = f(t2) - f(t1)
		B	DELAY_DIV12

NEG_DELTA	ADD	#7FFFh			;Add 1 to Delta
		SACL	rev_period		;Delta = 1 + f(t2) - f(t1)

DELAY_DIV12:
		LACC	rev_period		;Load the revolution time
		SPLK	#012,cmtn_delay
		RPT	#15
		SUBC	cmtn_delay		;Divide it by 12 (i.e. 30