park.asm

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

;=====================================================================================
; File name:        PARK.ASM                     
;                    
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:                                 
; This file contains source for the PARK transformation.
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx.  (Peripheral Independant).
; Description:        
;                (d,q,theta) -> (D,Q) Transformation
;                park_D =    Park_d*cos(theta) + Park_q*sin(theta)
;                park_Q =  - Park_d*sin(theta) + Park_q*cos(theta)
;
; Precision:    Defines precision of the sine calculation.

High_precision  .set    0

;
;================================================================================
; Routine Name: Generic function.        Routine Type: C Callable
;
; Description:
;  
;  C prototype : int park(void *in, void *out)
; The elements in  *in should be in the order d,q,theta.
; The elements in *out will be placed in the order D,Q.
;================================================================================
                .def        _park

                .ref    SINTAB_360


;================================================================================

__park_framesize .set 0002h
;================================================================================
_park:
                     POPD        *+
                SAR        AR0,*+
                SAR        AR1,*
                LARK        AR0,__park_framesize
                LAR        AR0,*0+,AR0

;================================================================================
                SBRK        #3        ; Point AR0 to the first argument.
;--------------------------------------------------------------------------------
                LAR        AR2,*-        ; get the argument #1 in AR2. 
                                ; ARP=AR0, AR2->i/p s, AR0->arg #2. 
;--------------------------------------------------------------------------------
                LAR        AR3,*+        ; get the argument #2 in AR3. 
                                ; ARP=AR0, AR0->arg #2, AR2->i/p's, AR3->o/p's.
;--------------------------------------------------------------------------------
                ADRK    #3      ; Restore AR0 to point to FR0.
                                ; ARP=AR0, AR0->FR0, AR2->i/p's, AR3->o/p's.
;--------------------------------------------------------------------------------
                       SPM     1       ; Set SPM for Q15 multiplication
;--------------------------------------------------------------------------------
                SETC        SXM     ; Sign extension mode on
;--------------------------------------------------------------------------------
                MAR     *,AR2   ; ARP=AR2, AR0->FR0, AR2->Park_d AR3->Park_D
;--------------------------------------------------------------------------------
                ADRK    #2      ; Point to theta.
                                ; ARP=AR2, AR0->FR0, AR2->Park_theta AR3->Park_D
;--------------------------------------------------------------------------------
;--- Normal precision option -------
        .if (High_precision != 1)
        
                LACC    *,AR0   ; ACC=theta.
                                ; ARP=AR0, AR0->FR0, AR2->Park_theta AR3->Park_D
;--------------------------------------------------------------------------------
                ADD     #8192   ;add 90 deg, i.e. COS(A)=SIN(A+90)
                                ; ARP=AR0, AR0->FR0, AR2->Park_theta AR3->Park_D
;--------------------------------------------------------------------------------
                AND     #07fffh ; ARP=AR0, AR0->FR0, AR2->Park_theta AR3->Park_D
;--------------------------------------------------------------------------------
                SACL    *       ; Store ACC in FR0.
                                ; ARP=AR0, AR0->FR0, AR2->Park_theta AR3->Park_D
;--------------------------------------------------------------------------------
                LACC    *,9     ; Load with shift to get table pointer in ACCH.
                                ; ARP=AR0, AR0->FR0, AR2->Park_theta AR3->Park_D
;--------------------------------------------------------------------------------
                SACH    *       ; Store table ptr in FR0
                                ; ARP=AR0, AR0->FR0, AR2->Park_theta AR3->Park_D
;--------------------------------------------------------------------------------
                LACL    *       ; Get table pointer in lower acc.
                                ; ARP=AR0, AR0->FR0, AR2->Park_theta AR3->Park_D
;--------------------------------------------------------------------------------
                AND     #255    ; Ensure table pointer is 8 bit.
                                ; ARP=AR0, AR0->FR0, AR2->Park_theta AR3->Park_D
;--------------------------------------------------------------------------------
                ADD     #SINTAB_360
                                ; ARP=AR0, AR0->FR0, AR2->Park_theta AR3->Park_D
