aci_mras.asm

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

       					; ARP=AR0, AR0->FR5, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
		SPL		*-		; FR5 = tmp5 = PREG low
						; ARP=AR0, AR0->FR4, AR2->ibeta_mras, AR3->K1 			
;----------------------------------------------------------------------------------
		MPYU	*		; PREG = K1*tmp1_high
						; ARP=AR0, AR0->FR4, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
       	PAC				; ACC = K1*tmp1_high
       					; ARP=AR0, AR0->FR4, AR2->ibeta_mras, AR3->K1  
;----------------------------------------------------------------------------------
      	ADRK	#2		; ARP=AR0, AR0->FR6, AR2->ibeta_mras, AR3->K1
;----------------------------------------------------------------------------------
		ADDS	*		; ACC = K1*tmp1_high + tmp4
						; ARP=AR0, AR0->FR6, AR2->ibeta_mras, AR3->K1
;----------------------------------------------------------------------------------
       	SBRK	#3		; ARP=AR0, AR0->FR3, AR2->ibeta_mras, AR3->K1
;----------------------------------------------------------------------------------
		SACL	*+,6	; FR3 = tmp1_low = ACC low   (Q31)
						; ARP=AR0, AR0->FR4, AR2->ibeta_mras, AR3->K1
;----------------------------------------------------------------------------------
        SACH	*+,6	; FR4 = tmp1_high = ACC high   (Q31)
						; ARP=AR0, AR0->FR5, AR2->ibeta_mras, AR3->K1
;----------------------------------------------------------------------------------
      	LACC	*		; ACC = tmp5
      					; ARP=AR0, AR0->FR5, AR2->ibeta_mras, AR3->K1
;----------------------------------------------------------------------------------
		AND		#0FC00h	; Pick MSB 6 bits in low 16 bits of K1*tmp1_low
						; ARP=AR0, AR0->FR5, AR2->ibeta_mras, AR3->K1 	 
;----------------------------------------------------------------------------------
 		SACH	*,6		; FR5 = tmp5 = MSB 6 bits in low 16 bits of K1*tmp1_low
 						; ARP=AR0, AR0->FR5, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
		LACL	*		; ACC = tmp5
						; ARP=AR0, AR0->FR5, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
		SBRK	#2		; ARP=AR0, AR0->FR3, AR2->ibeta_mras, AR3->K1 		
;----------------------------------------------------------------------------------
		ADDS	*		; FR3 = tmp1_low = MSB 6 bits in low 16 bits of K1*tmp1_low
						; ARP=AR0, AR0->FR3, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
		SACL	*-		; FR3 = tmp1_low = ACC low
						; ARP=AR0, AR0->FR2, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
		LACC	*		; ACC = sign1
						; ARP=AR0, AR0->FR2, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
        ADRK	#2		; ARP=AR0, AR0->FR4, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
     	BCND	DONE0,GT ; Check sign1 = positive ?
     					; ARP=AR0, AR0->FR4, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
		LACC	*-,16	; ACC high = tmp1_high
						; ARP=AR0, AR0->FR3, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
		ADDS	*		; ACC = tmp1_high + tmp1_low
						; ARP=AR0, AR0->FR3, AR2->ibeta_mras, AR3->K1
;----------------------------------------------------------------------------------
		NEG				; Make the result negative
						; ARP=AR0, AR0->FR3, AR2->ibeta_mras, AR3->K1
;----------------------------------------------------------------------------------
       	SACL	*+		; FR3 = tmp1_low = ACC low
       					; ARP=AR0, AR0->FR4, AR2->ibeta_mras, AR3->K1  
;----------------------------------------------------------------------------------
		SACH	*		; FR4 = tmp1_high = ACC high
						; ARP=AR0, AR0->FR4, AR2->ibeta_mras, AR3->K1
;----------------------------------------------------------------------------------
DONE0                   ; ARP=AR0, AR0->FR4, AR2->ibeta_mras, AR3->K1						
;----------------------------------------------------------------------------------
     	LACC	*-,16	; ACC high = tmp1_high  (Q31)
     					; ARP=AR0, AR0->FR3, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
		ADDS	*-		; ACC low = tmp1_low  (Q31)
						; ARP=AR0, AR0->FR2, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
		NEG				; ACC = -[tmp1_high tmp1_low]     (Q31)
						; ARP=AR0, AR0->FR2, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
      	SBRK	#2		; ARP=AR0, AR0->FR0, AR2->ibeta_mras, AR3->K1 	
