iclark.asm

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

;=====================================================================================
; File name:        ICLARK.ASM                     
;                    
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:                                 
; This file contains source for the inverse clark transformation.
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx.  (Peripheral Independant).
; Description:        
;                                (d,q) -> (a,b,c) Transformation
;                                clark_a = clark_q 
;                                clark_b = (-clark_q/2 + sqrt(3)/2 * clark_d) 
;                                clark_c = (-clark_q/2 - sqrt(3)/2 * clark_d) 
;
;
;================================================================================
; Routine Name: Generic function.        Routine Type: C Callable
;
; Description:
;  
;  C prototype : int invclark(void *in, void *out)
;================================================================================
                .def        _invclark
;================================================================================

__invclark_framesize .set 0001h
;================================================================================
_invclark:
                     POPD        *+
                SAR        AR0,*+
                SAR        AR1,*
                LARK        AR0,__invclark_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 #1 in AR2. 
                                ; 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->clark_d AR3->clark_a
;--------------------------------------------------------------------------------
                ADRK    #1      ; ARP=AR2, AR0->FR0, AR2->clark_q AR3->clark_a
;--------------------------------------------------------------------------------
                LACC    *-,AR3  ; ACC= clark_q.
                                ; ARP=AR3, AR0->FR0, AR2->clark_d, AR3->clark_a
;--------------------------------------------------------------------------------
                SACL    *+,AR0  ; Store clark_a=clark_q
                                ; ARP=AR0, AR0->FR0, AR2->clark_d, AR3->clark_b
;--------------------------------------------------------------------------------
                SPLK    #28378,* ;FR0=sqrt(3)/2
                                ; ARP=AR0, AR0->FR0, AR2->clark_d, AR3->clark_b
;--------------------------------------------------------------------------------
                LT      *,AR2   ; TREG = sqrt(3)/2.
                                ; ARP=AR2, AR0->FR0, AR2->clark_d, AR3->clark_b
;--------------------------------------------------------------------------------
                MPY     *+,AR0  ; PREG = clark_d * sqrt(3)/2.
                                ; ARP=AR0, AR0->FR0, AR2->clark_q, AR3->clark_b
;--------------------------------------------------------------------------------
                PAC             ; ACC = clark_d * sqrt(3)/2.
                                ; ARP=AR0, AR0->FR0, AR2->clark_q, AR3->clark_b
;--------------------------------------------------------------------------------
                SACH    *,AR2   ; FR0 = clark_d * sqrt(3)/2.
                                ; ARP=AR2, AR0->FR0, AR2->clark_q, AR3->clark_b
;--------------------------------------------------------------------------------
                LACC    *,16,AR0 ; ACCH = clark_q
                                ; ARP=AR0, AR0->FR0, AR2->clark_q, AR3->clark_b
;--------------------------------------------------------------------------------
                NEG             ; ACCH = -clark_q
                                ; ARP=AR0, AR0->FR0, AR2->clark_q, AR3->clark_b
;--------------------------------------------------------------------------------
                SFR             ; ACCH = -clark_q /2 
                                ; ARP=AR0, AR0->FR0, AR2->clark_q, AR3->clark_b
;--------------------------------------------------------------------------------
                ADD     *,16,AR3 ; ACCH = -clark_q /2  + clark_d * sqrt(3)/2
                                ; ARP=AR3, AR0->FR0, AR2->clark_q, AR3->clark_b
;--------------------------------------------------------------------------------
                SACH    *+,AR0  ; Store clark_b.
                                ; ARP=AR0, AR0->FR0, AR2->clark_q, AR3->clark_c
;--------------------------------------------------------------------------------
                SUB     *,16    ; ACCH = -clark_q /2  + clark_d * sqrt(3)/2
                                ;                      - clark_d * sqrt(3)/2.
                                ; i.e. ACCH = -clark_q/2
                                ; ARP=AR0, AR0->FR0, AR2->clark_q, AR3->clark_c
;--------------------------------------------------------------------------------
                SUB    *,16,AR3 ; ACCH = -clark_q/2 - clark_d * sqrt(3)/2.
                                ;      = clark_c
                                ; ARP=AR0, AR0->FR0, AR2->clark_q, AR3->clark_c
;--------------------------------------------------------------------------------
                SACH   *,AR1    ; Store clark_c.
                                ; ARP=AR0, AR0->FR0, AR2->clark_q, AR3->clark_c
;--------------------------------------------------------------------------------
                SPM     0
;--------------------------------------------------------------------------------
__invclark_exit:
                SBRK        #(__invclark_framesize+1)
                LAR        AR0,*-
                PSHD        *
                RET