vhzprof.asm

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

;=====================================================================================
; File name:        VHZPROF.ASM                       
;                    
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:                                 
; This file contains source for the Volt-Hz profile compute function.
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx.  (Peripheral Independant).
;
;
;================================================================================
; Routine Name: Volt Hz Profile Calculator.        Routine Type: C Callable
;
; Description:
;  
;  
;  C prototype : int Vhz_Profile_Calculate (VHZPROFILE_DATA *p)
;================================================================================
; History     Created Dec 29, 1999.
;
; Definition of VHPROFILE:
; struct VHZPROFILE_DATA { int fin,fmin,fmax,slope,vmax,vmin,vout,
;                          (int (*)(int))Vhz_Profile_Calculate };
;================================================================================                       

                .def        _Vhz_Profile_Calculate
;================================================================================
_Vhz_Profile_Calculate:

__Vhz_Profile_Calculate_framesize .set 0001h
;================================================================================
                     POPD        *+
                SAR        AR0,*+
                SAR        AR1,*
                LARK        AR0,__Vhz_Profile_Calculate_framesize
                LAR        AR0,*0+,AR0

;================================================================================
                SBRK        #3        ; Point AR0 to the first argument.
                                ; This is a pointer to the Vhz_profile_data.
;--------------------------------------------------------------------------------
                LAR        AR2,*        ; get the pointer in AR2.
;--------------------------------------------------------------------------------
                ADRK        #3        ; Restore frame pointer to FR0.
;--------------------------------------------------------------------------------
                MAR        *,AR2   ; ARP=AR2
;--------------------------------------------------------------------------------
                LACC        *+        ; get fin in acc.
;--------------------------------------------------------------------------------
                SUB        *-        ; subtract. ACC=fin-fmin
                                ; also AR2 now points back to fin.
                                ; ACC=fin-fmax.
                                ; This should be +ve. If -ve, or zero saturate!
;--------------------------------------------------------------------------------
                BCND        __VHZPRCALC_min_sat,LEQ
;--------------------------------------------------------------------------------
                LACC        *        ; get fin in acc.
;--------------------------------------------------------------------------------
                ADRK        #2        ; point to fmax.
;--------------------------------------------------------------------------------
                SUB        *        ; subtract. ACC=fin-fmax.
                                ; This should NOT be +ve. If +ve saturate!
                                ; also AR2 now points to fmax.
;--------------------------------------------------------------------------------
                BCND        __VHZPRCALC_max_sat,GT
;--------------------------------------------------------------------------------
; Normal compute.
;--------------------------------------------------------------------------------
                ; AR2 now points to fmax.
;--------------------------------------------------------------------------------                
                MAR        *+        ; point AR2 to slope.
                LT        *-        ; load slope in TREG and point AR2 back to fmax
;--------------------------------------------------------------------------------
                SBRK        #2        ; AR2 now points to fin.
;--------------------------------------------------------------------------------
                LACC        *+        ; load fin
                                ; AR2 now points to fmin.
;--------------------------------------------------------------------------------
                SUB        *,AR0        ; ACC=fin-fmin
                                ; AR2 now points to fmin.
                                ; ARP=AR0
;--------------------------------------------------------------------------------
;             ; TREG contains slope.
                
                SACL        *        ; store (fin-fmin) in FR0.
                MPY         *,AR2        ; PREG= slope *(fin-fmin).

            ; slope is in Q12. So product reg = Q12*Q15=Q27.
            ; i.e product register is S S I2 I1 I0 . F26 ===== F0.
            ; so a LEFT shift of 4 bits will put it in Q31, and then pick
            ; ACCH for Q15.
        
                SPM        2        ; Set product mode for LEFT shift of 4 bits.
                PAC        
                SPM        0         ; Restore compiler SPM mode requirement.

;--------------------------------------------------------------------------------
            ; AR2 now points to fmin.                
;--------------------------------------------------------------------------------
                ADRK        #4        ; point to Vmin.
;--------------------------------------------------------------------------------
                ADDH        *+        ; add Vmin to product and point to Vout.
;--------------------------------------------------------------------------------
                SACH        *,AR1        ; store Vout=Vmin+slope*(fin-fmin).
                                ; also ARP=AR1 in preparation for exit.
;--------------------------------------------------------------------------------
                B        __Vhz_profile_calculate_exit        
;--------------------------------------------------------------------------------

__VHZPRCALC_min_sat:

        ; If we are here it means the input freq < f_minimum and so we put out
        ; vout=vmin.
        ; AR2 is pointing to fin after subtraction.                


                ADRK        #5        ; point AR2 to Vmin
;--------------------------------------------------------------------------------
                LACC        *+        ; Load Vmin, 
                                ; also increment the pointer to pt to Vout
;--------------------------------------------------------------------------------
                SACL        *,AR1        ; store Vout.
                                ; also ARP=AR1 in preparation for exit.
;--------------------------------------------------------------------------------                
                B        __Vhz_profile_calculate_exit        
;--------------------------------------------------------------------------------
__VHZPRCALC_max_sat:

        ; If we are here it means the input freq > f_maxand so we put out
        ; vout=vmax.
        ; AR2 is pointing to fmax after subtraction.                

                ADRK        #2        ; point AR2 to Vmax
;--------------------------------------------------------------------------------
                LACC        *        ; Load Vmin, 
;--------------------------------------------------------------------------------
                ADRK        #2        ; also increment the pointer to pt to Vout
;--------------------------------------------------------------------------------
                SACL        *,AR1        ; store Vout.
                                ; also ARP=AR1 in preparation for exit.
;--------------------------------------------------------------------------------                
                B        __Vhz_profile_calculate_exit        
;--------------------------------------------------------------------------------
__Vhz_profile_calculate_exit:
                SBRK        #(__Vhz_Profile_Calculate_framesize+1)
                LAR        AR0,*-
                PSHD        *
                RET