pid_reg3.asm

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

      					; ARP=AR2, AR0->FR0, AR2->pid_out_reg3 
;----------------------------------------------------------------------------------
       	B		UPDATE_REG3 ; ARP=AR2, AR0->FR0, AR2->pid_out_reg3  
;----------------------------------------------------------------------------------
SAT_MAX_REG3           	; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
      	ADRK	#1		; ARP=AR2, AR0->FR0, AR2->pid_out_max
;----------------------------------------------------------------------------------
      	LACC	*		; ACC = pid_out_max   (Q15)
      					; ARP=AR2, AR0->FR0, AR2->pid_out_max
;----------------------------------------------------------------------------------
     	ADRK	#2		; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
    	SACL	*		; pid_out_reg3 = pid_out_max   (Q15)
      					; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
       	B		UPDATE_REG3 ; ARP=AR2, AR0->FR0, AR2->pid_out_reg3  
;----------------------------------------------------------------------------------
SAT_MIN_REG3           	; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
      	ADRK	#2		; ARP=AR2, AR0->FR0, AR2->pid_out_min
;----------------------------------------------------------------------------------
      	LACC	*+		; ACC = pid_out_min   (Q15)
      					; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
    	SACL	*		; pid_out_reg3 = pid_out_min   (Q15)
      					; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
UPDATE_REG3 			; ARP=AR2, AR0->FR0, AR2->pid_out_reg3  
;----------------------------------------------------------------------------------
      	LACC	*,15	; ACC = pid_out_reg3   (Q30)
      					; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
      	SBRK	#3		; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
      	SUB		*,16	; ACC = pid_out_reg3 - uprsat_reg3  (Q30)
      					; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
       	ADRK	#4		; ARP=AR2, AR0->FR0, AR2->saterr_reg3
;----------------------------------------------------------------------------------
      	SACH	*+		; saterr_reg3 = pid_out_reg3 - uprsat_reg3  (Q14)
      					; ARP=AR2, AR0->FR0, AR2->Ki_reg3
;----------------------------------------------------------------------------------
      	SPM		3		; Set right shifted 6 bit 
      					; ARP=AR2, AR0->FR0, AR2->Ki_reg3
;----------------------------------------------------------------------------------
    	LT		*		; TREG = Ki   (Q31-16 bit)
    					; ARP=AR2, AR0->FR0, AR2->Ki_reg3 		 
;----------------------------------------------------------------------------------
     	SBRK	#10		; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
     	MPY		*		; PREG = Ki*up   (Q38)
     					; ARP=AR2, AR0->FR0, AR2->up_reg3	 
;----------------------------------------------------------------------------------
       	PAC				; ACC = Ki*up   (Q32)
     					; ARP=AR2, AR0->FR0, AR2->up_reg3	 
;----------------------------------------------------------------------------------
     	SFR				; ACC = Ki*up   (Q31)
     					; ARP=AR2, AR0->FR0, AR2->up_reg3	 
;----------------------------------------------------------------------------------
     	SFR				; ACC = Ki*up   (Q30)
     					; ARP=AR2, AR0->FR0, AR2->up_reg3	 
;----------------------------------------------------------------------------------
     	SPM		1		; Set left shifted 1 bit 
      					; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
     	ADRK	#11		; ARP=AR2, AR0->FR0, AR2->Kc_reg3
;----------------------------------------------------------------------------------
     	LT		*		; TREG = Kc  (Q15)
     					; ARP=AR2, AR0->FR0, AR2->Kc_reg3		 
;----------------------------------------------------------------------------------
      	SBRK	#2		; ARP=AR2, AR0->FR0, AR2->saterr_reg3
;----------------------------------------------------------------------------------
     	MPY		*		; PREG = Kc*saterr_reg3   (Q29)
     					; ARP=AR2, AR0->FR0, AR2->saterr_reg3	 
