aci_mras.asm

TI的digital motor control lib的源代码。了解TI的编程规范

;----------------------------------------------------------------------------------
		LACC	*		; ACC = sign2
						; ARP=AR0, AR0->FR4, AR2->imalfa_old_low, AR3->K7                    
;----------------------------------------------------------------------------------
    	ADRK	#4		; ARP=AR0, AR0->FR8, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
      	BCND	DONE3,GT ; Check sign2 = positive ?
      					; ARP=AR0, AR0->FR8, AR2->imalfa_old_low, AR3->K7 
;----------------------------------------------------------------------------------
		LACC	*-,16	; ACC high = tmp2_high
						; ARP=AR0, AR0->FR7, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
       	ADDS	*		; ACC low = tmp2_low
       					; ARP=AR0, AR0->FR7, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
		NEG				; Make the result negative
						; ARP=AR0, AR0->FR7, AR2->imalfa_old_low, AR3->K7 
;----------------------------------------------------------------------------------
    	SACL	*+		; FR7 = tmp2_low = ACC low
    					; ARP=AR0, AR0->FR8, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
      	SACH	*		; FR8 = tmp2_high = ACC high
      					; ARP=AR0, AR0->FR8, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
DONE3		            ; ARP=AR0, AR0->FR8, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
		SBRK	#6		; ARP=AR0, AR0->FR2, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
        LT		*,AR2  	; TREG = tmp4
        				; ARP=AR0, AR0->FR2, AR2->imalfa_old_low, AR3->K7, ARP=AR2
;----------------------------------------------------------------------------------
     	MPYU	*-,AR0	; PREG = imalfa_old_low*tmp4
     					; ARP=AR2, AR0->FR2, AR2->imalfa_old_high, AR3->K7, ARP=AR0	
;----------------------------------------------------------------------------------
    	ADRK	#7		; ARP=AR0, AR0->FR9, AR2->imalfa_old_high, AR3->K7
;----------------------------------------------------------------------------------
		SPH		*,AR2	; FR9 = tmp3_low = PREG high (save partial result)
						; ARP=AR0, AR0->FR9, AR2->imalfa_old_high, AR3->K7, ARP=AR2
;----------------------------------------------------------------------------------
		MPYU	*+,AR0	; PREG = imalfa_old_high*tmp4
						; ARP=AR2, AR0->FR9, AR2->imalfa_old_low, AR3->K7, ARP=AR0
;----------------------------------------------------------------------------------
		PAC				; ACC = imalfa_old_high*tmp4
						; ARP=AR0, AR0->FR9, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
		ADDS	*+		; ACC = imalfa_old_high*tmp4 + tmp3_low
						; ARP=AR0, AR0->FR10, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
		SACH	*-,1	; FR10 = tmp3_high = ACC high (Q31)
						; ARP=AR0, AR0->FR9, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
		SACL	*,1		; FR9 = tmp3_low = ACC low  (Q31)
						; ARP=AR0, AR0->FR9, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
      	SBRK	#4		; ARP=AR0, AR0->FR5, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
		LACC	*		; ACC = sign3
						; ARP=AR0, AR0->FR5, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
 		ADRK	#5		; ARP=AR0, AR0->FR10, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
     	BCND	DONE4,GT ; Check sign3 = positive ?
     					; ARP=AR0, AR0->FR10, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
       	LACC	*-,16	; ACC high = tmp3_high
       					; ARP=AR0, AR0->FR9, AR2->imalfa_old_low, AR3->K7 
;----------------------------------------------------------------------------------
    	ADDS	*		; ACC low = tmp3_low
    					; ARP=AR0, AR0->FR9, AR2->imalfa_old_low, AR3->K7  
;----------------------------------------------------------------------------------
      	NEG				; Make the result negative
      					; ARP=AR0, AR0->FR9, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
   		SACL	*+		; FR9 = tmp3_low = ACC low
    					; ARP=AR0, AR0->FR10, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
      	SACH	*		; FR10 = tmp3_high = ACC high
      					; ARP=AR0, AR0->FR10, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
