qsin.asm

变频器程序

;===========================================================
;
; File Name     :qsin.asm
; 
; Originator    :Digital Control Systems Group 
;                Texas Instruments 
; 
; Description   :This file contain source code for Fixed point sin
;               
; Date          : 17/10/2000
;==========================================================
; 
; 
; Routine Name  : Generic Function      
; Routine Type  : C Callable
; 
; Description   :
; signed int qsin(signed int x)
;
; Algorithm     :
; sin(x):  where x is in radians
;       = ((((0.0372*x)-0.2338)*x+0.0544)*x+0.9826)*x+0.0013  when x>=0
;       
;========================================================================
; Pseudo Code
;======================================================================
;
; signed int qsin(signed int x)
; { 
;   int y;
;   temp=abs(x);
;   y=appx(temp);   /* Sin approximation for positive value of 'x' */
;   
;   if(x<0)
;   {
;       y=-y;
;   }
;   return y;
; } 
;
;====================================================================
; Function Local Frame
;====================================================================
;   |_______|
;   |_______|<- Stack Pointer                           (FP+2) <---AR1
;   |_______|<- Register to Register Tfr & Computation  (FP)   <---AR0 
;   |_______|<- Old FP                                  (FP-1)
;   |_______|<- Return Address of the Caller            (FP-2) 
;   |_______|<- Formal parameter X                      (FP-3) <---AR2
;======================================================================   

; Module definition for external referance
            .def    _qsin   


__sin_frs   .set    00001h          ; Local frame size for this routine 
__pi        .set    00c91h          ; PI in Q10 Format, for short immediate MPY  
__a0        .set    05532h          ; 0.0013 scaled by 2^24
__a1        .set    07dc6h          ; 0.9826 scaled by 2^15
__a2        .set    06f69h          ; 0.0544 scaled by 2^19
__a3        .set    0884bh          ; -0.2338 scaled by 2^17
__a4        .set    00986h          ; 0.0372 scaled by 2^16 for short immediate MPY


_qsin:      
            POPD    *+              ; Store the Return Address in stack
            SAR     AR0,*+          ; Store the Caller's Frame Pointer
            SAR     AR1,*
            LAR     AR0,#__sin_frs      
            LAR     AR0,*0+,AR2     ; Create Local frame for sin function
                                     
            SETC    SXM             
            SETC    OVM
            LAR     AR2,#0FFFDh      
            MAR     *0+             ; Modify AR2 to point to x
            LACC    *,16,AR0        ; ACC=x, where x is in scaled Q15 Format
            ABS
            SACH    *               ; Take absolute value of input

            LT      *               ; TREG=x in scaled Q15 format 
            MPY     #__pi           ; Convert x from scaled Q15 to radians
                                    ; P= x*pi(Q25) 
            PAC                     ; ACC=x*pi(Q25)
            SACH    *,4             ; Store the radian in Q13 format
            
            LT      *               ; TREG=x in Q13 format
            
            MPY     #__a4           ; P=x*a4 in Q29   
            LACC    #__a3,12        ; ACC=a3 in Q29
            APAC                    ; ACC=a3+x*a4 in Q29
            SACH    *,3             ; Store a3+x*a4 in Q16
            
            MPY     *               ; P=x*(a3+x*a4) in Q29
            LACC    #__a2,10        ; ACC=a2 in Q29
            APAC                    ; ACC=a2+x*(a3+x*a4) in Q29
            SACH    *,2             ; Store a2+x*(a3+x*a4) in Q15
            
            MPY     *               ; P=x*(a2+x*(a3+x*a4)) in Q28
            LACC    #__a1,13        ; ACC=a1 in Q28
            APAC                    ; ACC=a1+x*(a2+x*(a3+x*a4)) in Q28
            SACH    *,3             ; Store a1+x*(a2+x*(a3+x*a4)) in Q15
            
            MPY     *               ; P=x*(a1+x*(a2+x*(a3+x*a4))) in Q28
            LACC    #__a0,4         ; ACC=a0 in Q28
            APAC                    ; ACC=a0+x*(a1+x*(a2+x*(a3+x*a4))) in Q28
            SACH    *,3             ; Store ACC in Q15 format
            
            LACC    *,0,AR2         ; Return the result
            BIT     *,0,AR1
            BCND    positive,NTC
            CMPL                    ; Negate the result, if input is negative

positive:
            CLRC    OVM
            SBRK    #(__sin_frs+1)  ; Clear the local frame
            LAR     AR0,*-          ; Retrive Caller's frame pointer
            PSHD    *               ; Push the return address to TOS
            RET                     ; Return to the caller