aci_mras.asm

无刷直流电机的无传感器控制TI程序

; K5-K4*wr_hat*wr_hat = tmp1   =>  Q15-Q15*Q15*Q15 = Q31      
	
	LT		wr_hat_mras		    	; TREG = wr_hat(k)               (Q15)
   	MPY		wr_hat_mras		    	; PREG = wr_hat(k)*wr_hat(k)     (Q30)
   	PAC				    			; ACC  = wr_hat(k)*wr_hat(k)     (Q30)
  	SACH	tmp1_high,1				; tmp1_high = wr_hat(k)*wr_hat(k) (Q15)
  	LT		tmp1_high				; TREG = wr_hat(k)*wr_hat(k)      (Q15) 
   	MPY		K4						; PREG = K4*wr_hat(k)*wr_hat(k)   (Q30) 
   	PAC								; ACC  = K4*wr_hat(k)*wr_hat(k)   (Q30)                                                                      
	NEG								; ACC  = -K4*wr_hat(k)*wr_hat(k)  (Q30)        
  	SFL                     		; rotate to left 1 bit (SXM=1)    (Q31)
  	ADDH	K5              		; ACC = K5-K4*wr_hat(k)*wr_hat(k) (Q31)   
   	SACH	tmp1_high				; tmp1_high = K5-K4*wr_hat(k)*wr_hat(k)  (Q31)
   	SACL	tmp1_low				; tmp1_low = K5-K4*wr_hat(k)*wr_hat(k)   (Q31)     

; wr_hat(k)*K6 = tmp4  =>  Q15*Q15 = Q15  

   	LT		wr_hat_mras		    	; TREG = wr_hat(k)             (Q15)
   	MPY		K6		        		; PREG = K6*wr_hat(k)      	   (Q30)
   	PAC								; ACC  = K6*wr_hat(k)          (Q30)
  	SACH	tmp4,1        			; tmp4 = K6*wr_hat(k) 		   (Q15)

; tmp1*imalfa(k-1) = tmp2   =>   Q31*Q31 = Q31     	   	
; check sign for "imalfa_old" and "tmp1"
      
	LACC	tmp1_high				; ACC = tmp1_high	  
    XOR		imalfa_old_high			; 
    SACL	sign1					; Sign (0=+,1=-) for tmp1*imalfa(k-1)

; check sign for "imbeta_old" and "tmp1"
	
	LACC	tmp1_high				; ACC = tmp1_high	  
   	XOR		imbeta_old_high			;
   	SACL	sign2					; Sign (0=+,1=-) for tmp1*imbeta(k-1)

; check sign for "imalfa_old" and "tmp4"

   	LACC 	tmp4  		    		; ACC = tmp4
   	XOR		imalfa_old_high			; 
   	SACL	sign3					; Sign (0=+,1=-) for tmp4*imalfa(k-1)
  	
; take absolute for "imalfa_old" and "tmp1"

   	LACC	tmp1_high,16  			; ACC high = tmp1_high	
   	ADDS	tmp1_low				; ACC = tmp1_high tmp1_low
   	ABS
   	SACL	tmp1_low				; tmp1_low = ACC low
   	SACH	tmp1_high				; tmp1_high = ACC high
   	LACC	imalfa_old_high,16  	; ACC high = imalfa(k-1)_high	
   	ADDS	imalfa_old_low			; ACC = imalfa(k-1)_high imalfa(k-1)_low
   	ABS
   	SACL	imalfa_old_low			; imalfa(k-1)_low = ACC low
   	SACH	imalfa_old_high			; imalfa(k-1)_high = ACC high
   	   	
; now they're positive.  
 	   	  	   	
   	LT		imalfa_old_low  		; TREG = imalfa(k-1)_low            
   	MPYU	tmp1_low        		; PREG = imalfa(k-1)_low*tmp1_low 	
	SPH		tmp5					; tmp5 = PREG high (save partial result)
  	MPYU	tmp1_high       		; PREG = imalfa(k-1)_low*tmp1_high 	   
	LTP		imalfa_old_high 		; TREG = imalfa(k-1)_high, ACC = PREG
   	MPYU	tmp1_low	    		; PREG = imalfa(k-1)_high*tmp1_low (PREG_old)
   	ADDS	tmp5					; ACC = imalfa(k-1)_low*tmp1_high + tmp5
	MPYA	tmp1_high				; ACC = ACC + PREG_old, PREG_new = imalfa(k-1)_high*tmp1_high
	SACH 	tmp5					; tmp5 = ACC high
	SPLK	#1h,tmp2_low			; tmp2_low = 1 (carry bit)
	BCND	NO_C1,NC				; Carry bit = 0 ?
	B		CHECK1					; Branch to CHECK1 if carry bit = 1
NO_C1
	SPLK	#0h,tmp2_low			; tmp2_low = 0 (no carry bit)
CHECK1
	PAC								; ACC = imalfa(k-1)_high*tmp1_high
	ADDS	tmp5					; ACC = imalfa(k-1)_high*tmp1_high + tmp5
	ADDH	tmp2_low		      	; ACC = imalfa(k-1)_high*tmp1_high + tmp5 + tmp2_low (Carry bit)
   	SACH	tmp2_high,1				; tmp2_high = ACC high   (Q31)
   	SACL	tmp2_low,1				; tmp2_low = ACC low     (Q31)

