atan_div.asm

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

;=====================================================================
; Name:          ATAN_DIV.ASM
; Module name:   atan_div(int x, int y)
; Originator:    Digital Control System Group
;		     Texas Instruments
; Description:   This module implements a C-callable assembly routine
;		     for ArcTan function
;=====================================================================
; History:
;------------------------------------------------------------
; 09/15/00        Released as Revision 1.0
;                                 
;======================================================================
                .include "atan_div.inc"   ; atan lookup table
						 	; atan macro

;--------------------------------------------------------------------
; int atan_div(int x, int y);
;--------------------------------------------------------------------
; Function:      inverse tanget of the divident (x/y) 
; 									
; Arguments:     farctional Q15 sine, fractional Q15 cosine
;                min: -1.0   <--> 8000h
;                max: 0.9999 <--> 7FFFh
; 									
; Return value:  fractional Q15 angle (-1 .. 0,9999)
;                scaling factor: PI  
;
; Error:         < 3 LSB  
;--------------------------------------------------------------------
                .def    _atan_div
                .text
_atan_div:

        ;context save
        ;------------
                popd    *+              ;push return address
                sar     AR0, *+         ;push old frame pointer
                sar     AR1, *          
                lar     AR0,*           ;init new frame pointer  
                adrk    #3              ;alocate space for three local variables

        ;init pointer to (most left) argument
                mar     *,AR2
                lar     AR2,#-3      
                mar     *0+             ;AR2 = &parameter1

        ;store absolute values to local #1,#2
                setc    sxm
                lacc    *               ;ACC = sin
                abs                     ;ACC = abs(sin)
                adrk    #3              ;AR2 = local #1
                sacl    *               ;local #1 = abs(sin)
                sbrk    #4              ;AR2 = 2nd parameter
                lacc    *               ;ACC = cos
                abs                     ;ACC = abs(cos)
                adrk    #5              ;AR2 = local #2
                sacl    *-              ;local #2 = abs(cos), AR2 = abs(sin)

       ;calculate phase within 1st quadrant
                sub     *               ;ACC = abs(cos) - abs(sin), AR2 -> sin 
                bcnd    DEG_0_45, GT    
                bcnd    DEG_45_90, LT

DEG_45:         lacc    #2000h
                b       GET_QUADRANT

DEG_0_45:       ;Calc atan(abs(sin)/abs(cos)), abs(cos)>abs(sin)
                lacc    *+,16           ;ACC = abs(sin), AR2 -> abs(cos)
                rpt     #13
                subc    *               
                subc    *-              ;AR2 = local #1
                ATAN                    ;on entry: ACC = argument
                                        ;          AR2 --> local #1
                                        ;on exit:  return = ACC
                                        ;          AR2 --> local #1
                b       GET_QUADRANT

DEG_45_90:      ;Calc atan(abs(cos)/abs(sin)), abs(sin)>abs(cos)
                                        ;or actan(abs(sin)/abs(sin))
                mar     *+              ;AR2 -> abs(cos)
                lacc    *-,16           ;ACC = abs(cos), AR2 -> abs(sin) 
                rpt     #14
                subc    *
                ATAN                    ;on entry: ACC = argument
                                        ;          AR2 --> local #1
                                        ;on exit:  return = ACC
                                        ;          AR2 --> local #1
                neg
                add     #4000h          ;atan = pi/2 - actan
                b       GET_QUADRANT


GET_QUADRANT:   sacl    *               ;local #1 = phase(1Q)
                sbrk    #3              ;AR2 -> sin
                lacc    *-              ;ACC = sin, AR2 -> cos
                bcnd    Q34,LT

Q12:            lacc    *               ;ACC = cos
                adrk    #4              ;AR2 -> phase(1Q)

                bcnd    Q2,LT
Q1:             lacc    *               ;ACC = phase(1Q)
                b       epio
Q2:             lacc    #7fffH          
                sub     *               ;ACC = pi - phase(1Q)
                b       epio

Q34:            lacc    *               ;ACC = cos
                adrk    #4              ;AR2 -> phase(1Q)
                bcnd    Q4,GT
Q3:             lacc    *
                add     #8000h          ;ACC = phase(1Q) - pi
                b       epio
Q4:             lacc    *
                neg                     ;ACC = -phase(1Q)
                b       epio

        ;context restore                
        ;---------------
epio:           mar     *,AR1
                sbrk    #(3+1)          ;pop local var's+1 from stack
                lar     ar0, *-         ;restore old frame pointer
                pshd    *               ;restore return address
                ret