;----------------------------------------------------------------------------------
		ADDS	*+		; ACC = tmp2_low - tmp1_low  (Q31)
						; ARP=AR0, AR0->FR1, AR2->ibeta_mras, AR3->K1
;----------------------------------------------------------------------------------
		ADDH	*		; ACC = tmp2_high - tmp1_high  (Q31)
						; ARP=AR0, AR0->FR1, AR2->ibeta_mras, AR3->K1
;----------------------------------------------------------------------------------
       	ADRK	#5		; ARP=AR0, AR0->FR6, AR2->ibeta_mras, AR3->K1 
;----------------------------------------------------------------------------------
    	SACH	*		; FR6 = tmp4 or q(k) = tmp2_high - tmp1_high  (Q15)
    					; ARP=AR0, AR0->FR6, AR2->ibeta_mras, AR3->K1  
;----------------------------------------------------------------------------------
      	LACC	*,AR3	; ACC = q(k) (Q15)
      					; ARP=AR0, AR0->FR6, AR2->ibeta_mras, AR3->K1, ARP=AR3 
;----------------------------------------------------------------------------------
		SBRK	#3		; ARP=AR3, AR0->FR6, AR2->ibeta_mras, AR3->q  
;----------------------------------------------------------------------------------
		ADD		*		; ACC = q(k)+q(k-1)	 (Q15)
						; ARP=AR3, AR0->FR6, AR2->ibeta_mras, AR3->q  
;----------------------------------------------------------------------------------
       	SFR				; ACC = (q(k)+q(k-1))/2	 (Q15)
						; ARP=AR3, AR0->FR6, AR2->ibeta_mras, AR3->q  
;----------------------------------------------------------------------------------
       	SACL	*,AR2	; q(k) = (q(k)+q(k-1))/2 (Q15)
       					; ARP=AR3, AR0->FR6, AR2->ibeta_mras, AR3->q, ARP=AR2 
;----------------------------------------------------------------------------------
		LACC	*		; ACC = ibeta_mras  (Q15)
						; ARP=AR2, AR0->FR6, AR2->ibeta_mras, AR3->q		
;----------------------------------------------------------------------------------
      	ADRK	#2		; ARP=AR2, AR0->FR6, AR2->ibeta_old, AR3->q
;----------------------------------------------------------------------------------
       	SACL	* 		; ibeta_old = ibeta_mras  (Q15)
       					; ARP=AR2, AR0->FR6, AR2->ibeta_old, AR3->q
;----------------------------------------------------------------------------------
      	SBRK	#3		; ARP=AR2, AR0->FR6, AR2->ialfa_mras, AR3->q
;----------------------------------------------------------------------------------
		LACC	*		; ACC = ialfa_mras  (Q15)
						; ARP=AR2, AR0->FR6, AR2->ialfa_mras, AR3->q
;----------------------------------------------------------------------------------
		ADRK	#2		; ARP=AR2, AR0->FR6, AR2->ialfa_old, AR3->q
;----------------------------------------------------------------------------------
		SACL	*		; ialfa_old = ialfa_mras  (Q15)
						; ARP=AR2, AR0->FR6, AR2->ialfa_old, AR3->q
;----------------------------------------------------------------------------------
		SBRK	#4		; ARP=AR2, AR0->FR6, AR2->ualfa_mras, AR3->q 	
;----------------------------------------------------------------------------------
      	MAR		*,AR0	; ARP=AR0, AR0->FR6, AR2->ualfa_mras, AR3->q 
;----------------------------------------------------------------------------------
       	SBRK	#6		; ARP=AR0, AR0->FR0, AR2->ualfa_mras, AR3->q  
;----------------------------------------------------------------------------------
       	MAR		*,AR3	; ARP=AR3, AR0->FR0, AR2->ualfa_mras, AR3->q
;----------------------------------------------------------------------------------
		ADRK	#3		; ARP=AR3, AR0->FR0, AR2->ualfa_mras, AR3->K1
;----------------------------------------------------------------------------------
		MAR		*,AR2	; ARP=AR2, AR0->FR0, AR2->ualfa_mras, AR3->K1 
;----------------------------------------------------------------------------------
        .endif          ; ARP=AR2, AR0->FR0, AR2->ualfa_mras, AR3->K1 
;----------------------------------------------------------------------------------
 		.if (adt_model_)       	
