cur_mod.asm

变频器程序

;=====================================================================================
; File name:        CUR_MOD.ASM                      
;                    
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:  Current model for indirect field oriented control of Induction Motor               
;
; Note that theta_cur_mod = (0,7FFFh) => (0,360 degree) 
; 
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 02-15-2001	Release	Rev 1.0
;-------------------------------------------------------------------------------------
; The following modifications have made differently from its "ASM" version.
; 1) No Pole_pairs_number involved because of per-unit representation
; 2) Use "SFR" instead of "AND 7FFFh" to chance angle (0-360 deg) from 0-FFFFh to 0-7FFFh
; 3) No confusing base speed to cause different transformation of Q15 to Q12 (using 3 SFR's)
;-------------------------------------------------------------------------------------
;
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx.  (Peripheral Independent).
;================================================================================
; Routine Name: cur_mod_calc 				  					    Type: C Callable
;  
;  C prototype : void cur_mod_calc(struct CURMOD *p);
;
;        The struct object is defined in the header file "cur_mod.h" as follows:
;
; typedef struct {  int  i_cur_mod_D;  	/* Input: Syn. Rotating d-axis stator current (Q15) */
;                 int  i_cur_mod_Q;  	/* Input: Syn. Rotating q-axis stator current (Q15) */
;                 int  spd_cur_mod;     /* Input: Rotor speed (Q15) */
;                 int  i_mr;            /* Variable: Magnetizing current (Q12) */
;                 int  Kr;              /* Parameter: Constant (Q15) */
;                 int  Kt;              /* Parameter: Constant (Q12) */
;                 int  fs;              /* Variable: Rotor flux electrical speed (Q12) */
;                 int  K;               /* Parameter: Constant (Q0) */
;                 int  Teta_cm;         /* Variable: Rotor flux position (Q0) */
;                 int  theta_cur_mod;   /* Output: Rotor flux position (Q15) */
;		 	 	  int  (*calc)();	    /* Pointer to calculation function */ 
;				 } CURMOD;	            
;	 		 
;        Frame Usage Details:
;     step   |      a      |      b       |      c       |     d     
;____________|_____________|______________|______________|_____________
;     AR0  	 |    iSd      |    tmp       |          	 |     
;     AR1  	 |    iSq      |              | 			 |     
;     AR2  	 |     n       |    tmp       |    tetaincr  |     
;
;================================================================================
                .def        _cur_mod_calc
;================================================================================
__cur_mod_calc_framesize .set 0003h
;================================================================================
_cur_mod_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,__cur_mod_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_cur_mod_D, AR2->i_cur_mod_D
;----------------------------------------------------------------------------------
		ADRK	#3		; ARP=AR0, AR0->FR0, AR2->i_cur_mod_D
;----------------------------------------------------------------------------------
		MAR		*,AR2  	; ARP=AR2, AR0->FR0, AR2->i_cur_mod_D
;----------------------------------------------------------------------------------
		SETC 	SXM		; Turn sign extension mode on
						; ARP=AR2, AR0->FR0, AR2->i_cur_mod_D
;----------------------------------------------------------------------------------
		SETC	OVM		; Set overflow mode
						; ARP=AR2, AR0->FR0, AR2->i_cur_mod_D
;----------------------------------------------------------------------------------
 		SPM     0       ; Reset product mode
       					; ARP=AR2, AR0->FR0, AR2->i_cur_mod_D
;----------------------------------------------------------------------------------
      	LACC	*+,AR0	; ACC = i_cur_mod_D  (Q15)
      					; ARP=AR2, AR0->FR0, AR2->i_cur_mod_Q, ARP=AR0
;----------------------------------------------------------------------------------
	    SFR				; ACC = i_cur_mod_D  (Q14)
	                    ; ARP=AR0, AR0->FR0, AR2->i_cur_mod_Q
;----------------------------------------------------------------------------------
	    SFR				; ACC = i_cur_mod_D  (Q13)
	                    ; ARP=AR0, AR0->FR0, AR2->i_cur_mod_Q		
;----------------------------------------------------------------------------------
	    SFR				; ACC = i_cur_mod_D  (Q12)
	                    ; ARP=AR0, AR0->FR0, AR2->i_cur_mod_Q		
;----------------------------------------------------------------------------------
      	SACL	*+,AR2	; FR0 = iSd = i_cur_mod_D  (Q12)
      	                ; ARP=AR0, AR0->FR1, AR2->i_cur_mod_Q, ARP=AR2
;----------------------------------------------------------------------------------
      	LACC	*+,AR0	; ACC = i_cur_mod_Q  (Q15)
      					; ARP=AR2, AR0->FR1, AR2->spd_cur_mod, ARP=AR0
;----------------------------------------------------------------------------------
  		SFR				; ACC = i_cur_mod_Q  (Q14)
      					; ARP=AR0, AR0->FR1, AR2->spd_cur_mod
;----------------------------------------------------------------------------------
  		SFR				; ACC = i_cur_mod_Q  (Q13)
      					; ARP=AR0, AR0->FR1, AR2->spd_cur_mod
;----------------------------------------------------------------------------------
  		SFR				; ACC = i_cur_mod_Q  (Q12)
      					; ARP=AR0, AR0->FR1, AR2->spd_cur_mod
;----------------------------------------------------------------------------------
      	SACL	*+,AR2	; FR1 = iSq = i_cur_mod_Q  (Q12)
      	                ; ARP=AR0, AR0->FR2, AR2->spd_cur_mod, ARP=AR2
;----------------------------------------------------------------------------------
    	LACC	*+,AR0	; ACC = spd_cur_mod  (Q15)
    					; ARP=AR2, AR0->FR2, AR2->i_mr, ARP=AR0
;----------------------------------------------------------------------------------
  		SFR				; ACC = spd_cur_mod  (Q14)
      					; ARP=AR0 AR0->FR2, AR2->i_mr
;----------------------------------------------------------------------------------
  		SFR				; ACC = spd_cur_mod  (Q13)
      					; ARP=AR0 AR0->FR2, AR2->i_mr
;----------------------------------------------------------------------------------
  		SFR				; ACC = spd_cur_mod  (Q12)
      					; ARP=AR0 AR0->FR2, AR2->i_mr
;----------------------------------------------------------------------------------
      	SACL	*		; FR2 = n = spd_cur_mod  (Q12)
      	                ; ARP=AR0, AR0->FR2, AR2->i_mr
;----------------------------------------------------------------------------------
; *** Current Model ***
;----------------------------------------------------------------------------------
      	SBRK	#2		; ARP=AR0, AR0->FR0, AR2->i_mr 
;----------------------------------------------------------------------------------
		LACC	*,AR2	; ACC = FR0 = iSd  (Q12)
						; ARP=AR0, AR0->FR0, AR2->i_mr, ARP=AR2
;----------------------------------------------------------------------------------
		SUB		*+,AR0  ; ACC = iSd - i_mr (Q12)
						; ARP=AR2, AR0->FR0, AR2->Kr, ARP=AR0
;----------------------------------------------------------------------------------
		SACL	*		; FR0 = tmp = iSd - i_mr (Q12)
						; ARP=AR0, AR0->FR0, AR2->Kr
;----------------------------------------------------------------------------------
      	LT		*,AR2	; TREG = tmp = iSd - i_mr (Q12)
      					; ARP=AR0, AR0->FR0, AR2->Kr, ARP=AR2
;----------------------------------------------------------------------------------
   		MPY		*-,AR0	; PREG = Kr*(iSd - i_mr)  (Q27)
      					; ARP=AR2, AR0->FR0, AR2->i_mr, ARP=AR0
;----------------------------------------------------------------------------------
      	PAC				; ACC = Kr*(iSd - i_mr)  (Q27)
      					; ARP=AR0, AR0->FR0, AR2->i_mr
;----------------------------------------------------------------------------------
     	SACH	*,1		; FR0 = tmp = Kr*(iSd - i_mr)  (Q12)
      					; ARP=AR0, AR0->FR0, AR2->i_mr
;----------------------------------------------------------------------------------
	   	LACC	*,AR2	; ACC = FR0 = tmp = Kr*(iSd - i_mr)  (Q12)
      					; ARP=AR0, AR0->FR0, AR2->i_mr, ARP=AR2
;----------------------------------------------------------------------------------
		ADD		*		; ACC = Kr*(iSd - i_mr) + i_mr  (Q12)
						; ARP=AR2, AR0->FR0, AR2->i_mr
;----------------------------------------------------------------------------------
		SACL	*		; i_mr = Kr*(iSd - i_mr) + i_mr  (Q12)
						; ARP=AR2, AR0->FR0, AR2->i_mr
;----------------------------------------------------------------------------------
      	BCND	I_MRNOTZERO,NEQ ; Branch to I_MRNOTZERO if i_mr != 0
      					; ARP=AR2, AR0->FR0, AR2->i_mr 
;----------------------------------------------------------------------------------
 		LACC	#0		; ACC = 0
 						; ARP=AR2, AR0->FR0, AR2->i_mr 
;----------------------------------------------------------------------------------
		MAR		*,AR0	; ARP=AR0, AR0->FR0, AR2->i_mr 
;----------------------------------------------------------------------------------