aci_se.asm

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

;----------------------------------------------------------------------------------
		RPT		#15		; Repeat SUBC 16 times
						; ARP=AR0, AR0->FR1, AR2->theta_r_se  
;----------------------------------------------------------------------------------
       	SUBC	*		; Dividing K1_se*(psi_dr_se*i_qs_se-psi_qr_se*i_ds_se)/psi_r_2
       					; ARP=AR0, AR0->FR1, AR2->theta_r_se 
;----------------------------------------------------------------------------------
		SACL	*-,AR2	; FR1 = w_sl = K1_se*(psi_dr_se*i_qs_se-psi_qr_se*i_ds_se)/psi_r_2 (Q15)
						; ARP=AR0, AR0->FR0, AR2->theta_r_se, ARP=AR2 
;----------------------------------------------------------------------------------
     	LACC	#DIFF_MAX_LIMIT	; ACC = DIFF_MAX_LIMIT  (Q15)
     					; ARP=AR2, AR0->FR0, AR2->theta_r_se  
;----------------------------------------------------------------------------------
       	SUB		*		; ACC = DIFF_MAX_LIMIT - theta_r_se (Q15)
     					; ARP=AR2, AR0->FR0, AR2->theta_r_se
;----------------------------------------------------------------------------------
       	BCND	LESS_MAX,GT ; Branch to LESS_MAX if DIFF_MAX_LIMIT > theta_r_se
       					; ARP=AR2, AR0->FR0, AR2->theta_r_se 
;----------------------------------------------------------------------------------
      	B		SKIP_DIFF  ; ARP=AR2, AR0->FR1, AR2->theta_r_se
;----------------------------------------------------------------------------------
LESS_MAX				; ARP=AR2, AR0->FR0, AR2->theta_r_se
;----------------------------------------------------------------------------------
		LACC	#DIFF_MIN_LIMIT	; ACC = DIFF_MIN_LIMIT  (Q15)
     					; ARP=AR2, AR0->FR0, AR2->theta_r_se 
;----------------------------------------------------------------------------------
       	SUB		*		; ACC = DIFF_MIN_LIMIT - theta_r_se (Q15)
     					; ARP=AR2, AR0->FR0, AR2->theta_r_se
;----------------------------------------------------------------------------------
       	BCND	MORE_MIN,LT ; Branch to MORE_MIN if DIFF_MIN_LIMIT < theta_r_se
       					; ARP=AR2, AR0->FR0, AR2->theta_r_se 
;----------------------------------------------------------------------------------
     	B		SKIP_DIFF  ; ARP=AR2, AR0->FR0, AR2->theta_r_se
;----------------------------------------------------------------------------------
MORE_MIN				; ARP=AR2, AR0->FR0, AR2->theta_r_se
;----------------------------------------------------------------------------------
      	LT		*+		; TREG = theta_r_se  (Q15)
      					; ARP=AR2, AR0->FR0, AR2->K2_se 
;----------------------------------------------------------------------------------
		MPY		*+		; PREG = K2_se*theta_r_se  (Q21)
      					; ARP=AR2, AR0->FR0, AR2->theta_r_old
;----------------------------------------------------------------------------------
		PAC				; ACC = K2_se*theta_r_se  (Q21)
      					; ARP=AR2, AR0->FR0, AR2->theta_r_old
;----------------------------------------------------------------------------------
      	LT		*-		; TREG = theta_r_old  (Q15)
      					; ARP=AR2, AR0->FR0, AR2->K2_se
;----------------------------------------------------------------------------------
     	MPY		*,AR0	; PREG = K2_se*theta_r_old  (Q21)
      					; ARP=AR2, AR0->FR0, AR2->K2_se, ARP=AR0
;----------------------------------------------------------------------------------
     	SPAC			; ACC = K2_se*theta_r_se-K2_se*theta_r_old  (Q21)
      					; ARP=AR0, AR0->FR0, AR2->K2_se
;----------------------------------------------------------------------------------
		SFL				; ACC = K2_se*theta_r_se-K2_se*theta_r_old  (Q22)
      					; ARP=AR0, AR0->FR0, AR2->K2_se
;----------------------------------------------------------------------------------
		SFL				; ACC = K2_se*theta_r_se-K2_se*theta_r_old  (Q23)
      					; ARP=AR0, AR0->FR0, AR2->K2_se
;----------------------------------------------------------------------------------
		SFL				; ACC = K2_se*theta_r_se-K2_se*theta_r_old  (Q24)
      					; ARP=AR0, AR0->FR0, AR2->K2_se