ADT_MODEL:
;----------------------------------------------------------------------------------
IM_ALFA                 ; ARP=AR2, AR0->FR0, AR2->ualfa_mras, AR3->K1
;----------------------------------------------------------------------------------
      	MAR		*,AR3	; ARP=AR3, AR0->FR0, AR2->ualfa_mras, AR3->K1 
;----------------------------------------------------------------------------------
		ADRK	#11		; ARP=AR3, AR0->FR0, AR2->ualfa_mras, AR3->wr_hat_mras
;----------------------------------------------------------------------------------
		LT		*		; TREG = wr_hat_mras  (Q15)
						; ARP=AR3, AR0->FR0, AR2->ualfa_mras, AR3->wr_hat_mras
;----------------------------------------------------------------------------------
		MPY		*,AR0	; PREG = wr_hat_mras*wr_hat_mras (Q30)
						; ARP=AR3, AR0->FR0, AR2->ualfa_mras, AR3->wr_hat_mras, ARP=AR0 
;----------------------------------------------------------------------------------
      	PAC				; ACC = wr_hat_mras*wr_hat_mras (Q30)
						; ARP=AR0, AR0->FR0, AR2->ualfa_mras, AR3->wr_hat_mras 
;----------------------------------------------------------------------------------
		SACH	*,1		; FR0 = tmp1_high = wr_hat_mras*wr_hat_mras (Q15)
						; ARP=AR0, AR0->FR0, AR2->ualfa_mras, AR3->wr_hat_mras		
;----------------------------------------------------------------------------------
      	LT		*,AR3	; TREG = wr_hat_mras*wr_hat_mras (Q15)     
      	                ; ARP=AR0, AR0->FR0, AR2->ualfa_mras, AR3->wr_hat_mras, ARP=AR3
;----------------------------------------------------------------------------------
        SBRK	#8		; ARP=AR3, AR0->FR0, AR2->ualfa_mras, AR3->K4      	
;----------------------------------------------------------------------------------
		MPY		*+		; PREG = K4*wr_hat_mras*wr_hat_mras (Q30)
                        ; ARP=AR3, AR0->FR0, AR2->ualfa_mras, AR3->K5
;----------------------------------------------------------------------------------
		PAC				; ACC = K4*wr_hat_mras*wr_hat_mras (Q30)
                        ; ARP=AR3, AR0->FR0, AR2->ualfa_mras, AR3->K5 
;----------------------------------------------------------------------------------
		NEG				; ACC = -K4*wr_hat_mras*wr_hat_mras (Q30)
                        ; ARP=AR3, AR0->FR0, AR2->ualfa_mras, AR3->K5
;----------------------------------------------------------------------------------
		SFL				; ACC = -K4*wr_hat_mras*wr_hat_mras (Q31)
                        ; ARP=AR3, AR0->FR0, AR2->ualfa_mras, AR3->K5
;----------------------------------------------------------------------------------
      	ADDH	*+,AR0	; ACC = K5-K4*wr_hat_mras*wr_hat_mras (Q31)
                        ; ARP=AR3, AR0->FR0, AR2->ualfa_mras, AR3->K6, ARP=AR0
;----------------------------------------------------------------------------------
      	SACH	*+		; FR0 = tmp1_high = K5-K4*wr_hat_mras*wr_hat_mras (Q31)
                        ; ARP=AR0, AR0->FR1, AR2->ualfa_mras, AR3->K6 	
;----------------------------------------------------------------------------------
      	SACL	*+,AR3	; FR1 = tmp1_low = K5-K4*wr_hat_mras*wr_hat_mras (Q31)
                        ; ARP=AR0, AR0->FR2, AR2->ualfa_mras, AR3->K6, ARP=AR3 	
;----------------------------------------------------------------------------------
		LT		*		; TREG = K6  (Q15)
						; ARP=AR3, AR0->FR2, AR2->ualfa_mras, AR3->K6 				
;----------------------------------------------------------------------------------
		ADRK	#6		; ARP=AR3, AR0->FR2, AR2->ualfa_mras, AR3->wr_hat_mras
;----------------------------------------------------------------------------------
		MPY		*,AR0	; PREG = K6*wr_hat_mras (Q30)
						; ARP=AR3, AR0->FR2, AR2->ualfa_mras, AR3->wr_hat_mras, ARP=AR0
;----------------------------------------------------------------------------------
		PAC				; ACC = K6*wr_hat_mras (Q30)
						; ARP=AR0, AR0->FR2, AR2->ualfa_mras, AR3->wr_hat_mras