park.asm

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

;===========================================================================
; 文件名:	park.asm
;
; 模块名: 	PARK					      
;
; 初始化程序名: PARK_INIT
;
; 公司:	达盛科技
;
; 功能描述:	完成矢量从两相正交静止坐标系到两相正交旋转坐标系的变换.
;
;			id = ialfa * cos_teta + ibeta * sin_teta
;			iq = -ialfa *sin_teta + ibeta * cos_teta
;
;			        |~~~~~~~~~~~~~~~~~|
;	park_d	o------>|		          |----->o  park_D
;	park_q	o------>|      PARK	      |
;	theta_p	o------>|		          |----->o  park_Q
;			        |_________________|
;
;	说明:	0 < theta_p < 7FFFh  对应  0 度< theta_p < 360 度 
;
;
; 目标板cpu:	x2407
;=====================================================================================
; 修改记录:
;-------------------------------------------------------------------------------------
; 最终修改日期:2005.6.10	版本号	Ver 1.0	
;===========================================================================
;(使用子程序就把下面的变量声明文件复制到主程序中)
;		.ref	PARK, PARK_INIT					;子程序调用
;		.ref	park_d, park_q, theta_p			;输入
;		.ref	park_D, park_Q					;输出
;===========================================================================
;变量声明
		.def	PARK, PARK_INIT					;子程序调用
		.def	park_d, park_q, theta_p			;输入
		.def	park_D, park_Q					;输出
;===========================================================================
;选项
;--------
High_precision	.set	0	;'1'对应高精度'0'则对应普通精度

;===========================================================================
		
		.ref	SINTAB_360

park_d		.usect "park",1
park_q		.usect "park",1
theta_p		.usect "park",1
park_D		.usect "park",1
park_Q		.usect "park",1
t_ptr		.usect "park",1
ip_val		.usect "park",1
cos_theta	.usect "park",1
sin_theta	.usect "park",1
nxt_entry	.usect "park",1
delta_angle	.usect "park",1
GPR0_park	.usect "park",1

;=====================================================================
PARK_INIT: 
;=====================================================================
		RET


;=====================================================================
PARK: 
;=====================================================================
;计算 Cos(theta_p)
;--- 高精度选项 -------
	.if (High_precision)
	;高精度计算采用查表插值方法
		ldp	#theta_p
		LACC	theta_p
		ADD	#8192			;加 90 度, 即 COS(A)=SIN(A+90)
		AND	#07FFFh 		;强制正数循环
		SACL	GPR0_park	;90度 = 7FFFh/4
		LACC	GPR0_park,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_p
		LACC	theta_p
		ADD	#8192			;加 90 度, 即 COS(A)=SIN(A+90)
		AND	#07FFFh 		;强制正数循环
		SACL	GPR0_park	;90 度 = 7FFFh/4
		LACC	GPR0_park,9
		SACH	t_ptr
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	cos_theta	;cos_theta = Cos(theta_p) in Q15
	.endif
;-----------------------------------

;计算 Sin(theta_p)
;--- 高精度选项 -------
	.if (High_precision)
	;高精度计算采用查表插值方法

		LACC	theta_p,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_p,9;
		SACH	t_ptr
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	sin_theta	;sin_theta = Sin(theta_p)  Q15
	.endif
;-----------------------------------

;Park 变换计算
		SPM	1				; 设置乘积移位模式以适合 Q15 乘法运算        
		ZAC					; acc 清零
		LT	park_q     		; TREG = ibeta
		MPY	sin_theta   	; PREG = ibeta * sin_teta
		LTA	park_d     		; ACC = ibeta * sin_teta 且 TREG = ialfa 
		MPY	cos_theta       ; PREG = ialfa * cos_teta
		MPYA	sin_theta   ; ACC = ibeta*sin_teta + ialfa*cos_teta 且 PREG=ialfa*sin_teta
		SACH	park_D		; id = ialfa * cos_teta + ibeta * sin_teta        
	
		LACC	#0          ; 清 ACC
		LT	park_q     		; TREG = ialfa
		MPYS	cos_theta   ; ACC = -ialfa*sin_teta 且 PREG = ibeta*cos_teta
		APAC              	; ACC = -ialfa *sin_teta + ibeta * cos_teta
		SACH	park_Q      ; iq = -ialfa *sin_teta + ibeta * cos_teta
		SPM	0           	; 复位乘积移位
		RET