;----------------------------------------------------------------------------------
     	SACH	*,7,AR2	; FR0 = tmp1_se = K2_se*theta_r_se-K2_se*theta_r_old  (Q15)
      					; ARP=AR0, AR0->FR0, AR2->K2_se, ARP=AR2
;----------------------------------------------------------------------------------
      	ADRK	#2		; ARP=AR2, AR0->FR0, AR2->K3_se 		
;----------------------------------------------------------------------------------
		LT		*+		; TREG = K3_se  (Q15)
						; ARP=AR2, AR0->FR0, AR2->w_psi_r 
;----------------------------------------------------------------------------------
		MPY		*+		; PREG = K3_se*w_psi_r  (Q30)
						; ARP=AR2, AR0->FR0, AR2->K4_se
;----------------------------------------------------------------------------------
       	PAC				; ACC = K3_se*w_psi_r  (Q30)
						; ARP=AR2, AR0->FR0, AR2->K4_se
;----------------------------------------------------------------------------------
		LT		*-,AR0	; TREG = K4_se  (Q15)
						; ARP=AR2, AR0->FR0, AR2->w_psi_r, ARP=AR0 
;----------------------------------------------------------------------------------
		MPY		*+,AR2	; PREG = K4_se*tmp1_se  (Q30)
						; ARP=AR0, AR0->FR1, AR2->w_psi_r, ARP=AR2  		
;----------------------------------------------------------------------------------
		APAC			; ACC = K3_se*w_psi_r + K4_se*tmp1_se (Q30)
						; ARP=AR2, AR0->FR1, AR2->w_psi_r
;----------------------------------------------------------------------------------
		SACH	*,1		; w_psi_r = K3_se*w_psi_r + K4_se*tmp1_se (Q15)
						; ARP=AR2, AR0->FR1, AR2->w_psi_r
;----------------------------------------------------------------------------------
     	SBRK	#4		; ARP=AR2, AR0->FR1, AR2->theta_r_se
;----------------------------------------------------------------------------------
SKIP_DIFF  				; ARP=AR2, AR0->FR1, AR2->theta_r_se
;----------------------------------------------------------------------------------
     	LACC	*		; ACC = theta_r_se  (Q15)
     					; ARP=AR2, AR0->FR1, AR2->theta_r_se
;----------------------------------------------------------------------------------
      	ADRK	#2		; ARP=AR2, AR0->FR1, AR2->theta_r_old
;----------------------------------------------------------------------------------
     	SACL	*		; theta_r_old = theta_r_se  (Q15)
     					; ARP=AR2, AR0->FR1, AR2->theta_r_old
;----------------------------------------------------------------------------------
     	ADRK	#2		; ARP=AR2, AR0->FR1, AR2->w_psi_r
;----------------------------------------------------------------------------------
		LACC	*,AR0	; ACC = w_psi_r   (Q15)
						; ARP=AR2, AR0->FR1, AR2->w_psi_r, ARP=AR0
;----------------------------------------------------------------------------------
     	SUB		*,AR2	; ACC = w_psi_r - w_sl (FR1)  (Q15)
						; ARP=AR0, AR0->FR1, AR2->w_psi_r, ARP=AR2
;----------------------------------------------------------------------------------
		ADRK	#2		; ARP=AR2, AR0->FR1, AR2->wr_hat_se
;----------------------------------------------------------------------------------
		SACL	*		; wr_hat_se = w_psi_r - w_sl (FR1)  (Q15)
						; ARP=AR2, AR0->FR1, AR2->wr_hat_se
;----------------------------------------------------------------------------------
  		LT		*+		; TREG = wr_hat_se  (Q15)
  						; ARP=AR2, AR0->FR1, AR2->base_rpm_se
;----------------------------------------------------------------------------------
      	MPY		*+		; PREG = wr_hat_se*base_rpm_se  (Q15)
  						; ARP=AR2, AR0->FR1, AR2->wr_hat_rpm_se
;----------------------------------------------------------------------------------
    	PAC				; ACC = wr_hat_se*base_rpm_se  (Q15)
  						; ARP=AR2, AR0->FR1, AR2->wr_hat_rpm_se
;----------------------------------------------------------------------------------
    	SACH	*,1,AR1	; wr_hat_rpm_se = wr_hat_se*base_rpm_se  (Q0)
  						; ARP=AR2, AR0->FR1, AR2->wr_hat_rpm_se, ARP=AR1
;----------------------------------------------------------------------------------
_aci_se_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 	#(__aci_se_calc_framesize+1)
        LAR  	AR0,*-
        PSHD	*
        
        RET