DONE4		            ; ARP=AR0, AR0->FR10, AR2->imalfa_old_low, AR3->K7     	
;----------------------------------------------------------------------------------
      	LACC	*-,16	; ACC high = tmp3_high
      					; ARP=AR0, AR0->FR9, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
       	ADDS	*-		; ACC low = tmp3_low
       					; ARP=AR0, AR0->FR8, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
    	ADDH	*-		; ACC = tmp3_high + tmp2_high
    					; ARP=AR0, AR0->FR7, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
      	ADDS	*		; ACC = tmp3_low + tmp2_low
      					; ARP=AR0, AR0->FR7, AR2->imalfa_old_low, AR3->K7 		
;----------------------------------------------------------------------------------
      	ADRK	#2		; ARP=AR0, AR0->FR9, AR2->imalfa_old_low, AR3->K7
;----------------------------------------------------------------------------------
		SACL	*+		; FR9 = tmp3_low = ACC low
						; ARP=AR0, AR0->FR10, AR2->imalfa_old_low, AR3->K7		
;----------------------------------------------------------------------------------
       	SACH	*-,AR3	; FR10 = tmp3_high = ACC high
						; ARP=AR0, AR0->FR9, AR2->imalfa_old_low, AR3->K7, ARP=AR3
;----------------------------------------------------------------------------------
		LT		*,AR2	; TREG = K7  (Q15)
						; ARP=AR3, AR0->FR9, AR2->imalfa_old_low, AR3->K7, ARP=AR2 
;----------------------------------------------------------------------------------
       	SBRK	#4		; ARP=AR2, AR0->FR9, AR2->ibeta_mras, AR3->K7
;----------------------------------------------------------------------------------
		MPY		*,AR0	; PREG = K7*ibeta_mras  (Q30)
						; ARP=AR2, AR0->FR9, AR2->ibeta_mras, AR3->K7, ARP=AR0
;----------------------------------------------------------------------------------
		PAC				; ACC = K7*ibeta_mras  (Q30)
						; ARP=AR0, AR0->FR9, AR2->ibeta_mras, AR3->K7
;----------------------------------------------------------------------------------
		SFL				; ACC = K7*ibeta_mras  (Q31)
						; ARP=AR0, AR0->FR9, AR2->ibeta_mras, AR3->K7
;----------------------------------------------------------------------------------
		ADDS	*+		; ACC = K7*ibeta_mras + tmp3_low  (Q31)
						; ARP=AR0, AR0->FR10, AR2->ibeta_mras, AR3->K7
;----------------------------------------------------------------------------------
		ADDH	*,AR2	; ACC = K7*ibeta_mras + tmp3_high  (Q31)
						; ARP=AR0, AR0->FR10, AR2->ibeta_mras, AR3->K7, ARP=AR2
;----------------------------------------------------------------------------------
		ADRK	#10		; ARP=AR2, AR0->FR10, AR2->imbeta_low, AR3->K7
;----------------------------------------------------------------------------------
		SACL	*-		; imbeta_low = ACC low  (Q31)
						; ARP=AR2, AR0->FR10, AR2->imbeta_high, AR3->K7		
;----------------------------------------------------------------------------------
        SACH	*		; imbeta_high = ACC high  (Q31)
						; ARP=AR2, AR0->FR10, AR2->imbeta_high, AR3->K7	
;----------------------------------------------------------------------------------
		LACC	*+,16	; ACC high = imbeta_high  (Q31)
						; ARP=AR2, AR0->FR10, AR2->imbeta_low, AR3->K7
;----------------------------------------------------------------------------------
     	ADDS	*		; ACC low = imbeta_low  (Q31)
     					; ARP=AR2, AR0->FR10, AR2->imbeta_low, AR3->K7
;----------------------------------------------------------------------------------
      	SBRK	#4 		; ARP=AR2, AR0->FR10, AR2->imbeta_old_low, AR3->K7
;----------------------------------------------------------------------------------
        SACL	*-		; imbeta_old_low = imbeta_low  (Q31)
						; ARP=AR2, AR0->FR10, AR2->imbeta_old_high, AR3->K7		
