cur_mod.asm

变频器程序

      	SACL	*		; FR0 = tmp = iSq/i_mr = 0  (Q12) if i_mr = 0
      					; ARP=AR0, AR0->FR0, AR2->i_mr 
;----------------------------------------------------------------------------------
       	B		I_MRZERO ; ARP=AR0, AR0->FR0, AR2->i_mr 
;----------------------------------------------------------------------------------
I_MRNOTZERO            	; ARP=AR2, AR0->FR0, AR2->i_mr 
;----------------------------------------------------------------------------------
; ***** division (iSq/i_mr)
;----------------------------------------------------------------------------------
	    LACC	*,AR0	; ACC = i_mr  (Q12)
	    				; ARP=AR2, AR0->FR0, AR2->i_mr, ARP=AR0
;----------------------------------------------------------------------------------
      	BCND	I_MRZERO,EQ ; Branch to I_MRZERO if i_mr = 0
      					; ARP=AR0, AR0->FR0, AR2->i_mr  	
;----------------------------------------------------------------------------------
		ADRK	#1		; ARP=AR0, AR0->FR1, AR2->i_mr
;----------------------------------------------------------------------------------
      	LACC	*-		; ACC = FR1 = iSq  (Q12)
      					; ARP=AR0, AR0->FR0, AR2->i_mr
;----------------------------------------------------------------------------------
    	ABS				; ACC = |iSq|  (Q12)
      					; ARP=AR0, AR0->FR0, AR2->i_mr
;----------------------------------------------------------------------------------
		SACL	*		; FR0 = tmp = |iSq|  (Q12)
      					; ARP=AR0, AR0->FR0, AR2->i_mr	
;----------------------------------------------------------------------------------
    	LACC	*,12,AR2 ; ACC = |iSq| left shifted 12 bit (tmp=iSq/i_mr=Q12)
    					; ARP=AR0, AR0->FR0, AR2->i_mr, ARP=AR2
;----------------------------------------------------------------------------------
      	RPT		#15		; Repeat SUBC 16 times
      					; ARP=AR2, AR0->FR0, AR2->i_mr
;----------------------------------------------------------------------------------
		SUBC	*		; Dividing |iSq|/i_mr
						; ARP=AR2, AR0->FR0, AR2->i_mr		
;----------------------------------------------------------------------------------
    	MAR		*,AR0	; ARP=AR0, AR0->FR0, AR2->i_mr
;----------------------------------------------------------------------------------
     	SACL	*+		; FR0 = tmp = |iSq|/i_mr  (Q12)
						; ARP=AR0, AR0->FR1, AR2->i_mr  	
;----------------------------------------------------------------------------------
		LACC	*-		; ACC = iSq  (Q12)
						; ARP=AR0, AR0->FR0, AR2->i_mr	
;----------------------------------------------------------------------------------
		BCND	ISQPOS,GT ; Branch to ISQPOS if iSq > 0
						; ARP=AR0, AR0->FR0, AR2->i_mr 
;----------------------------------------------------------------------------------
     	LACC	*		; ACC = FR0 = tmp = |iSq|/i_mr  (Q12)
     					; ARP=AR0, AR0->FR0, AR2->i_mr	
;----------------------------------------------------------------------------------
		NEG				; ACC = -|iSq|/i_mr  (Q12)
     					; ARP=AR0, AR0->FR0, AR2->i_mr	
;----------------------------------------------------------------------------------
       	SACL	*		; FR0 = tmp = -|iSq|/i_mr  (Q12)
     					; ARP=AR0, AR0->FR0, AR2->i_mr
;----------------------------------------------------------------------------------
ISQPOS                 	; ARP=AR0, AR0->FR0, AR2->i_mr
;----------------------------------------------------------------------------------
I_MRZERO 				; ARP=AR0, AR0->FR0, AR2->i_mr 
;----------------------------------------------------------------------------------
     	LT		*,AR2	; TREG = FR0 = tmp = iSq/i_mr  (Q12)
     					; ARP=AR0, AR0->FR0, AR2->i_mr, ARP=AR2
;----------------------------------------------------------------------------------
		ADRK	#2		; ARP=AR2, AR0->FR0, AR2->Kt	
;----------------------------------------------------------------------------------
      	MPY		*+,AR0	; PREG = Kt*(iSq/i_mr)  (Q24)
      					; ARP=AR2, AR0->FR0, AR2->fs, ARP=AR0 
;----------------------------------------------------------------------------------
		PAC				; ACC = Kt*(iSq/i_mr)  (Q24)
      					; ARP=AR0, AR0->FR0, AR2->fs
;----------------------------------------------------------------------------------
		SACH	*,4		; FR0 = tmp = Kt*(iSq/i_mr) = slip (Q12)
      					; ARP=AR0, AR0->FR0, AR2->fs
;----------------------------------------------------------------------------------
		LACC	*		; ACC = FR0 = tmp = Kt*(iSq/i_mr) = slip (Q12)
						; ARP=AR0, AR0->FR0, AR2->fs 
;----------------------------------------------------------------------------------
      	ADRK	#2		; ARP=AR0, AR0->FR2, AR2->fs
;----------------------------------------------------------------------------------
		ADD		*,AR2   ; ACC = Kt*(iSq/i_mr) + n (Q12)
						; ARP=AR0, AR0->FR2, AR2->fs, ARP=AR2 
