f2407qep2.asm

TI 的DSP2407A的无速度传感器永磁同步电机FOC控制程序

;=====================================================================================================================
; File name:       F2407QEP2.ASM                     
;                    
; Originator:	Digital Control Systems Group
;			Texas Instruments
; Description:                                   
; This file contains source for the QEP theta calculation function.
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 09-15-2000	Release	Rev 1.00
; 04-04-2001	Release	Rev 1.10
; 02-15-2002	Release	Rev 1.20  ; change left shift from 14 to 2 for dir_QEP 
;================================================================================
; Applicability: F240x.  (Peripheral dependant).
;
;================================================================================
; Routine Name: F2407_qep_theta_calc                    Routine Type: C Callable
;
; Description:
;  
;  C prototype : int F2407_qep_theta_calc(struct QEP *p)
;================================================================================
                
; Definition of the QEP structure is as follows:
;
;typedef struct {int dir_QEP;        /* Output: Motor rotation direction (Q0)  */
;                int cal_angle;      /* Parameter: Encoder offset calibration (Q0) */
;                int theta_raw;      /* Variable: Raw angle of timer value (Q0)  */
;                int mech_scale;     /* Parameter: Scaling factor for raw angle Q0->Q15 */
;                int theta_mech;     /* Output: Motor Mechanical Angle (Q15)  */
;                int polepairs;      /* Parameter: No. of poles/2 of the motor (Q0) */
;                int theta_elec;     /* Output: Motor Electrical Angle (Q15)  */
;                int QEP_cnt_idx;    /* Variable: Pulses count on encoder at index (Q0) */
;                int index_sync_flg; /* Output: Index sync status (Q0) */
;                int (*calc)();      /* Pointer to the calc funtion           */
;                int (*init)();      /* Pointer to the init funcion           */
;                int (*indexevent)(); /* Pointer to index event handler       */
;                }  QEP ;
;
;================================================================================
                  .include ..\include\x240x.h
;================================================================================
                  .def        _F2407_EV1_qep_theta_calc
;================================================================================
__F2407_EV1_qep_theta_calc_framesize .set 0001h
;================================================================================
_F2407_EV1_qep_theta_calc:

     	POPD	*+									       ; Keep return address
       	SAR  	AR0,*+								       ; Keep old frame pointer (FP)
       	SAR   	AR1,*								       ; Keep old stack pointer (SP)
       	LARK  	AR0,__F2407_EV1_qep_theta_calc_framesize   ; Load AR0 with frame size	
       	LAR   	AR0,*0+,AR0							       ; AR0->FP0 (new FP), ARP=AR0

;================================================================================
        SBRK    #3              ; ARP=AR0, AR0->FR0-3 (1st argument)
;--------------------------------------------------------------------------------
        LAR     AR2,*           ; ARP=AR0, AR0->dir_QEP, AR2->dir_QEP		
;--------------------------------------------------------------------------------
        ADRK    #3              ; Move AR0 back to FR0
                                ; ARP=AR0, AR0->FR0, AR2->dir_QEP
;--------------------------------------------------------------------------------
       	SETC 	SXM				; Turn sign extension mode on
       							; ARP=AR0, AR0->FR0, AR2->dir_QEP
;--------------------------------------------------------------------------------
        MAR     *,AR3           ; ARP=AR0, AR0->FR0, AR2->dir_QEP, ARP=AR3
;--------------------------------------------------------------------------------
        LAR     AR3,#GPTCONB    ; Point AR3 to GPTCON
                                ; ARP=AR3, AR0->FR0 AR2->dir_QEP, AR3->GPTCON
;--------------------------------------------------------------------------------
		LACC	*,2				; ACC = (GPTCON)*2^2
								; ARP=AR3, AR0->FR0 AR2->dir_QEP, AR3->GPTCON	
;--------------------------------------------------------------------------------
		AND		#1,16			; ACC = (GPTCON)*2^2 and 1
		                        ; ARP=AR3, AR0->FR0 AR2->dir_QEP, AR3->GPTCON	
;--------------------------------------------------------------------------------
       	MAR		*,AR2			; ARP=AR3, AR0->FR0, AR2->dir_QEP, AR3->GPTCON, ARP=AR2 
;--------------------------------------------------------------------------------
       	SACH	*+,AR3			; QEP_dir = ACC high
       							; ARP=AR2, AR0->FR0, AR2->cal_angle, AR3->GPTCON, ARP=AR3
;--------------------------------------------------------------------------------
        LAR     AR3,#T4CNT      ; ARP=AR3, AR0->FR0 AR2->cal_angle, AR3->T4CNT
;--------------------------------------------------------------------------------
       	LACC	*,AR2			; LACC = T4CNT  (Q0)
       							; ARP=AR3, AR0->FR0 AR2->cal_angle, AR3->T4CNT, ARP=AR2
;--------------------------------------------------------------------------------
       	ADD		*+				; ACC = T4CNT + cal_angle  (Q0)
       							; ARP=AR2, AR0->FR0 AR2->theta_raw, AR3->T4CNT
;--------------------------------------------------------------------------------
      	SACL	*				; theta_raw = T4CNT + cal_angle  (Q0)
      	                        ; ARP=AR2, AR0->FR0 AR2->theta_raw, AR3->T4CNT
;--------------------------------------------------------------------------------
        LT		*+				; TREG = theta_raw  (Q0)
        						; ARP=AR2, AR0->FR0 AR2->mech_scale, AR3->T4CNT
;--------------------------------------------------------------------------------
      	MPY		*+				; PREG = theta_raw*mech_scale  (Q26)
        						; ARP=AR2, AR0->FR0 AR2->theta_mech, AR3->T4CNT
;--------------------------------------------------------------------------------
       	PAC						; ACC = theta_raw*mech_scale  (Q26)
        						; ARP=AR2, AR0->FR0 AR2->theta_mech, AR3->T4CNT
;--------------------------------------------------------------------------------
     	AND		#7FFFh,11		; Modulo 360 degree in Q26
     						    ; ARP=AR2, AR0->FR0 AR2->theta_mech, AR3->T4CNT
;--------------------------------------------------------------------------------
      	SACH	*,5				; theta_mech = theta_raw*mech_scale  (Q15)
      	                        ; ARP=AR2, AR0->FR0 AR2->theta_mech, AR3->T4CNT
;--------------------------------------------------------------------------------
      	LT		*+				; TREG = theta_mech  (Q15)
      							; ARP=AR2, AR0->FR0 AR2->polepairs, AR3->T4CNT
;--------------------------------------------------------------------------------
       	MPY		*+				; PREG = theta_mech*polepairs  (Q15)
       							; ARP=AR2, AR0->FR0 AR2->theta_elec, AR3->T4CNT
;--------------------------------------------------------------------------------
      	PAC						; ACC = theta_mech*polepairs  (Q15)
       							; ARP=AR2, AR0->FR0 AR2->theta_elec, AR3->T4CNT
;--------------------------------------------------------------------------------
     	AND		#7FFFh			; Modulo 360 degree in Q15
     						    ; ARP=AR2, AR0->FR0 AR2->theta_elec, AR3->T4CNT
;--------------------------------------------------------------------------------
        SACL	*,AR1			; theta_elec = theta_mech*polepairs  (Q15)
                                ; ARP=AR2, AR0->FR0 AR2->theta_elec, AR3->T4CNT, ARP=AR1
;--------------------------------------------------------------------------------
_F2407_EV1_qep_theta_calc_end:

      ; MAR     *,AR1   ; can be removed if this condition is met on
      ;                 ; every path to this code. (i.e., ARP=AR1 here)

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

        RET