freq_theta.asm

f2407:电机控制

;-------------------------------------------------------------------------------------
;Function name: void freq_theta(freq_theta_struct *p, unsigned int frequence_r);
;Description: A increment of theta will be obtained throught this function, which base
;	on the reference frequence. Assume that the range of (0, 2*pi) will be express as
;	(0, 0xffff) in DSP. So, for a fcHz carrier and frHz modulation wave, there are 
;	fc/fr step in a cycle(i.e. 2*pi). Thus in per step, the increment of theta can
;	be expressed below:
;						delta_theta=2*pi/(fc/fr)=0xffff*fr/fc
;	In this application, the cariier frequence is 10kHz, the modulation frequence is a
;	variable, frHz, and their base value of UI mode is 200Hz. So the UI mode of them can
;	express as:
;			for carrier: 10kHz/300Hz=33(0x21)             （注意：300Hz作为频率的基值）
;			for modulation: fr/300
;	Therefore, the increment of theta is:
;						delta_theta=(0xffff/(fc/300))*(fr/300)=k_theta*fr_ui  	(1)
;	In the equation 1, ktheta=0xffff/0x21=65535/33==0xF83D (Q5). fr_ui: UI mode for
;	modulation frequence, Q16.
;Function parameter:
;		freq_theta_struct *p;	//pointer of freq_theta structure
;		int freqence_r;			//Q16, reference frequence
;Function return structure:
;		typedef struct
;		{	unsigned int ktheta;    //Q5, ktheta=2*pi/fc
;			unsigned int theta;		//Q0, increment of theta in each step
;		}freq_theta_struct;

;2002.10.7		
;---------------------------------------------------------------------------------------
		.def	_freq_theta
_freq_theta_frame .set 1h                  

		.text
_freq_theta:
		POPD	*+     
		SAR		AR0,*+
		SAR		AR1,* 
		LAR		AR0,#_freq_theta_frame
		LAR		AR0,*0+,AR0
		LAR		AR2,*,AR2			;C COMPATIBLE
									;STACK: frequence_r/ *p/address/AR0/AR1
									;ARP:AR2, AR0:AR1, AR2:AR1
		SBRK	#3                                         
		CLRC	SXM
		SPM		3					;PREG right shift 6
		LAR		AR3,*-
		LT		*,AR3				;ARP:AR3, AR2:frequence_r, AR3:ktheta
		MPYU	*+					;delta_theta=frequence_r*ktheta=fr*(2*pi)/fc
									;ARP:AR3, AR2:frequence_r, AR3:delta_theta
		PAC  
		SFL                                                                 
		ADD		#01h,15
		ADD		*,16 
		SACH	*,AR1
		SPM		0
		SBRK	#(_freq_theta_frame+1)
		LAR		AR0,*-
		PSHD	*
		RET
		.end