ipark.asm

DSP关于F2407的实例程序

;=====================================================================
; Filename	: 	IPARK.asm
; Module Name	: 	IPARK
; Description:	PARK逆变换，从同步旋转坐标系变换到两相静止坐标系。
; Ualfa0 = USM0 * cos_ThetaS0 - UST0 * sin_ThetaS0
; Ubeta0 = USM0 * sin_ThetaS0 + UST0 * cos_ThetaS0
; 根据matlab计算，USM0、UST0和Ualfa0、Ubeta0同量纲。
;
;				|~~~~~~~~~~~~~~~|
; USM0(Q11)		o---->	|		|----->o  Ualfa0(Q11)
; UST0(Q11)		o---->	|   IPARK     	|
; ThetaS0		o---->	|		|----->o  Ubeta0(Q11)
;				|_______________|
;
; Note: 0<ThetaS0<7FFFh   ( equivalent to 0<ThetaS0<360 deg  )
; Modify: 2004.6.18
;=====================================================================
		.include "f2407.h"
		.global	SINTAB_360        	
		.global	IPARK,IPARK_INIT			; function call
		
		.global	USM0,UST0,ThetaS0			; Inputs
		.global	Ualfa0,Ubeta0				; Outputs
		.global	t_ptr,ip_val,cos_ThetaS0,sin_ThetaS0,nxt_entry,delta_angle
		.global	TEMP

;====================================================================
IPARK: 
;====================================================================
		SETC	SXM					; Sign extension mode
		SPM	1					
;-----------------------------------		
;Calculate Sin(ThetaS0) using look-up + interpolation method
		LDP	#DP_DATA		
		LACC	ThetaS0,9
		SACH	t_ptr			;Table pointer = ThetaS0 * 256 /0x7FFFH
		
		SFR				;Convert Interpolation value(ip_val) to Q15
		AND	#07FFFh 		;Force ip_val to a positive number
		SACL	ip_val			;ip_val存插值，Q15带符号数，这里是巧妙的地方，好好理解！
		
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	sin_ThetaS0		;sin_ThetaS0 = Sin(ThetaS0) in Q15
		ADD	#1h				;Inc Table pointer
		TBLR	nxt_entry		;Get next entry i.e. (Entry + 1)
		LACC	nxt_entry
		SUB	sin_ThetaS0		;Find Delta of 2 points
		SACL	delta_angle
		LT	delta_angle
		MPY	ip_val			;ip_val = interpolation value
		PAC
		
		SACH	ip_val			;PM=1,去掉了一个符号位。ip_val和sin_ThetaS0同Q格式。
		LACC	ip_val
		ADD	sin_ThetaS0
		SACL	sin_ThetaS0		;最终结果，Q15	
;-----------------------------------
;Calculate Cos(ThetaS0) using look-up + interpolation method
		LACC	ThetaS0
		ADD	#8192			;COS(A)=SIN(A+90)
		AND	#7FFFH			;A+90>360时取低15位，相当于减去360。
		SACL	TEMP
		
		LACC	TEMP,9
		SACH	t_ptr			;Table pointer = ThetaS0 * 256 /0x7FFFH
		
		SFR
		AND	#7FFFH			;求差值
		SACL	ip_val
		
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	cos_ThetaS0		
		ADD	#1
		TBLR	nxt_entry
		LACC	nxt_entry
		SUB	cos_ThetaS0		;差值，不一定正负。
		SACL	delta_angle
		LT	delta_angle		;Q15
		MPY	ip_val
		PAC				
		SACH	ip_val			;因为delta_angle是Q15格式，PM=1,ip_val和sin_ThetaS0同Q格式。
		
		LACC	ip_val
		ADD	cos_ThetaS0		;由cos的性质，相加不会溢出。
		SACL	cos_ThetaS0		;最终结果，Q15		
;-----------------------------------
; Calculate the Inverse Park transform
; Ualfa0 = USM0 * cos_ThetaS0 - UST0 * sin_ThetaS0		
		LT	cos_ThetaS0
		MPY	USM0
		PAC
		LT	sin_ThetaS0
		MPY	UST0
		SPAC
		SACH	Ualfa0		;变换结果(Q11)		
; Ubeta0 = USM0 * sin_ThetaS0 + UST0 * cos_ThetaS0
		LT	cos_ThetaS0
		MPY	UST0
		PAC
		LT	sin_ThetaS0
		MPY	USM0
		APAC
		SACH	Ubeta0		;变换结果(Q11)
;-----------------------------------
		SPM		0					; SPM reset
		RET