;--------------------------------------------------------------------------------
                TBLR    *+,AR2  ; FR0 = COS(theta).
                                ; ARP=AR2, AR0->FR1, AR2->Park_theta AR3->Park_D
;--------------------------------------------------------------------------------
                LACC    *-,9,AR0 ;ACCH= table_ptr (theta >> 9)
                                ; ARP=AR0, AR0->FR1, AR2->Park_q AR3->Park_D
;--------------------------------------------------------------------------------
                SACH    *       ; Store table ptr in FR1
                                ; ARP=AR0, AR0->FR1, AR2->Park_q AR3->Park_D
;--------------------------------------------------------------------------------
                LACL    *       ; Get table pointer in lower acc.
                                ; ARP=AR0, AR0->FR1, AR2->Park_q AR3->Park_D
;--------------------------------------------------------------------------------
                AND     #255    ; Ensure table pointer is 8 bit.
                                ; ARP=AR0, AR0->FR1, AR2->Park_q AR3->Park_D
;--------------------------------------------------------------------------------
                ADD     #SINTAB_360
                                ; ARP=AR0, AR0->FR1, AR2->Park_q AR3->Park_D
;--------------------------------------------------------------------------------
                TBLR    *       ; FR1 = SIN(theta).
                                ; ARP=AR0, AR0->FR1, AR2->Park_q AR3->Park_D
        .endif
;-----------------------------------
                ZAC             ; ACC = 0

                LT      *-,AR2   ; TREG = Sin(theta)
                                ; ARP=AR2, AR0->FR0, AR2->Park_q AR3->Park_D
;--------------------------------------------------------------------------------
                MPY     *-      ; PREG = sin(theta).Park_q
                                ; ARP=AR2, AR0->FR0, AR2->Park_d AR3->Park_D
;--------------------------------------------------------------------------------
                LTA     *+,AR0   ; TREG = Park_d
                                ; ACC  = sin(theta).Park_q
                                ; ARP=AR0, AR0->FR0, AR2->Park_q AR3->Park_D
;--------------------------------------------------------------------------------
                MPY     *+      ; PREG = Park_d * cos(theta)
                                ; ACC  = sin(theta).Park_q
                                ; ARP=AR0, AR0->FR1, AR2->Park_q AR3->Park_D
;--------------------------------------------------------------------------------
                MPYA    *-,AR3  ; ACC =  Park_q.sin(theta) + Park_d * cos(theta)
                                ; PREG=  Park_d.sin(theta)
                                ; ARP=AR3, AR0->FR0, AR2->Park_q AR3->Park_D
;--------------------------------------------------------------------------------
                SACH    *+,AR2  ; Store Park_D.
                                ; PREG=  Park_d.sin(theta)
                                ; ARP=AR2, AR0->FR0, AR2->Park_q AR3->Park_Q
;--------------------------------------------------------------------------------
                ZAC             ; ARP=AR2, AR0->FR0, AR2->Park_q AR3->Park_Q
;--------------------------------------------------------------------------------
                LT      *,AR0   ; TREG = Park_q
                                ; ARP=AR0, AR0->FR0, AR2->Park_q AR3->Park_Q
;--------------------------------------------------------------------------------
                MPYS    *,AR3   ; PREG = Park_q.cos(theta).
                                ; ACC  = - Park_d.sin(theta)
                                ; ARP=AR0, AR0->FR0, AR2->Park_q AR3->Park_Q
;--------------------------------------------------------------------------------
                APAC            ; ACC = - Park_d.sin(theta) + Park_q.cos(theta).
                                ; ARP=AR0, AR0->FR0, AR2->Park_q AR3->Park_Q
;--------------------------------------------------------------------------------
                SACH    *,AR1   ; Store Park_Q.

                SPM     0
;--------------------------------------------------------------------------------
__park_exit:
                SBRK        #(__park_framesize+1)
                LAR        AR0,*-
                PSHD        *
                RET