;----------------------------------------------------------------------------------
      	APAC			; ACC = Ki*up + Kc*(pid_out_reg3-uprsat_reg3)  (Q30)
     					; ARP=AR2, AR0->FR0, AR2->saterr_reg3
;----------------------------------------------------------------------------------
     	SBRK	#7		; ARP=AR2, AR0->FR0, AR2->ui_lo_reg3		 
;----------------------------------------------------------------------------------
    	ADDS	*-		; ACC = ui + Ki*up + Kc*(pid_out_reg3-uprsat_reg3)  (Q30)
     					; ARP=AR2, AR0->FR0, AR2->ui_hi_reg3
;----------------------------------------------------------------------------------
    	ADDH	*+		; ACC = ui + Ki*up + Kc*(pid_out_reg3-uprsat_reg3)  (Q30)
     					; ARP=AR2, AR0->FR0, AR2->ui_lo_reg3	 
;----------------------------------------------------------------------------------
    	SACL	*-		; ui = ui + Ki*up + Kc*(pid_out_reg3-uprsat_reg3)  (Q30)
     					; ARP=AR2, AR0->FR0, AR2->ui_hi_reg3  	
;----------------------------------------------------------------------------------
     	SACH	*		; ui = ui + Ki*up + Kc*(pid_out_reg3-uprsat_reg3)  (Q30)
     					; ARP=AR2, AR0->FR0, AR2->ui_hi_reg3  		 
;----------------------------------------------------------------------------------
    	ADRK	#11		; ARP=AR2, AR0->FR0, AR2->Kd_reg3	
;----------------------------------------------------------------------------------
     	LT		*		; TREG = Kd   (Q14)
     					; ARP=AR2, AR0->FR0, AR2->Kd_reg3	 
;----------------------------------------------------------------------------------
       	SBRK	#12		; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
   		MPY		*		; PREG = Kd*up  (Q28)
   						; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
     	PAC				; ACC = Kd*up  (Q29)
   						; ARP=AR2, AR0->FR0, AR2->up_reg3	 
;----------------------------------------------------------------------------------
   		ADRK	#13		; ARP=AR2, AR0->FR0, AR2->up1_reg3
;----------------------------------------------------------------------------------
    	MPY		*		; PREG = Kd*up1  (Q28)
   						; ARP=AR2, AR0->FR0, AR2->up1_reg3	
;----------------------------------------------------------------------------------
     	SPAC	        ; ACC = Kd*up - Kd*up1  (Q29)
   						; ARP=AR2, AR0->FR0, AR2->up1_reg3	
;----------------------------------------------------------------------------------
     	SBRK	#10		; ARP=AR2, AR0->FR0, AR2->ud_lo_reg3	
;----------------------------------------------------------------------------------
    	SACL	*+,1	; ud_lo_reg3 = Kd*up - Kd*up1  (Q30)
   						; ARP=AR2, AR0->FR0, AR2->ud_hi_reg3	
;----------------------------------------------------------------------------------
    	SACH	*,1		; ud_hi_reg3 = Kd*up - Kd*up1  (Q30)
   						; ARP=AR2, AR0->FR0, AR2->ud_hi_reg3	
;----------------------------------------------------------------------------------
		SBRK	#4		; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
    	LACC	*		; ACC = up_reg3  (Q14)
    					; ARP=AR2, AR0->FR0, AR2->up_reg3	
;----------------------------------------------------------------------------------
     	ADRK	#13		; ARP=AR2, AR0->FR0, AR2->up1_reg3 	 
;----------------------------------------------------------------------------------
       	SACL	*,AR1	; up1_reg3 = up_reg3  (Q14)
    					; ARP=AR2, AR0->FR0, AR2->up1_reg3, ARP=AR1	
;----------------------------------------------------------------------------------
_pid_reg3_calc_exit:
        ;;	MAR     *,AR1   ; can be removed if this condition is met on
       	                ; every path to this code. (i.e., ARP=AR1 here)

		SPM     0       
        CLRC	OVM
        CLRC	SXM

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