ileg2drv.asm

这个是关于实现同步电机FOC的控制程序源代码

;*************************************************************
; 文件名:	ileg2drv.asm
;
; 模块名: 	ILEG2DRV
;
; 初始化程序名: ILEG2DRV_INIT
;
; 公司:	达盛科技
;
; 功能描述:	设置ADC选项，将采样的两路电流信号通过可变的增益和偏
;           移量进行处理，获得可用电流信号。
;
;                    |~~~~~~~~~~~~~~~~~~|
; I_ch_sel  o------->|  		        |
; Ia_gain   o------->|Q13  ILEG2DRV  Q15|----->o Ia_out
; Ib_gain   o------->|Q13  	         Q15|----->o Ib_out
; Ia_offset o------->|Q15 	            |
; Ib_offset o------->|Q15 		        |
;                    |__________________|
;
; 目标板cpu:		x240/243/2407
;
;=====================================================================================
; 修改记录:
;-------------------------------------------------------------------------------------
; 最后更新时间:2005.6.15	版本号:	Ver 1.0
;======================================================================
; 设置说明
;-----------------------------------------------------------
; 1. Ix_gain   范围 -3.999999 --> +3.99999 (i.e. Q13)
; 2. Ix_offset 范围 -0.999999 --> +0.99999 (i.e. Q15)
; 3. Ix_out    范围 -0.999999 --> +0.99999 (i.e. Q15)
;    with:
;    1.0 x (VrefHi - VrefLo) = +0.999999 (7FFFh)
;    0.5 x (VrefHi - VrefLo) =  0        (0000/FFFFh)
;    0.0 x (VrefHi - VrefLo) = -0.999999 (8000h)
;
; I_ch_sel 16进制值与 采样通道的对应关系:
; _________________________________________________
;  |	x	|	x	| Ib select	| Ia select	|
; -------------------------------------------------
;
; For x240
;    Ia 选择:	0,1,2...7	-->	Ch0,1,2,...7
;    Ib 选择:	0,1,2...7	-->	Ch8,9,10,...15
;
; For x243
;    Ia 选择:	0,1,2...7	-->	Ch0,1,2,...7
;    Ib 选择:	0,1,2...7	-->	Ch0,1,2,...7
;
; For x2407
;    Ia 选择:	0,1,2...F	-->	Ch0,1,2,...15
;    Ib 选择:	0,1,2...F	-->	Ch0,1,2,...15

;-----------------------------------------------------------
; 变量声明
;-----------------------------------------------------------
;		.ref	ILEG2DRV, ILEG2DRV_INIT				;function call
;		.ref	Ia_gain,Ib_gain,Ia_offset,Ib_offset	;Inputs
;		.ref	Ia_out, Ib_out					    ;Outputs

;-----------------------------------------------------------
; 变量声明
;-----------------------------------------------------------
		.def	ILEG2DRV, ILEG2DRV_INIT				;function call
		.def	Ia_gain,Ib_gain,Ia_offset,Ib_offset	;Inputs
		.def	Ia_out, Ib_out					    ;Outputs

******************************************************
* 变量定义
******************************************************
		.include	"x24x_app.h"
******************************************************
I_ch_sel	.usect "ileg2drv",1
Ia_gain		.usect "ileg2drv",1
Ib_gain		.usect "ileg2drv",1
Ia_offset	.usect "ileg2drv",1
Ib_offset	.usect "ileg2drv",1
Ia_out		.usect "ileg2drv",1
Ib_out		.usect "ileg2drv",1
I_temp		.usect "ileg2drv",1

******************************************************
* 参数设置
******************************************************
		.if x240		; '240 EVM
I_ch_sel_	.set	0032h		; Select Ch3(Phase B),2(Phase A)
		.endif
		.if x243		; '243 EVM
I_ch_sel_	.set	0001h		; Select Ch1(Phase B),0(Phase A)
		.endif
		.if x2407		; 
I_ch_sel_	.set	1331H;6CC6h;6886h; Select(Phase B),(Phase A) - for 'techv2407'
		.endif

		.if x240|x243
ADC_PS_	.set	0001b	; Use p/s=1
EVSOC_	.set	1		; EV Event Starts Conversion
		.endif

		.if x2407
ACQ_PS_	.set	0011b	; Acquisition clk p/s=1/2*(conv p/s) -> Acuisition win=4*clk
CON_PS_	.set	0		; Conversion clk p/s=1/1
CAL_	.set	0		; Calibration register = 0
		.endif
                              
Ia_offset_	.set	0;530h		; Offset introduced by XOR
Ib_offset_	.set	0;430h		;
Ia_gain_	.set	1fffh		; gain=1.0 (Q13)
Ib_gain_	.set	1fffh		; gain=1.0 (Q13)