; check the sign condition
		
	LACC	sign1					; ACC = sign1
	BCND  	DONE1, GT    			; Check sign1 = positive ?
	LACC	tmp2_high,16    		; ACC high = tmp2_high  
   	ADDS	tmp2_low				; ACC  = tmp2_high tmp2_low
	NEG								; Make the result negative
   	SACH	tmp2_high				; tmp2_high = ACC high  
   	SACL	tmp2_low				; tmp2_low = ACC low

DONE1		

; imbeta_old*tmp4 = tmp3   =>   Q31*Q15 = Q31     	   	
; check sign for "imbeta_old" and "tmp4"
        
  	LACC 	tmp4     				; ACC = tmp4
   	XOR		imbeta_old_high			; 	
  	SACL	sign1					; Sign (0=+,1=-) for tmp4*imbeta(k-1)

; take absolute for "imbeta_old" and "tmp4"    	   	

   	LACC	tmp4					; ACC = tmp4
   	ABS								; ACC = |tmp4|
   	SACL	tmp4					; tmp4 = ACC low  	   	       
  	LACC	imbeta_old_high,16  	; ACC high= imbeta(k-1)_high
   	ADDS	imbeta_old_low			; ACC = imbeta(k-1)
   	ABS								; ACC = |imbeta(k-1)|
   	SACL	imbeta_old_low			; imbeta(k-1)_low = ACC low
   	SACH	imbeta_old_high			; imbeta(k-1)_high = ACC high
   	   	
; now they're positive.  
 	   	  	   	
   	LT		tmp4  					; TREG = tmp4            
   	MPYU	imbeta_old_low  		; PREG = imbeta(k-1)_low*tmp4 	   
   	SPH		tmp3_low        		; tmp3_low = PREG high      
   	MPYU 	imbeta_old_high 		; PREG = imbeta(k-1)_high*tmp4
   	PAC								; ACC = imbeta(k-1)_high*tmp4
   	ADDS  	tmp3_low				; ACC = imbeta(k-1)_high*tmp4+tmp3_low
   	SACH	tmp3_high,1				; tmp3_high = ACC high    (Q31)
   	SACL	tmp3_low,1				; tmp3_low = ACC low      (Q31)

; check the sign condition
		
	LACC	sign1					; ACC = sign1
	BCND  	DONE2, GT    			; Check sign1 = positive ?
 	LACC	tmp3_high,16    		; ACC high= tmp3_high
   	ADDS	tmp3_low				; ACC = tmp3_low
	NEG								; Make the result negative
   	SACH	tmp3_high				; tmp3_high = ACC high  
   	SACL	tmp3_low				; tmp3_low = ACC low

DONE2		
		                    	   	   	   	
; tmp2 - tmp3 = tmp3   =>  Q31 - Q31 = Q31

	LACC	tmp2_high,16    		; ACC high = tmp2_high
   	ADDS	tmp2_low				; ACC low = tmp2_low
	SUBS	tmp3_low				; ACC = tmp2_low-tmp3_low
	SUBH 	tmp3_high				; ACC = tmp2_high-tmp3_high
	SACL	tmp3_low				; tmp3_low = tmp2_low-tmp3_low    (L 16-bit)
	SACH	tmp3_high 				; tmp3_high = tmp2_high-tmp3_high (H 16-bit)

; ialfa(k)*K7 = tmp2   =>   Q15*Q15 = Q31
		
	LT		ialfa_mras				; TREG = ialfa(k)             (Q15)
	MPY		K7						; PREG = ialfa(k)*K7		  (Q30)
	PAC								; ACC = ialfa(k)*K7           (Q30)
	SACH	tmp2_high,1				; tmp2_high = ACC high        (Q31)
   	SACL	tmp2_low,1				; tmp2_low = ACC low	      (Q31)  

; imalfa(k) = tmp3 + tmp2    =>   Q31 = Q31 + Q31 

	LACC	tmp3_high,16    		; ACC high = tmp3_high
   	ADDS	tmp3_low				; ACC low = tmp3_low
	ADDS	tmp2_low				; ACC = tmp3_low+tmp2_low
	ADDH 	tmp2_high				; ACC = tmp3_high+tmp2_high
	SACL	imalfa_low				; imalfa_low = tmp3_low+tmp2_low    (L 16-bit)
	SACH	imalfa_high 			; imalfa_high = tmp3_high+tmp2_high (H 16-bit) 

IM_BETA			

; tmp1*imbeta(k-1) = tmp2   =>   Q31*Q31 = Q31     	   	
; already checked sign for "imbeta_old" and "tmp1" kept in sign2
; already took absolute for "imbeta_old" and "tmp1"

