aci_se.asm

TI公司24X系列DSP控制永磁同步电机PMSM

;=====================================================================================
; File name:        ACI_SE.ASM                     
;                    
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:  Open-Loop Speed Estimator of Induction Motor               
; 
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 02-08-2001	Release	Rev 1.00
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx.  (Peripheral Independent).
;================================================================================
; Routine Name: aci_se_calc 				  					    Type: C Callable
;  
;  C prototype : void aci_se_calc(struct ACISE *p);
;
;        The struct object is defined in the header file "aci_se.h" as follows:
;
; typedef struct {  int  i_qs_se;  	/* Input: Stationary q-axis stator current (Q15) */
;				  int  psi_dr_se;  	/* Input: Stationary d-axis rotor flux (Q15) */
;				  int  i_ds_se;		/* Input: Stationary d-axis stator current (Q15) */
;				  int  psi_qr_se;	/* Input: Stationary q-axis rotor flux (Q15) */		
;		 	 	  int  K1_se;		/* Parameter: Constant using in speed computation (Q15) */
;                 int  psi_r_2;     /* Variable: Squared rotor flux (Q15)  */ 
;   			  int  theta_r_se;  /* Input: Rotor flux angle (Q15) */    		  
;		 	 	  int  K2_se;		/* Parameter: Constant using in differentiator (Q6) */
;    			  int  theta_r_old; /* Variable: Previous rotor flux angle (Q15) */    		  
;		 	 	  int  K3_se;		/* Parameter: Constant using in low-pass filter (Q15) */ 
;		 	 	  int  wr_psi_r;	/* Variable: Synchronous rotor flux speed in per-unit (Q15) */
;		 	 	  int  K4_se;		/* Parameter: Constant using in low-pass filter (Q15) */
;		 	 	  int  wr_hat_se;	/* Output: Estimated speed in per unit (Q15) */
;				  int  base_rpm_se; /* Parameter: Base rpm speed (Q0) */		 	 	  
;		 	 	  int  wr_hat_rpm_se;	/* Output: Estimated speed in rpm  (Q0) */
;		 	 	  int  (*calc)();	/* Pointer to calculation function */ 
;				 } ACISE;	            
;	 		 
;        Frame Usage Details:
;     step   |      a      |      b       |      c       |     d     
;____________|_____________|______________|______________|_____________
;     AR0  	 |  tmp1_se    |              | 			 |     
;     AR1  	 |    w_sl     |              | 			 |     
;
;================================================================================
                .def        _aci_se_calc
;================================================================================
DIFF_MAX_LIMIT			.set 	7333h	; Maximum limit to differentiate angle (Q15)
DIFF_MIN_LIMIT			.set 	0CCDh	; Minimum limit to differentiate angle (Q15)

; Important!! Note that the default DIFF_LIMIT are shown, if it is neccessary 
; to be changed, the module library (e.g., clib_011.lib) must be rebuilt to take the effect.  
;================================================================================
__aci_se_calc_framesize .set 0002h
;================================================================================
_aci_se_calc:
 													; Assume now ARP=AR1
     	POPD	*+									; Keep return address
       	SAR  	AR0,*+								; Keep old frame pointer (FP)
       	SAR   	AR1,*								; Keep old stack pointer (SP)
       	LARK  	AR0,__aci_se_calc_framesize			; Load AR0 with frame size	
       	LAR   	AR0,*0+,AR0							; AR0->FP0 (new FP), ARP=AR0

;================================================================================
		SBRK	#3		; ARP=AR0, AR0->FR0-3 (1st argument)		
;----------------------------------------------------------------------------------
		LAR		AR2,*	; ARP=AR0, AR0->i_qs_se, AR2->i_qs_se
;----------------------------------------------------------------------------------
		ADRK	#3		; ARP=AR0, AR0->FR0, AR2->i_qs_se
;----------------------------------------------------------------------------------
		MAR		*,AR2  	; ARP=AR2, AR0->FR0, AR2->i_qs_se
;----------------------------------------------------------------------------------
		SETC 	SXM		; Turn sign extension mode on
						; ARP=AR2, AR0->FR0, AR2->i_qs_se
;----------------------------------------------------------------------------------
		SETC	OVM		; Set overflow mode
						; ARP=AR2, AR0->FR0, AR2->i_qs_se
;----------------------------------------------------------------------------------
 		SPM     0       ; Reset product mode
       					; ARP=AR2, AR0->FR0, AR2->i_qs_se