ILEG2DRV_INIT

		LDP	#I_ch_sel			;
		SPLK	#I_ch_sel_,I_ch_sel		; Set channel select
		SPLK	#Ia_gain_,Ia_gain		; Set gains
		SPLK	#Ib_gain_,Ib_gain		;
		SPLK	#Ia_offset_,Ia_offset	; Set offsets
		SPLK	#Ib_offset_,Ib_offset	;

		ldp	#GPTCON>>7		; Set T1UF as ADC trigger
		lacc	GPTCON
		and	#0FE7Fh			;0FE7Fh
		or	#01b<<7
		sacl	GPTCON

		.if x240|x243
		ldp	#ADC_CNTL2>>7	; Configure ADC_CNTL2/1
		splk	#(EVSOC_<<10 + ADC_PS_),ADC_CNTL2
;		splk	#0F900H+(I_ch_sel_&70H)+(I_ch_sel_&3H)<<1,ADC_CNTL1
		ldp	#I_ch_sel		; Configure channel select
		LACC	I_ch_sel,1
		AND	#000Eh
		SACL	I_temp
		LACC	I_ch_sel
		AND	#0070h
		OR	I_temp
		OR	#0F900h
		ldp	#ADC_CNTL1>>7
 		SACL	ADC_CNTL1
		.endif

		.if x2407
		LDP	#CALIBRATION>>7			; Configure CALIBRATION
		SPLK	#CAL_,CALIBRATION	; Set Calibration register
		SPLK	#4000h,ADCL_CNTL1	; Reset entire Module
		SPLK	#(03000h+ACQ_PS_<<8+CON_PS_<<7),ADCL_CNTL1	; Config ADCL_CNTL1
		SPLK	#0302h,ADCL_CNTL2 	; Allow EVA to start conversion
		SPLK	#3,MAXCONV			; 2 conversions
		bldd	#I_ch_sel,CHSELSEQ1	; Configure channel select
		.endif

		ret

ILEG2DRV:
		.if x240|x243
		ldp	#ADC_CNTL1>>7	; Check ADCEOC bit
Wait
		bit	ADC_CNTL1,BIT7	;
		bcnd	Wait,TC		;

		LACC	ADC_FIFO1	; Read 1st converted value
		XOR	#8000h			; Convert to Q15
		ldp	#Ia_out
		SACL	Ia_out
		LT	Ia_gain			; Ia_gain in Q13	
		MPY	Ia_out			; Q13 x Q15 = Q28
		PAC
		ADD	Ia_offset,13	; add offset in Q28
		neg					; positive => going into motor
		SACH	Ia_out,3	; Convert final result to Q15

		ldp	#ADC_FIFO2>>7	; Read 2nd converted value
		LACC	ADC_FIFO2		;
		XOR	#8000h			; Convert to Q15
		ldp	#Ib_out
		SACL	Ib_out
		LT	Ib_gain			; Ib_gain in Q13	
		MPY	Ib_out			; Q13 x Q15 = Q28
		PAC
		ADD	Ib_offset,13	; add offset in Q28
		neg					; positive => going into motor
		SACH	Ib_out,3	; Convert final result to Q15

		ldp	#I_ch_sel		; Reconfigure channel select
		LACC	I_ch_sel,1
		AND	#000Eh
		SACL	I_temp
		LACC	I_ch_sel
		AND	#0070h
		OR	I_temp
		OR	#0F900h
		ldp	#ADC_CNTL1>>7
 		SACL	ADC_CNTL1
		.endif

		.if x2407
		ldp	#ADCL_CNTL2>>7		; Check SEQ_BSY bit
Wait
		bit	ADCL_CNTL2,BIT12	;
		bcnd	Wait,TC			;

		LACC	ADC_RESULT0		; Read 1st converted value
		XOR		#8000h			; Convert to Q15
		ldp		#Ia_out
		SACL	Ia_out
  		LT		Ia_gain			; Ia_gain in Q13	
		MPY		Ia_out			; Q13 x Q15 = Q28
		PAC
		ADD		Ia_offset,13	; add offset in Q28
		neg						; positive => going into motor
		SACH	Ia_out,3		; Convert final result to Q15

		LDP		#ADC_RESULT1>>7	; Read 2nd converted value
		LACC	ADC_RESULT2
		XOR		#8000h			; Convert to Q15
		ldp		#Ib_out
		SACL	Ib_out
		LT		Ib_gain			; Ib_gain in Q13	
		MPY		Ib_out			; Q13 x Q15 = Q28
		PAC
		ADD		Ib_offset,13	; add offset in Q28
		neg						; positive => going into motor
		SACH	Ib_out,3		; Convert final result to Q15

		bldd	I_ch_sel,#CHSELSEQ1	; Reconfigure channel select
	
		ldp		#ADCL_CNTL2>>7	;ADD BY YZJ FOR RESET 
		lacl	ADCL_CNTL2
		OR		#4000h
		sacl	ADCL_CNTL2		;END
		.endif

		RET