ipark.asm

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

;=====================================================================================
; File name:        IPARK.ASM                     
;                    
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:                                 
; This file contains source for the INVERSE PARK transformation.
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.00
;================================================================================
; 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        _ipark

                .ref    SINTAB_360


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

__ipark_framesize .set 0002h
;================================================================================
_ipark:
                     POPD        *+
                SAR        AR0,*+
                SAR        AR1,*
                LARK        AR0,__ipark_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
;--------------------------------------------------------------------------------
                LTS     *+,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
;--------------------------------------------------------------------------------
                MPYA    *,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
;--------------------------------------------------------------------------------
__ipark_exit:
                SBRK        #(__ipark_framesize+1)
                LAR        AR0,*-
                PSHD        *
                RET