; now they're positive.  
 	   	  	   	
   	LT		imbeta_old_low  		; TREG = imbeta(k-1)_low            
   	MPYU	tmp1_low        		; PREG = imbeta(k-1)_low*tmp1_low 	
	SPH		tmp5					; tmp5 = PREG high  (save partial result)
    MPYU	tmp1_high       		; PREG = imbeta(k-1)_low*tmp1_high 	   
	LTP		imbeta_old_high 		; TREG = imbeta(k-1)_high, ACC = PREG
   	MPYU	tmp1_low	    		; PREG = imbeta(k-1)_high*tmp1_low (PREG_old)
	ADDS	tmp5					; ACC = imbeta(k-1)_low*tmp1_high + tmp5
	MPYA	tmp1_high				; ACC = ACC + PREG_old, PREG_new = imbeta(k-1)_high*tmp1_high
	SACH 	tmp5					; tmp5 = ACC high
	SPLK	#1h,tmp2_low			; tmp2_low = 1 (carry bit)
	BCND	NO_C2,NC				; Carry bit = 0 ?
	B		CHECK2					; Branch to CHECK2 if carry bit = 1
NO_C2
	SPLK	#0h,tmp2_low			; tmp2_low = 0 (no carry bit)
CHECK2
	PAC								; ACC = imbeta(k-1)_high*tmp1_high
	ADDS	tmp5					; ACC = imbeta(k-1)_high*tmp1_high + tmp5
	ADDH	tmp2_low		      	; ACC = imbeta(k-1)_high*tmp1_high + tmp5 + tmp2_low (Carry bit)
   	SACH	tmp2_high,1				; tmp2_high = ACC high   (Q31)
   	SACL	tmp2_low,1				; tmp2_low = ACC low     (Q31)

; check the sign condition
		
	LACC	sign2					; ACC = sign2
	BCND  	DONE3, GT    			; Check sign2 = positive ?
	LACC	tmp2_high,16    		; ACC high = tmp2_high  
    ADDS	tmp2_low				; ACC  = tmp2_high tmp2_low
	NEG								; Make the result negative
   	SACH	tmp2_high				; tmp2_high = ACC high  
	SACL	tmp2_low				; tmp2_low = ACC low

DONE3		
		                    	   	   	   	
; imalfa_old*tmp4 = tmp3  =>  Q31*Q15 = Q31     	   	
; already checked sign for "imalfa_old" and "tmp4" kept in sign3
; already took absolute for "imalfa_old" and "tmp4"
                
; now they're positive.  
 	   	  	   	
 	LT		tmp4  					; TREG = tmp4            
   	MPYU	imalfa_old_low   		; PREG = imalfa(k-1)_low*tmp4 	   
   	SPH		tmp3_low        		; tmp3_low = PREG high      
   	MPYU 	imalfa_old_high			; PREG = imalfa(k-1)_high*tmp4
   	PAC								; ACC = imalfa(k-1)_high*tmp4
   	ADDS  	tmp3_low				; ACC = imalfa(k-1)_high*tmp4+tmp3_low
   	SACH	tmp3_high,1				; tmp3_high = ACC high     (Q31)
   	SACL	tmp3_low,1				; tmp3_low = ACC low       (Q31)

; check the sign condition
		
	LACC	sign3					; ACC = sign3
	BCND  	DONE4, GT    			; Check sign3 = positive ?
	LACC	tmp3_high,16    		; ACC high= tmp3_high
   	ADDS	tmp3_low				; ACC = tmp3_low
	NEG								; Make the result negative
   	SACH	tmp3_high				; tmp3_high = ACC high  
   	SACL	tmp3_low				; tmp3_low = ACC low
DONE4		

; tmp2 + tmp3 = tmp3  => Q31 + Q31 = Q31

	LACC	tmp3_high,16    		; ACC high = tmp3_high
   	ADDS	tmp3_low				; ACC low = tmp3_low
	ADDS	tmp2_low				; ACC = tmp3_low+tmp2_low
	ADDH 	tmp2_high				; ACC = tmp3_high+tmp2_high
	SACL	tmp3_low				; tmp3_low = tmp3_low+tmp2_low    (L 16-bit)
	SACH	tmp3_high 				; tmp3_high = tmp3_high+tmp2_high (H 16-bit)

; ibeta(k)*K7 = tmp2  =>  Q15*Q15 = Q31
		
	LT		ibeta_mras				; TREG = ibeta(k)          	  (Q15)
	MPY		K7						; PREG = ibeta(k)*K7		  (Q30)		
	PAC								; ACC = ialfa(k)*K7           (Q30)
	SACH	tmp2_high,1				; tmp2_high = ACC high        (Q31) 
   	SACL	tmp2_low,1				; tmp2_low = ACC low	      (Q31)

; imbeta(k) = tmp3 + tmp2    =>   Q31 = Q31 + Q31 

	LACC	tmp3_high,16    		; ACC high = tmp3_high
   	ADDS	tmp3_low				; ACC low = tmp3_low
	ADDS	tmp2_low				; ACC = tmp3_low+tmp2_low
	ADDH 	tmp2_high				; ACC = tmp3_high+tmp2_high
	SACL	imbeta_low				; imbeta_low = tmp3_low+tmp2_low    (L 16-bit)
	SACH	imbeta_high 			; imbeta_high = tmp3_high+tmp2_high (H 16-bit)