qcos.asm

变频器程序

;===========================================================
;
; File Name     :qcos.asm
; 
; Originator    :Digital Control Systems Group 
;                Texas Instruments 
; 
; Description   :This file contain source code for Fixed point sin
;               
; Date          : 20/10/2000
;==========================================================
; 
; 
; Routine Name  : Generic Function      
; Routine Type  : C Callable
; 
; Description   :
; int qcos(int x)
;
; Algorithm     :
; cos(x):  where x is in radians
;       = (((((-0.0076*x)+0.0595)*x-0.0211)*x-0.4879)*x-0.0028)*x+1.0   
;
; This equation is applicable only in the first quadrant    
; If the input lies in other quadrant, they are converted to the first quadrant 
; and the above equation is used, the resultant is appropriately modified
; based on the input parameter's quadrant.
; 
; The value of 'x' in all the four Quadrant is given below      
;
;   Quadrant    Range of 'x'(ABS(x))            
;   I           0000h(0000h ) - 3fffh(3fffh)  0 to PI/2      
;   II          4000h(4000h ) - 7fffh(7fffh)  PI/2 to PI 
;   III         8000h(7fffh*) - bfffh(4001h) -PI to -PI/2
;   IV          c000h(4000h ) - ffffh(0001h) -PI/2 to 0
; * ABS(8000h) will be saturated to 7fffh, only if OVM bit is set
; ==================================================================== 
; Pseudo Code:
; ====================================================================
; signed int qcos(signed int x)
; { 
;   int cs,flag;
;
;   x=abs(x);       /* Range of ABS(x) in I and IV Quadrant ~0000h-03fffh*/
;   flag=0x4000-x;  /* Flag +ve in I & IV Quadrant              */
;   if(flag<0)      /* True in the 2nd and 3rd Quadrant (Cos(x) is -ve) */ 
;   x=0x7fff-x; 
;  
;   cs=appx(x);     /* use approximation equation to find the cos(x)    */
;   if(flag>=0)
;       return cs;  /* Return the result for I and IV Quadrant          */
;   else                            
;       return -cs; /* Negate and return the result */
; }
;
;
;============================================================

; Function Local Frame
;   |_______|
;   |_______|<- Stack Pointer                           (FP+2) <---AR1 
;   |_______|<- I&IV Quadrant Flag                      (FP+1) <---AR2
;   |_______|<- Register to Register Tfr & Computation  (FP)   <---AR0 
;   |_______|<- Old FP                                  (FP-1)
;   |_______|<- Return Address of the Caller            (FP-2) 
;   |_______|<- Formal parameter X                      (FP-3) 
   

; Module definition for external referance
            .def    _qcos 
            

__cos_frs   .set    00002h          ; Local frame size for this routine 
__pi        .set    00c91h          ; PI in Q10 Format, for short immediate MPY  
__a0        .set    07fffh          ; ~1.0 Represented in Q15
__a1        .set    0a440h          ; -0.0028 scaled by 2^23 
__a2        .set    08319h          ; -0.4879 scaled by 2^16
__a3        .set    0a995h          ; -0.0211 scaled by 2^20
__a4        .set    079dbh          ; 0.0595 scaled by 2^19 
__a5        .set    0106fh          ; -0.0076 scaled by 2^19 for Immediate MPY


     
_qcos:      
            POPD    *+              ; Store the Return Address in stack
            SAR     AR0,*+          ; Store the Caller's Frame Pointer
            SAR     AR1,*
            LAR     AR0,#__cos_frs      
            LAR     AR0,*0+,AR2     ; Create Local frame for cos function
            
            SETC    OVM             ; To saturate ABS value of 0x8000 to 0x7fff                        
            SETC    SXM             
            
            LAR     AR2,#0FFFDh      
            MAR     *0+             ; Modify AR2 to point to x, which is in Q15
            LACC    *,16            ; Convert the 2's complement to absolute value
            ABS
            ADRK    #3h             ; AR2=FP
            SACH    *               ; (FP)=abs(x)           
            
            CLRC    OVM
            
            LACC    #04000h             
            SUB     *+              ; ACC=0x3fffh-abs(x)
            SACH    *-              ; (FP+1) -ve for II and III quadrant
            BCND    cos_pos,GEQ     ; cos(x) is +ve, if x is in I and IV quadrant
                                    
            ADD     #03fffh         ; ACC=0x3fffh+0x4000h-abs(x)
            SACL    *               ; (FP)=0x7fff-abs(x)
                    
cos_pos:    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     #__a5           ; P=x*a5 in Q32
            LACC    #__a4,13        ; ACC=a4 in Q32  
            APAC                    ; ACC=a4+x*a5 in Q32  
            SACH    *,3             ; Store a4+x*a5 in Q19  
            
            MPY     *               ; P=x*(a4+x*a5) in Q32              
            LACC    #__a3,12        ; ACC=a3 in Q32   
            APAC                    ; ACC=a3+x*(a4+x*a5) in Q32
            SACH    *               ; Store a3+x*(a4+x*a5) in Q16
            
            MPY     *               ; P=x*(a3+x*(a4+x*a5)) in Q29
            LACC    #__a2,13        ; ACC=a2 in Q29
            APAC                    ; ACC=a2+x*(a3+x*(a4+x*a5)) in Q29
            SACH    *,2             ; Store a2+x*(a3+x*(a4+x*a5)) in Q15
            
            MPY     *               ; P=x*(a2+x*(a3+x*(a4+x*a5))) in Q28
            LACC    #__a1,5         ; ACC=a1 in Q28
            APAC                    ; ACC=a1+x*(a2+x*(a3+x*(a4+x*a5))) in Q28
            SACH    *,2             ; Store a1+x*(a2+x*(a3+x*(a4+x*a5))) in Q14
            
            MPY     *               ; P=x*(a1+x*(a2+x*(a3+x*(a4+x*a5)))) in Q27
            LACC    #__a0,12        ; ACC=a0 in Q27
            APAC                    ; ACC=a0+x*(a1+x*(a2+x*(a3+x*(a4+x*a5)))) in Q27
            SACH    * ,4            ; Store a0+x*(a1+x*(a2+x*(a3+x*(a4+x*a5)))) in Q15  
            LACC    *+              ; Return the result 
            
            BIT     *,15,AR1        ; Check whether x was in (I,IV Q) or (II, III Q)
            BCND    positive,NTC
            NEG                     ; Negate the result if x is in II&III Quadrant
            
positive:   SBRK    #(__cos_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