;----------------------------------------------------------------------------------
      	SACL	*		; fs = Kt*(iSq/i_mr) + n (Q12)
      					; ARP=AR2, AR0->FR2, AR2->fs	
;----------------------------------------------------------------------------------
; *** rotor flux position calculation *** 		
;----------------------------------------------------------------------------------
		LACC	*+,AR0	; ACC = fs  (Q12)
						; ARP=AR2, AR0->FR2, AR2->K, ARP=AR0
;----------------------------------------------------------------------------------
		ABS				; ACC = |fs|  (Q12)
						; ARP=AR0, AR0->FR2, AR2->K   		
;----------------------------------------------------------------------------------
     	SACL	*		; FR2 = tmp = |fs|  (Q12)
						; ARP=AR0, AR0->FR2, AR2->K
;----------------------------------------------------------------------------------
     	LT		*,AR2	; TREG = FR2 = tmp = |fs|  (Q12)
						; ARP=AR0, AR0->FR2, AR2->K, ARP=AR2
;----------------------------------------------------------------------------------
		MPY		*-,AR0	; PREG = K*|fs|  (Q12)
						; ARP=AR2, AR0->FR2, AR2->fs, ARP=AR0
;----------------------------------------------------------------------------------
     	PAC				; ACC = K*|fs|  (Q12)
						; ARP=AR0, AR0->FR2, AR2->fs
;----------------------------------------------------------------------------------
		SACH	*,4,AR2	; FR2 = tetaincr = K*|fs|  (Q0)
						; ARP=AR0, AR0->FR2, AR2->fs, ARP=AR2
;----------------------------------------------------------------------------------
		BIT		*,0,AR0	; TC bit in ST1 register = fs(bit15) = MSB of fs
						; ARP=AR2, AR0->FR2, AR2->fs, ARP=AR0
;----------------------------------------------------------------------------------
		BCND	FS_NEG,TC ; Branch to FS_NEG if TC = 1 or fs < 0
						; ARP=AR0, AR0->FR2, AR2->fs 
;----------------------------------------------------------------------------------
		LACL	*,AR2	; ACC = FR2 = tetaincr  (Q0)
						; ARP=AR0, AR0->FR2, AR2->fs, ARP=AR2
;----------------------------------------------------------------------------------
		ADRK	#2		; ARP=AR2, AR0->FR2, AR2->Teta_cm      	
;----------------------------------------------------------------------------------
		ADDS	*		; ACC =  Teta_cm + tetaincr  (Q0)
						; ARP=AR2, AR0->FR2, AR2->Teta_cm
;----------------------------------------------------------------------------------
      	SACL	*+		; Teta_cm = Teta_cm + tetaincr  (Q0)
						; ARP=AR2, AR0->FR2, AR2->theta_cur_mod	
;----------------------------------------------------------------------------------
		B		FS_POS	; ARP=AR2, AR0->FR2, AR2->theta_cur_mod	
;----------------------------------------------------------------------------------
FS_NEG                 	; ARP=AR0, AR0->FR2, AR2->fs      	
;----------------------------------------------------------------------------------
    	MAR		*,AR2	; ARP=AR2, AR0->FR2, AR2->fs	
;----------------------------------------------------------------------------------
	   	ADRK	#2		; ARP=AR2, AR0->FR2, AR2->Teta_cm	
;----------------------------------------------------------------------------------
		LACL	*,AR0	; ACC = Teta_cm  (Q0)
						; ARP=AR2, AR0->FR2, AR2->Teta_cm, ARP=AR0
;----------------------------------------------------------------------------------
		SUBS	*,AR2	; ACC = Teta_cm - tetaincr (Q0)
						; ARP=AR0, AR0->FR2, AR2->Teta_cm, ARP=AR2
;----------------------------------------------------------------------------------
		SACL	*+		; Teta_cm = Teta_cm - tetaincr (Q0)
						; ARP=AR2, AR0->FR2, AR2->theta_cur_mod	
;----------------------------------------------------------------------------------
FS_POS                 	; ARP=AR2, AR0->FR2, AR2->theta_cur_mod	
;----------------------------------------------------------------------------------
    	SFR				; ACC = Teta_cm  (0-FFFFh)->(0-7FFFh)=(0-360) (Q15) 
    					; ARP=AR2, AR0->FR2, AR2->theta_cur_mod
;----------------------------------------------------------------------------------
		AND		#7FFFh	; Force theta_cur_mod around 0-7FFFh
                        ; ARP=AR2, AR0->FR2, AR2->theta_cur_mod
;----------------------------------------------------------------------------------
     	SACL	*,AR1	; theta_cur_mod = Teta_cm (Q15)
     					; ARP=AR2, AR0->FR2, AR2->theta_cur_mod, ARP=AR1
;----------------------------------------------------------------------------------
_cur_mod_calc_exit:
        ;;	MAR     *,AR1   ; can be removed if this condition is met on
       	                ; every path to this code. (i.e., ARP=AR1 here)

        CLRC	OVM
        CLRC	SXM

    	SBRK 	#(__cur_mod_calc_framesize+1)
        LAR  	AR0,*-
        PSHD	*
        
        RET