flux.asm

DSP关于F2407的实例程序

;=====================================================================
; File Name	:	FLUX.asm
; Module Name	:	FLUX
; Initialization Routine: FLUX_INIT	
; Description:	转子磁链观测器
;	
; KesaiR0 = ( Tr * KesaiR1 + Lmd * Tpwm * ISM0 ) / ( Tr + Tpwm )		
; delta_omiga = IST0 * Lmd / ( Tr * KesaiR0 )
; ThetaS0 = ThetaS1 + Tpwm * ( OmigaR0 + delta_omiga )
;
;		|~~~~~~~~~~~~~~~~~~~|
; ISM0 	->	|		    |----->  ThetaS0
; IST0 	->	|  	   	    |
; 		|       FLUX        |
; OmigaR0 ->	|                   |----->  KesaiR0
; 		|___________________|
;
; Note: 0<ThetaS0<7FFFh   ( equivalent to 0<ThetaS0<360 deg  )
; Modify: 2004.8.24
;=====================================================================
		.include "f2407.h"		
		.global	FLUX,FLUX_INIT		; function call
				
		.global	ISM0,IST0,OmigaR0,OmigaS0,OmigaS1; Inputs  
		.global	ThetaS1,KesaiR1			; Inputs
		.global	ThetaS0,KesaiR0			; Outputs  
		.global	TEMP,TEMP1
		.global delta_omiga
           	.global	FLUX_C1,FLUX_C2,FLUX_C3,Tpwm	;constant

;=====================================================================
FLUX_INIT:
;=====================================================================
		LDP	#DP_DATA
		SPLK	#0,ThetaS1	
		SPLK	#0,KesaiR1	;装初值		
		SPLK	#32735,FLUX_C1	;要通过计算，这里先随便赋值,FLUX_C1=Tr/(Tr+Tpwm)=(Q15,数值大约在0.8-1),Tr=200mS,Lmd=225mH,Rr=1.2ohm
		SPLK	#58,FLUX_C2	;FLUX_C2=Lmd*Tpwm/(Tr+Tpwm)=0.675mH(Q8)＝
		SPLK	#23,FLUX_C3	;FLUX_C3=Lmd/Tr*7FFFh/(2*pi)*(2**-8)=Lmd/Tr*20.3712=(delta_omiga为Q0,1/mS)
		SPLK	#16384,Tpwm	;Tpwm=0.2mS，Q15		
		RET
;=====================================================================
FLUX:
;=====================================================================
		SETC	SXM
		SPM	1		;符号数相乘,左移1位.
;------------------------------------------------------------
; calculate KesaiR0
; KesaiR0 = ( Tr * KesaiR1 + Lmd * Tpwm * ISM0 ) / ( Tr + Tpwm )=KesaiR1*FLUX_C1+ISM0*FLUX_C2
		LDP	#DP_DATA 
;		SPLK	#4096,ISM0
;		SPLK	#2048,IST0
;		SPLK	#0,OmigaR0
;		SPLK	#1000H,KesaiR1
		LT	KesaiR1
		MPY	FLUX_C1
		PAC		
		LT	ISM0
		MPY	FLUX_C2
		APAC				;不会溢出
		BCND	Kesai_GT,GT
		SPLK	#0001H,TEMP
		LACC	TEMP,16		;Kesai<=0时（理论上不可能出现为负的情况）赋给一个最小值1，下面的计算分母不能为负和零。
Kesai_GT:
		SACH	KesaiR0		;KesaiR0结果(Q4)
		SACH	KesaiR1		;为下一次计算做准备。
;------------------------------------------------------------
; calculate delta_omiga
; delta_omiga = IST0 * Lmd / ( Tr * KesaiR0 )=IST0*FLUX_C3/KesaiR0,单位1/mS
		MAR	*,AR0
		LAR	AR0,#0
;		SPLK	#0BCF8H,IST0
		LACC	IST0
		SACL	TEMP		;TEMP=IST0
		BCND	IST_GEQ,GEQ
		NEG
		SACL	TEMP		;TEMP=-IST0,取绝对值
		LAR	AR0,#1
IST_GEQ:
		LT	TEMP
		MPY	FLUX_C3
		PAC
		SPLK	#7FFFH,TEMP1
		SPM	0
		LT	KesaiR0
		MPY	TEMP1
		SPAC
		SPM	1
		BCND	OMIGA_MAX,GEQ	;;看是否delta_omiga超过7FFFH,超过不能32位除16位。
		LT	TEMP
		MPY	FLUX_C3
		PAC
		RPT	#15
		SUBC	KesaiR0		; 32除16位。
		SACL	delta_omiga	;delta_omiga结果
		LACC	delta_omiga
		BANZ	OMIGA_NEG
		B	CAL_ThetaS0
OMIGA_NEG:
		NEG
		SACL	delta_omiga
		B	CAL_ThetaS0
OMIGA_MAX:	
		BANZ	MAX_NEG
		SPLK	#7FFFH,delta_omiga	;正的最大值
		B	CAL_ThetaS0
MAX_NEG:
		SPLK	#8000H,delta_omiga	;负的最大值
		B	CAL_ThetaS0		
;------------------------------------------------------------	
; calculate ThetaS0		
; ThetaS0 = ThetaS1 + Tpwm * ( OmigaR0 + delta_omiga )=ThetaS1 + Tpwm * OmigaS0
CAL_ThetaS0:
		LACC	delta_omiga
		ADD	OmigaR0
		ADD	OmigaR0		;加两次，相当OmigaR0×2，因为机械转速和电转速相差p（极对数,此电机是4极2对）倍。	
		SACL	OmigaS0
		SACL	OmigaS1		;在PARK变换中用到。
		
		LT	OmigaS0
		MPY	Tpwm
		SPM	1
		PAC
		SACH	TEMP		;TEMP=Tpwm*OmigaS0
		LACC	TEMP
		ADD	ThetaS1
		AND	#7FFFH			;超过360要处理一下
		SACL	ThetaS0		;ThetaS0结果
		SACL	ThetaS1		;下一次计算用
;------------------------------------------------------------		
		SPM	0				;复位
		RET