park.asm

DSP关于F2407的实例程序

;=====================================================================
; Filename	: 	PARK.asm
; Module Name	: 	PARK			      
; Description:	PARK变换
;	ISM0 = Ialfa0 * cos_PARK_Theta + Ibeta0 * sin_PARK_Theta
;	IST0 = Ibeta0 * cos_PARK_Theta - Ialfa0 * sin_PARK_Theta
;	PARK_Theta = ThetaS1 + OmigaS1 * Tpwm
;
;				|~~~~~~~~~~~~~~~|
;	alfa0(Q11)	o---->	|		|----->o  ISM0(Q11)
;	Ibeta0(Q11)	o---->	|      PARK	|
;	PARK_Theta	o---->	|		|----->o  IST0(Q11)
;				|______________	|
;
; Note: 0 < PARK_Theta < 7FFFh   (i.e. equivalent to 0 < PARK_Theta < 360 deg)
; Modify: 2004.6.16
;=====================================================================
		.include "f2407.h"
		.global	SINTAB_360				;sin table		
		.global	PARK,PARK_INIT				;function call
		.global	PDPINTA_INIT
		
		.global	Ialfa0,Ibeta0,PARK_Theta		;Inputs
		.global	ISM0,IST0,IST1,IST2			;Outputs
		.global TEMP
		.global Tpwm,ThetaS1,OmigaS1
		.global	t_ptr,ip_val,cos_PARK_Theta,sin_PARK_Theta,nxt_entry,delta_angle ;计算sin，cos时所用到变量
;=================================================================
PARK_INIT: 
;=================================================================	
		LDP	#DP_DATA
		SPLK	#0,OmigaS1
		SPLK	#16384,Tpwm	;Tpwm=0.2mS(Q15)
		RET	
;=================================================================
PARK: 
;=================================================================
		SETC	SXM					; Sign extension mode
		SPM	1
;-----------------------------------------------------------------
;Calculate PARK_Theta
;PARK_Theta = ThetaS1 + OmigaS1 * Tpwm
		LDP	#DP_DATA
		LT	OmigaS1
		MPY	Tpwm
		SPH	TEMP
		LACC	TEMP
		ADD	ThetaS1
		AND	#7FFFH
		SACL	PARK_Theta		
;-----------------------------------------------------------------	
;Calculate Sin(PARK_Theta) using look-up + interpolation method
		LDP	#DP_DATA
		
		LACC	PARK_Theta,9
		SACH	t_ptr			;Table pointer = PARK_Theta * 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_PARK_Theta		;sin_PARK_Theta = Sin(PARK_Theta) in Q15
		ADD	#1h			;Inc Table pointer
		TBLR	nxt_entry		;Get next entry i.e. (Entry + 1)
		LACC	nxt_entry
		SUB	sin_PARK_Theta		;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_PARK_Theta同Q格式。
		LACC	ip_val
		ADD	sin_PARK_Theta
		SACL	sin_PARK_Theta		;最终结果，Q15	
;-----------------------------------------------------------------
;Calculate Cos(PARK_Theta) using look-up + interpolation method
		LACC	PARK_Theta
		ADD	#8192			;COS(A)=SIN(A+90)
		AND	#7FFFH			;A+90>360时取低15位，相当于减去360。
		SACL	TEMP
		
		LACC	TEMP,9
		SACH	t_ptr			;Table pointer = PARK_Theta * 256 /0x7FFFH
		
		SFR
		AND	#7FFFH			;求差值
		SACL	ip_val
		
		LACC	#SINTAB_360
		ADD	t_ptr
		TBLR	cos_PARK_Theta		
		ADD	#1
		TBLR	nxt_entry
		LACC	nxt_entry
		SUB	cos_PARK_Theta		;差值，不一定正负。
		SACL	delta_angle
		LT	delta_angle		;Q15
		MPY	ip_val
		PAC				
		SACH	ip_val			;因为delta_angle是Q15格式，PM=1,ip_val和sin_PARK_Theta同Q格式。
		
		LACC	ip_val
		ADD	cos_PARK_Theta		;由cos的性质，相加不会溢出。
		SACL	cos_PARK_Theta		;最终结果，Q15		
;-----------------------------------------------------------------
; Calculate the Park transform 
; 根据matlab计算，ISM0、IST0和Ialfa0、Ibeta0同量纲。
; ISM0 = Ialfa0 * cos_PARK_Theta + Ibeta0 * sin_PARK_Theta
		LT	cos_PARK_Theta
		MPY	Ialfa0
		PAC
		LT	sin_PARK_Theta
		MPY	Ibeta0
		APAC			;不会溢出
		SACH	ISM0		;ISM0结果(Q11)
;		LACC	ISM0  
;		ADD	#200
;		BCND	IST_CAL,GEQ
;		B	PDPINTA_INIT	;ISM0为负时不正常，保护。
; IST0 = Ibeta0 * cos_PARK_Theta - Ialfa0 * sin_PARK_Theta
IST_CAL:	
		;SETC	SXM
;		LDP	#DP_DATA
;		LACC	IST2
;		SACL	IST1			;IST0->IST1存上一次结果。
		
		LT	cos_PARK_Theta
		MPY	Ibeta0
		PAC
		LT	sin_PARK_Theta
		MPY	Ialfa0
		SPAC			;不会溢出
		SACH	IST0		;IST0结果(Q11)
		
		SACH	IST2
;		LACC	IST2
;		SUB	IST1
;		ABS
;		SUB	#8000			;还是不行，8000的话，闭环计算还是有问题。看来不能这样滤波。
;		BCND	IST_ENDCAL,LT		;前后两个采样差值超过8000，则进行高频尖刺滤波。
;		LACC	IST1
;		SACL	IST2			;滤波，把前一次值当成当前值。
		
;		LACC	IST0  
;		ADD	#200
;		BCND	IST_ENDCAL,GEQ
;		B	PDPINTA_INIT	;IST0为负时不正常，保护。
;--------------------------------------------------------------------
IST_ENDCAL:
		SPM	0	; SPM reset
		RET