;----------------------------------------------------------------------------------
        SACH	*		; imbeta_old_high = imbeta_high  (Q31)
						; ARP=AR2, AR0->FR10, AR2->imbeta_old_high, AR3->K7	
;----------------------------------------------------------------------------------
      	ADRK	#2		; ARP=AR2, AR0->FR10, AR2->imalfa_high, AR3->K7	
;----------------------------------------------------------------------------------
		LACC	*+,16	; ACC high = imalfa_high (Q31)
						; ARP=AR2, AR0->FR10, AR2->imalfa_low, AR3->K7	
;----------------------------------------------------------------------------------
		ADDS	*		; ACC low = imalfa_low   (Q31)
						; ARP=AR2, AR0->FR10, AR2->imalfa_low, AR3->K7
;----------------------------------------------------------------------------------
        SBRK	#4		; ARP=AR2, AR0->FR10, AR2->imalfa_old_low, AR3->K7 
;----------------------------------------------------------------------------------
       	SACL	*-		; imalfa_old_low = imalfa_low  (Q31)
       					; ARP=AR2, AR0->FR10, AR2->imalfa_old_high, AR3->K7 
;----------------------------------------------------------------------------------
       	SACH	*		; imalfa_old_high = imalfa_high  (Q31)
       					; ARP=AR2, AR0->FR10, AR2->imalfa_old_high, AR3->K7 
;----------------------------------------------------------------------------------
E_ALFA		            ; ARP=AR2, AR0->FR10, AR2->imalfa_old_high, AR3->K7
;----------------------------------------------------------------------------------
    	SBRK	#4		; ARP=AR2, AR0->FR10, AR2->ialfa_mras, AR3->K7
;----------------------------------------------------------------------------------
    	LACC	*,16	; ACC high = ialfa_mras
    					; ARP=AR2, AR0->FR10, AR2->ialfa_mras, AR3->K7  
;----------------------------------------------------------------------------------
    	ADRK 	#9		; ARP=AR2, AR0->FR10, AR2->imalfa_low, AR3->K7
;----------------------------------------------------------------------------------
		SUBS	*-		; ACC low = ialfa_mras - imalfa_low
						; ARP=AR2, AR0->FR10, AR2->imalfa_high, AR3->K7  
;----------------------------------------------------------------------------------
		SUBH	*,AR0	; ACC high = ialfa_mras - imalfa_high
						; ARP=AR2, AR0->FR10, AR2->imalfa_high, AR3->K7, ARP=AR0
;----------------------------------------------------------------------------------
		SBRK	#9		; ARP=AR0, AR0->FR1, AR2->imalfa_high, AR3->K7      	
;----------------------------------------------------------------------------------
		SACH	*-,AR2	; FR1 = tmp1_low = ialfa_mras - imalfa_high (Q15)
						; ARP=AR0, AR0->FR0, AR2->imalfa_high, AR3->K7, ARP=AR2
;----------------------------------------------------------------------------------
       	SBRK	#7		; ARP=AR2, AR0->FR0, AR2->ibeta_mras, AR3->K7
;----------------------------------------------------------------------------------
		LACC	*,16	; ACC high = ibeta_mras
						; ARP=AR2, AR0->FR0, AR2->ibeta_mras, AR3->K7 
;----------------------------------------------------------------------------------
       	ADRK	#10		; ARP=AR2, AR0->FR0, AR2->imbeta_low, AR3->K7
;----------------------------------------------------------------------------------
		SUBS	*-		; ACC low = ibeta_mras - imbeta_low
						; ARP=AR2, AR0->FR0, AR2->imbeta_high, AR3->K7
;----------------------------------------------------------------------------------
       	SUBH	*,AR0	; ACC high = ibeta_mras - imbeta_high
						; ARP=AR2, AR0->FR0, AR2->imbeta_high, AR3->K7, ARP=AR0
;----------------------------------------------------------------------------------
      	SACH	*,AR3	; FR0 = tmp1_high = ibeta_mras - i