;----------------------------------------------------------------------------------
 	  	LT		*+		; TREG = i_qs_se  (Q15)              	
 	                	; ARP=AR2, AR0->FR0, AR2->psi_dr_se           
;----------------------------------------------------------------------------------
      	MPY		*+		; PREG = psi_dr_se*i_qs_se  (Q30)
      	                ; ARP=AR2, AR0->FR0, AR2->i_ds_se
;----------------------------------------------------------------------------------
		PAC				; ACC = psi_dr_se*i_qs_se  (Q30)
      	                ; ARP=AR2, AR0->FR0, AR2->i_ds_se 
;----------------------------------------------------------------------------------
     	LT		*+		; TREG = i_ds_se  (Q15)
     					; ARP=AR2, AR0->FR0, AR2->psi_qr_se
;----------------------------------------------------------------------------------
      	MPY		*+,AR0	; PREG = psi_qr_se*i_ds_se  (Q30)
      	                ; ARP=AR2, AR0->FR0, AR2->K1_se, ARP=AR0      	
;----------------------------------------------------------------------------------
		SPAC			; ACC = psi_dr_se*i_qs_se-psi_qr_se*i_ds_se  (Q30)
						; ARP=AR0, AR0->FR0, AR2->K1_se    
;----------------------------------------------------------------------------------
		SACH	*,1,AR2	; FR0 = tmp1_se = psi_dr_se*i_qs_se-psi_qr_se*i_ds_se  (Q15)
						; ARP=AR0, AR0->FR0, AR2->K1_se, ARP=AR2
;----------------------------------------------------------------------------------
       	LT		*-,AR0	; TREG = K1_se   (Q15)
       					; ARP=AR2, AR0->FR0, AR2->psi_qr_se, ARP=AR0 
;----------------------------------------------------------------------------------
  		MPY		*		; PREG = K1_se*(psi_dr_se*i_qs_se-psi_qr_se*i_ds_se) (Q30)
  						; ARP=AR0, AR0->FR0, AR2->psi_qr_se
;----------------------------------------------------------------------------------
		PAC				; ACC = K1_se*(psi_dr_se*i_qs_se-psi_qr_se*i_ds_se) (Q30)
  						; ARP=AR0, AR0->FR0, AR2->psi_qr_se
;----------------------------------------------------------------------------------
    	SACH	*+,1,AR2 ; FR0 = tmp1_se = K1_se*(psi_dr_se*i_qs_se-psi_qr_se*i_ds_se) (Q15)
  						; ARP=AR0, AR0->FR1, AR2->psi_qr_se, ARP=AR2
;----------------------------------------------------------------------------------
     	LT		*		; TREG = psi_qr_se  (Q15)
     					; ARP=AR2, AR0->FR1, AR2->psi_qr_se  	
;----------------------------------------------------------------------------------
       	MPY		*		; PREG = psi_qr_se*psi_qr_se  (Q30)
     					; ARP=AR2, AR0->FR1, AR2->psi_qr_se  
;----------------------------------------------------------------------------------
		PAC				; ACC = psi_qr_se*psi_qr_se  (Q30)
     					; ARP=AR2, AR0->FR1, AR2->psi_qr_se 
;----------------------------------------------------------------------------------
     	SBRK	#2		; ARP=AR2, AR0->FR1, AR2->psi_dr_se  
;----------------------------------------------------------------------------------
     	LT		*		; TREG = psi_dr_se  (Q15)
     					; ARP=AR2, AR0->FR1, AR2->psi_dr_se  	
;----------------------------------------------------------------------------------
       	MPY		*		; PREG = psi_dr_se*psi_dr_se  (Q30)
     					; ARP=AR2, AR0->FR1, AR2->psi_dr_se  
;----------------------------------------------------------------------------------
		APAC			; ACC = psi_qr_se^2 + psi_dr_se^2  (Q30)
						; ARP=AR2, AR0->FR1, AR2->psi_dr_se 
;----------------------------------------------------------------------------------
		ADRK	#4		; ARP=AR2, AR0->FR1, AR2->psi_r_2 
;----------------------------------------------------------------------------------
      	SACH	*+,1,AR0 ; psi_r_2 = psi_qr_se^2 + psi_dr_se^2  (Q15)
						; ARP=AR2, AR0->FR0, AR2->theta_r_se, ARP=AR0
;----------------------------------------------------------------------------------
		LACC	*+,15	; ACC = FR0 = tmp1_se left shifted by 15 (w_sl=Q15)
						; ARP=AR0, AR0->FR1, AR2->theta_r_se