i_park.asm

这是实现三相异步电动机FOC的DSP开发源代码

;===========================================================================
; 文件名:	I_park.asm				
;
; 模块名: 	I_PARK	
;
; 初始化程序名: I_PARK_INIT
;
; 公司:	达盛科技
;
; 功能描述:	将矢量从两相正交旋转坐标系变换到两相正交静止坐标系
;
; 			id = ialfa * cos_teta - ibeta * sin_teta
; 			iq = ialfa *sin_teta + ibeta * cos_teta
;
;			        |~~~~~~~~~~~~~~~|
;	ipark_D	o------>|		        |----->o  ipark_d
;	ipark_Q	o------>|      I_PARK   |
;	theta_ipo------>|		        |----->o  ipark_q
;			        |_______________|
;
;	说明:	0 < theta_ip < 7FFFh   对应   0 < theta_ip < 360 deg  
;
;	
;
; 目标板cpu:	x2407 
;
;=====================================================================================
; 修改记录:
;-------------------------------------------------------------------------------------
; 最终修改日期:2005.6.15	版本号:	Ver 1.0	
;===========================================================================
;(要调用模块，就将下面声明语句复制到主程序代码中相应位置)
;		.ref	I_PARK, I_PARK_INIT				;子程序调用
;		.ref	ipark_D, ipark_Q, theta_ip		;输入
;		.ref	ipark_d, ipark_q				;输出
;===========================================================================
;变量定义
		.def	I_PARK, I_PARK_INIT				;子程序调用
		.def	ipark_D, ipark_Q, theta_ip		;输入
		.def	ipark_d, ipark_q				;输出
;===========================================================================
;高精度选项
;--------
High_precision	.set	0	;'1'对应高精度'0'则对应普通精度 
;===========================================================================
		
		.ref	SINTAB_360
		
ipark_d		.usect "I_park",1
ipark_q		.usect "I_park",1
theta_ip	.usect "I_park",1
ipark_D		.usect "I_park",1
ipark_Q		.usect "I_park",1

t_ptr		.usect "I_park",1
ip_val		.usect "I_park",1
cos_theta	.usect "I_park",1
sin_theta	.usect "I_park",1
nxt_entry	.usect "I_park",1
delta_angle	.usect "I_park",1
GPR0_ipark	.usect "I_park",1
;=====================================================================
I_PARK_INIT:    ;初始化程序
;=====================================================================
		ldp		#ipark_D
		SPLK	#2FFFh, ipark_D
		SPLK	#2FFFh, ipark_Q
		RET

;======================================================================
I_PARK: 		;控制程序
;======================================================================
;计算 Cos(theta_p)
;--- 高精度选项 -------
	.if (High_precision)
	;高精度计算采用查表插值方法
		ldp	#theta_ip
		LACC	theta_ip
		ADD	#8192			;加 90 度, 即 COS(A)=SIN(A+90)
		AND	#07FFFh 		;强制正数循环
		SACL	GPR0_ipark	;90 度 = 7FFFh/4
		LACC	GPR0_ipark,9
		SACH	t_ptr		;表指针
		SFR					;插值(ip_val)转换成 Q15
		AND	#07FFFh 		;强制 ip_val 为正数
		SACL	ip_val
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	cos_theta	;cos_theta = Cos(theta)  Q15
		ADD	#1h				;表指针加1
		TBLR	nxt_entry	;获取下一个 entry 即 (Entry + 1)
		LACC	nxt_entry
		SUB	cos_theta		;计算增量
		SACL	delta_angle
		LT	delta_angle
		MPY	ip_val			;ip_val = 插值
		PAC
		SACH	ip_val,1
		LACC	ip_val
		ADD	cos_theta
		SACL	cos_theta	;cos_theta = 最终插值
	.endif
;-----------------------------------

;--- 普通精度选项 -------
	.if (High_precision != 1)
	;普通精度采用 256 点查表
		ldp	#theta_ip
		LACC	theta_ip
		ADD	#8192			;加 90 度, 即 COS(A)=SIN(A+90)
		AND	#07FFFh 		;强制正数循环
		SACL	GPR0_ipark	;90 度 = 7FFFh/4
		LACC	GPR0_ipark,9
		SACH	t_ptr
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	cos_theta	;cos_theta = Cos(theta_p)  Q15
	.endif
;-----------------------------------

;计算 Sin(theta_p)
;--- 高精度计算选项 -------
	.if (High_precision)
	;高精度计算采用查表插值方法
		LACC	theta_ip,9
		SACH	t_ptr		;保存表指针
		SFR					;插值(ip_val) 转换成 Q15
		AND	#07FFFh 		;强制 ip_val 为正数
		SACL	ip_val
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	sin_theta	;sin_theta = Sin(theta)  Q15
		ADD	#1h				;表指针加1
		TBLR	nxt_entry	;获取下一个 entry 即 (Entry + 1)
		LACC	nxt_entry
		SUB	sin_theta		;计算增量
		SACL	delta_angle
		LT	delta_angle
		MPY	ip_val			;ip_val = 插值
		PAC
		SACH	ip_val,1
		LACC	ip_val
		ADD	sin_theta
		SACL	sin_theta	;sin_theta = 最终插值
	.endif
;-----------------------------------

;--- 普通精度正弦选项 -------
	.if (High_precision != 1)
	;普通精度采用 256 点查表
		LACC	theta_ip,9
		SACH	t_ptr
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	sin_theta		;sin_theta = Sin(theta_p) in Q15
	.endif
;-----------------------------------
		LDP	 #ipark_D			;判断ipark_D是否超限，限制输出电压
		LACC ipark_D			;
		ABS						;
		SUB #3FFFH				;默认限制值	
		BCND CHECH_Q,LT 		;
		LACC #3FFFH
		SACL	ipark_D	;
CHECH_Q							
		LACC ipark_Q			;判断ipark_Q是否超限，限制输出电压
		ABS						;
		SUB #3FFFH				;默认限制值
		BCND INVERSE_PARK,LT	;
		LACC #3FFFH
		SACL ipark_Q			;

;Inverse Park变换		
INVERSE_PARK				
		SETC	SXM			; 允许符号扩展
		SPM	1				; 设置乘积移位1
 
;park_q = ipark_Q * cos_theta + ipark_D * sin_theta
 
		LACC	#0			; Clear ACC
		LT	ipark_D			; TREG = Udref
		MPY	sin_theta		; PREG = Udref * sin_theta
		LTA	ipark_Q			; ACC = Udref*sin_theta 且 TREG=Uqref
		MPY	cos_theta		; PREG = Uqref * cos_teta
		MPYA	sin_theta	; ACC = Uqref*cos_theta + Udref*sin_theta 且 TREG=Uqref*sin_theta 
		SACH	ipark_q		; Ubeta = Uqref*cos_theta + Udref*sin_theta 
		 
;park_d = ipark_D * cos_theta - ipark_Q * sin_theta		
		LACC	#0			; Clear ACC
		LT	ipark_D			; TREG = Uqref
		MPYS	cos_theta	; ACC = -Uqref*sin_theta 且 PREG = Udref*cos_theta 
		APAC				; ACC = -Uqref*sin_theta + Udref*cos_theta 
		SACH	ipark_d		; Ualfa = -Uqref*sin_theta + Udref*cos_theta
		
		SPM	0				; SPM reset
		RET