506 lines
16 KiB
NASM
506 lines
16 KiB
NASM
;;###########################################################################
|
|
;;
|
|
;; FILE: IQNatan2PU.asm
|
|
;;
|
|
;; TITLE: C Callable IQ Math Function
|
|
;;
|
|
;;###########################################################################
|
|
;;
|
|
;; Ver | Date | Who | Description of changes
|
|
;; =====|=============|=======|==============================================
|
|
;; 1.4 | 17 May 2002 | A. T. | Original Release.
|
|
;; -----|-------------|-------|----------------------------------------------
|
|
;; | | |
|
|
;;
|
|
;;###########################################################################
|
|
;;#############################################################################
|
|
;;!
|
|
;;! Copyright: Copyright (C) 2023 Texas Instruments Incorporated -
|
|
;;! All rights reserved not granted herein.
|
|
;;! Limited License.
|
|
;;!
|
|
;;! Texas Instruments Incorporated grants a world-wide, royalty-free,
|
|
;;! non-exclusive license under copyrights and patents it now or hereafter
|
|
;;! owns or controls to make, have made, use, import, offer to sell and sell
|
|
;;! ("Utilize") this software subject to the terms herein. With respect to the
|
|
;;! foregoing patent license, such license is granted solely to the extent that
|
|
;;! any such patent is necessary to Utilize the software alone. The patent
|
|
;;! license shall not apply to any combinations which include this software,
|
|
;;! other than combinations with devices manufactured by or for TI
|
|
;;! ("TI Devices").
|
|
;;! No hardware patent is licensed hereunder.
|
|
;;!
|
|
;;! Redistributions must preserve existing copyright notices and reproduce this
|
|
;;! license (including the above copyright notice and the disclaimer and
|
|
;;! (if applicable) source code license limitations below) in the documentation
|
|
;;! and/or other materials provided with the distribution.
|
|
;;!
|
|
;;! Redistribution and use in binary form, without modification, are permitted
|
|
;;! provided that the following conditions are met:
|
|
;;!
|
|
;;! * No reverse engineering, decompilation, or disassembly of this software is
|
|
;;! permitted with respect to any software provided in binary form.
|
|
;;! * Any redistribution and use are licensed by TI for use only
|
|
;;! with TI Devices.
|
|
;;! * Nothing shall obligate TI to provide you with source code for the
|
|
;;! software licensed and provided to you in object code.
|
|
;;!
|
|
;;! If software source code is provided to you, modification and redistribution
|
|
;;! of the source code are permitted provided that the following conditions
|
|
;;! are met:
|
|
;;!
|
|
;;! * any redistribution and use of the source code, including any resulting
|
|
;;! derivative works, are licensed by TI for use only with TI Devices.
|
|
;;! * any redistribution and use of any object code compiled from the source
|
|
;;! code and any resulting derivative works, are licensed by TI for use
|
|
;;! only with TI Devices.
|
|
;;!
|
|
;;! Neither the name of Texas Instruments Incorporated nor the names of its
|
|
;;! suppliers may be used to endorse or promote products derived from this
|
|
;;! software without specific prior written permission.
|
|
;;#############################################################################
|
|
|
|
;;===========================================================================
|
|
;; Function: _IQNatan2PU
|
|
;;===========================================================================
|
|
;;
|
|
;; C Usage: extern long _IQNatan2PU(long Y, long X);
|
|
;;
|
|
;;---------------------------------------------------------------------------
|
|
;;
|
|
;; On Entry: ACC = Y in IQ format
|
|
;; SP-4 = X in IQ format
|
|
;;
|
|
;; Regs Used: XAR7, XAR6, XAR5, XAR4, XAR0, XT, P
|
|
;;
|
|
;; On Exit: ACC = Per Unit result in IQ format (0.0=0Deg, 1.0=360Deg)
|
|
;; if( X == 0.0 and Y == 0.0 ), ACC = 0.0
|
|
;; if( Y >= 0.0 ), 0.0 <= ACC <= 0.5
|
|
;; if( Y < 0.0 ), 0.5 < ACC < 1.0
|
|
;;
|
|
;; Q range: 29 to 1
|
|
;;
|
|
;; Note: A value of Q30 is not feasible since the result would
|
|
;; saturate for angles greater then +/- 2.0 radians.
|
|
;;
|
|
;;---------------------------------------------------------------------------
|
|
;; Algorithm: The algorithm steps to calculate the "atan2" of the given
|
|
;; input X and Y is as follows:
|
|
;;
|
|
;; Step(1): if( abs(X) >= abs(Y) )
|
|
;; Numerator = abs(Y)
|
|
;; Denominator = abs(X)
|
|
;; else
|
|
;; Numerator = abs(X)
|
|
;; Denominator = abs(Y)
|
|
;;
|
|
;; Step(2): Ratio = Numerator/Denominator
|
|
;;
|
|
;; Note: Ratio range = 0.0 to 1.0
|
|
;;
|
|
;; Step(3): Use the upper 6-bits of the "Ratio" value as an
|
|
;; index into the table to obtain the coefficients
|
|
;; for a second order equation:
|
|
;;
|
|
;; _IQNatan2Table:
|
|
;; CoeffA0[0]
|
|
;; CoeffA1[0]
|
|
;; CoeffA2[0]
|
|
;; .
|
|
;; .
|
|
;; CoeffA0[63]
|
|
;; CoeffA1[63]
|
|
;; CoeffA2[63]
|
|
;;
|
|
;; Step(4): Calculate the angle using the folowing equation:
|
|
;;
|
|
;; arctan(Ratio) = A0 + A1*Ratio + A2*Ratio*Ratio
|
|
;; arctan(Ratio) = A0 + Ratio(A1 + A2*Ratio)
|
|
;;
|
|
;;
|
|
;; Step(5): The final angle is determined as follows:
|
|
;;
|
|
;; if( X >= 0 and Y >= 0 and abs(X) >= abs(Y) )
|
|
;; Angle = arctan(abs(Y)/abs(X))
|
|
;; if( X >= 0 and Y >= 0 and abs(X) < abs(Y) )
|
|
;; Angle = PI/2 - arctan(abs(X)/abs(Y))
|
|
;; if( X < 0 and Y >= 0 and abs(X) < abs(Y) )
|
|
;; Angle = PI/2 + arctan(abs(X)/abs(Y))
|
|
;; if( X < 0 and Y >= 0 and abs(X) >= abs(Y) )
|
|
;; Angle = PI - arctan(abs(Y)/abs(X))
|
|
;;
|
|
;; Step(6): Convert to Per Unit (PU) value:
|
|
;;
|
|
;; PUangle(Ratio) = arctan(Ratio)/(2*pi)
|
|
;; PUangle(Ratio) = arctan(Ratio) * 0.159154943
|
|
;; PUangle(Ratio) = 0.0 to 0.5
|
|
;; if( Y < 0 )
|
|
;; PUAngle(Ratio) = 1.0 - PUAngle(Ratio);
|
|
;;
|
|
;; PUangle(Ratio) = 0.0 (0deg) to 1.0 (360deg)
|
|
;;
|
|
;;---------------------------------------------------------------------------
|
|
;; Benchmark:
|
|
;; * Code executes from 0-wait SARAM block.
|
|
;; * _IQatan2 & _IQdiv Tables located in seperate 0-wait SARAM or 1-wait ROM block.
|
|
;; * Stack located in seperate 0-wait SARAM block.
|
|
;; * Includes LCR/LRETR.
|
|
;;
|
|
;; Cycles = 121 (if 16 <= Q <= 29 and _IQatan2 & _IQdiv Tables in 0-wait SARAM)
|
|
;; = 124 (if 1 <= Q <= 15 and _IQatan2 & _IQdiv Tables in 0-wait SARAM)
|
|
;;
|
|
;; Cycles = 127 (if 16 <= Q <= 29 and _IQatan2Table & _IQdivTable in 1-wait ROM)
|
|
;; = 130 (if 1 <= Q <= 15 and _IQatan2Table & _IQdivTable in 1-wait ROM)
|
|
;;
|
|
;;===========================================================================
|
|
|
|
IQNatan2PU .macro q_value
|
|
;; Perform Step 1:
|
|
SETC OVM ; Make sure overflow mode is on
|
|
MOVL *SP++,ACC ; SP-6 = Y
|
|
ABS ACC ; ACC = abs(Y)
|
|
MOVL XT,@ACC ; XT = abs(Y)
|
|
MOVL ACC,*-SP[6] ; ACC = X
|
|
ABS ACC ; ACC = abs(X)
|
|
MOVL P,@ACC ; P = abs(X)
|
|
SUBL ACC,@XT ; ACC = abs(X) - abs(Y)
|
|
MOVL *SP++,ACC ; SP-4 = abs(X) - abs(Y)
|
|
|
|
MOVL ACC,@XT ; ACC = abs(Y)
|
|
MINL ACC,@P ; if( abs(Y) > abs(X) ), ACC = abs(X)
|
|
MOVL *SP++,ACC ; SP-2 = Numerator
|
|
MOVL ACC,@P ; ACC = abs(X)
|
|
MAXL ACC,@XT ; if( abs(X) < abs(Y) ), ACC = abs(Y)
|
|
; ACC = Denominator
|
|
; SP-10 = X
|
|
; SP-8 = Return Address
|
|
; SP-6 = Y
|
|
; SP-4 = abs(X) - abs(Y)
|
|
; SP-2 = Numerator
|
|
|
|
;; Perform Step 2:
|
|
CSB ACC ; Count sign bits, T = n
|
|
LSLL ACC,T ; ACC = Dnorm (Q31)
|
|
MOVL @XAR4,ACC ; Temp save "Dnorm"
|
|
LSR AH,#6
|
|
MOVB @AH,#0xFE,EQ ; If Den = 0, load zero value
|
|
MOVZ AR0,@AH ; AR0 = index into _IQdivTable
|
|
MOVL XAR7,#(_IQdivTable-254)
|
|
MOVB AH,#(62 - q_value) ; AH = 62 - q_value
|
|
SUBR @T,AH ; T = 62 - q_value - n
|
|
MOVL XAR5,@XT ; AR5H = 62 - q_value - n
|
|
MOVL XT,*+XAR7[AR0] ; (1) XT = initial Dm estimate (Q29)
|
|
MOV @AH,#0x4000
|
|
MOVB AL,#0
|
|
MOVL XAR6,@ACC ; XAR6 = 2.0 (Q29)
|
|
|
|
IMPYL P,XT,@XAR4 ; (2)
|
|
QMPYL ACC,XT,@XAR4 ; (2) ACC:P = Dm*Dnorm (Q60)
|
|
LSL64 ACC:P,#1 ; (3) ACC:P = Dm*Dnorm (Q61)
|
|
SUBL ACC,@XAR6 ; (4) ACC = Dm*Dnorm - 2.0 (Q29)
|
|
NEG ACC ; (5) ACC = 2.0 - Dm*Dnorm (Q29)
|
|
IMPYL P,XT,@ACC ; (6)
|
|
QMPYL ACC,XT,@ACC ; (6) ACC:P = Dm(2.0 - Dm*Dnorm) (Q58)
|
|
LSL64 ACC:P,#3 ; (7) ACC:P = Dm(2.0 - Dm*Dnorm) (Q61)
|
|
|
|
MOVL XT,@ACC ; (1) XT = Dm (Q29)
|
|
IMPYL P,XT,@XAR4 ; (2)
|
|
QMPYL ACC,XT,@XAR4 ; (2) ACC:P = Dm*Dnorm (Q60)
|
|
LSL64 ACC:P,#1 ; (3) ACC:P = Dm*Dnorm (Q61)
|
|
SUBL ACC,@XAR6 ; (4) ACC = Dm*Dnorm - 2.0 (Q29)
|
|
NEG ACC ; (5) ACC = 2.0 - Dm*Dnorm (Q29)
|
|
IMPYL P,XT,@ACC ; (6)
|
|
QMPYL ACC,XT,@ACC ; (6) ACC:P = Dm(2.0 - Dm*Dnorm) (Q58)
|
|
LSL64 ACC:P,#3 ; (7) ACC:P = Dm(2.0 - Dm*Dnorm) (Q61)
|
|
|
|
MOVL XT,*-SP[2] ; XT = Numerator
|
|
IMPYL P,XT,@ACC
|
|
QMPYL ACC,XT,@ACC ; ACC:P = N * Dm
|
|
LSL64 ACC:P,#2
|
|
MOVL XT,@XAR5 ; T = 62 - q_value - n
|
|
ASR64 ACC:P,T ; scale back result
|
|
; P = Ratio in IQ format
|
|
;; Perform Step 3:
|
|
MOVL XT,@P ; XT = Ratio
|
|
MOVB ACC,#64 ; ACC = 64 = Elements In Table
|
|
IMPYL P,XT,@ACC
|
|
QMPYL ACC,XT,@ACC ; ACC:P = 64*Ratio
|
|
.if q_value >= 16
|
|
LSL64 ACC:P,#(32 - q_value) ; ACC = int(64*Ratio)
|
|
.else
|
|
LSL64 ACC:P,#16
|
|
LSL64 ACC:P,#(16 - q_value) ; ACC = int(64*Ratio)
|
|
.endif
|
|
MPY P,@AL,#6 ; P = 6 * int(64*Ratio)
|
|
MOVZ AR0,@PL ; AR0 = _IQatan2Table index
|
|
MOVL XAR5,#_IQatan2Table ; Point to A0
|
|
MOVL XAR6,#_IQatan2Table+2 ; Point to A1
|
|
MOVL XAR7,#_IQatan2Table+4 ; Point to A2
|
|
|
|
;; Perform Step 4:
|
|
IMPYL P,XT,*+XAR7[AR0]
|
|
QMPYL ACC,XT,*+XAR7[AR0] ; ACC:P = A2*Ratio (q_value + Q30)
|
|
.if q_value >= 16
|
|
LSL64 ACC:P,#(32-q_value) ; ACC = A2*Ratio (Q30)
|
|
.else
|
|
LSL64 ACC:P,#16
|
|
LSL64 ACC:P,#(16-q_value) ; ACC = A2*Ratio (Q30)
|
|
.endif
|
|
ADDL ACC,*+XAR6[AR0] ; ACC = A1 + A2*Ratio (Q30)
|
|
IMPYL P,XT,@ACC
|
|
QMPYL ACC,XT,@ACC ; ACC:P = Ratio(A1 + A2*Ratio) (q_value + Q30)
|
|
.if q_value >= 16
|
|
LSL64 ACC:P,#(32-q_value) ; ACC:P = Ratio(A1 + A2*Ratio) (Q30)
|
|
.else
|
|
LSL64 ACC:P,#16
|
|
LSL64 ACC:P,#(16-q_value) ; ACC:P = Ratio(A1 + A2*Ratio) (Q30)
|
|
.endif
|
|
ADDL ACC,*+XAR5[AR0] ; ACC = A0 + Ratio(A1 + A2*Ratio) (Q30)
|
|
.if (q_value >= 14) & (q_value <= 29)
|
|
LSR64 ACC:P,#(30-q_value) ; ACC = A0 + Ratio(A1 + A2*Ratio) (q_value)
|
|
.endif
|
|
.if q_value <= 13
|
|
LSR64 ACC:P,#16
|
|
LSR64 ACC:P,#(14-q_value) ; ACC = A0 + Ratio(A1 + A2*Ratio) (q_value)
|
|
.endif
|
|
|
|
;; Perform Step 5:
|
|
MOVL XAR7,#(_IQatan2HalfPITable + q_value*2)
|
|
MOVL XAR6,*XAR7 ; XAR6 = PI/2
|
|
MOVL P,@ACC ; P = arctan(Ratio)
|
|
NEG ACC ; ACC = -arctan(Ratio)
|
|
TBIT *-SP[9],#15 ; Test if(X >= 0)
|
|
MOVL @P,ACC,NTC ; if(X >= 0), P = -arctan(Ratio)
|
|
MOVL ACC,@P
|
|
NEG ACC ; ACC = -P
|
|
TBIT *-SP[3],#15 ; Test if(abs(X) >= abs(Y))
|
|
MOVL @P,ACC,NTC ; if(abs(X) >= abs(Y)), P = -P
|
|
|
|
MOVL ACC,@XAR6 ; ACC = PI/2
|
|
NEG ACC ; ACC = -PI/2
|
|
TBIT *-SP[9],#15 ; Test if(X < 0)
|
|
NEGTC ACC ; if(X < 0), ACC = PI/2
|
|
ADDL ACC,@XAR6 ; ACC = ACC + PI/2
|
|
TBIT *-SP[3],#15 ; Test if(abs(X) >= abs(Y))
|
|
MOVL @XAR6,ACC,NTC ; if(abs(X) >= abs(Y)), XAR6 = ACC
|
|
MOVL ACC,@XAR6
|
|
ADDL ACC,@P ; ACC = arctan(abs(Y),X)
|
|
|
|
;; Perform Step 6:
|
|
MOVL XT,@ACC
|
|
MOV @PL,#0x60DC ; P = 1/(2*pi) (Q32)
|
|
MOV @PH,#0x28BE
|
|
QMPYL P,XT,@P ; P = arctan(Ratio) * 1/(2*pi) = PUangle (q_value)
|
|
.if q_value <= 15 ; ACC = 1.0 (q_value)
|
|
MOV @AL,#(1 << q_value)
|
|
MOV @AH,#0x0000
|
|
.endif
|
|
.if q_value == 16
|
|
MOV @AL,#0x0000
|
|
MOV @AH,#0x0001
|
|
.endif
|
|
.if q_value >= 17
|
|
MOV @AL,#0x0000
|
|
MOV @AH,#(1 << (q_value - 16))
|
|
.endif
|
|
SUBL ACC,@P ; ACC = 1.0 - PUangle
|
|
TBIT *-SP[5],#15 ; Test if(Y < 0)
|
|
MOVL @P,ACC,TC ; if(Y < 0), P = 1.0 - PUangle
|
|
MOVL ACC,@P ; ACC = PUangle
|
|
|
|
SUBB SP,#6 ; Deallocate stack
|
|
CLRC OVM ; Turn off overflow mode
|
|
LRETR
|
|
.endm
|
|
|
|
;; The value "GLOBAL_Q" needs to be supplied by the assembler
|
|
;; using the "-dGLOBAL_Q=q_value" directive:
|
|
.sect "IQmath"
|
|
.ref _IQatan2HalfPITable
|
|
.ref _IQatan2Table
|
|
.ref _IQdivTable
|
|
|
|
.if GLOBAL_Q == 30
|
|
.def __IQ30atan2PU
|
|
__IQ30atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 29
|
|
.def __IQ29atan2PU
|
|
__IQ29atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 28
|
|
.def __IQ28atan2PU
|
|
__IQ28atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 27
|
|
.def __IQ27atan2PU
|
|
__IQ27atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 26
|
|
.def __IQ26atan2PU
|
|
__IQ26atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 25
|
|
.def __IQ25atan2PU
|
|
__IQ25atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 24
|
|
.def __IQ24atan2PU
|
|
__IQ24atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 23
|
|
.def __IQ23atan2PU
|
|
__IQ23atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 22
|
|
.def __IQ22atan2PU
|
|
__IQ22atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 21
|
|
.def __IQ21atan2PU
|
|
__IQ21atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 20
|
|
.def __IQ20atan2PU
|
|
__IQ20atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 19
|
|
.def __IQ19atan2PU
|
|
__IQ19atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 18
|
|
.def __IQ18atan2PU
|
|
__IQ18atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 17
|
|
.def __IQ17atan2PU
|
|
__IQ17atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 16
|
|
.def __IQ16atan2PU
|
|
__IQ16atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 15
|
|
.def __IQ15atan2PU
|
|
__IQ15atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 14
|
|
.def __IQ14atan2PU
|
|
__IQ14atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 13
|
|
.def __IQ13atan2PU
|
|
__IQ13atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 12
|
|
.def __IQ12atan2PU
|
|
__IQ12atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 11
|
|
.def __IQ11atan2PU
|
|
__IQ11atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 10
|
|
.def __IQ10atan2PU
|
|
__IQ10atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 9
|
|
.def __IQ9atan2PU
|
|
__IQ9atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 8
|
|
.def __IQ8atan2PU
|
|
__IQ8atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 7
|
|
.def __IQ7atan2PU
|
|
__IQ7atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 6
|
|
.def __IQ6atan2PU
|
|
__IQ6atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 5
|
|
.def __IQ5atan2PU
|
|
__IQ5atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 4
|
|
.def __IQ4atan2PU
|
|
__IQ4atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 3
|
|
.def __IQ3atan2PU
|
|
__IQ3atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 2
|
|
.def __IQ2atan2PU
|
|
__IQ2atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
.if GLOBAL_Q == 1
|
|
.def __IQ1atan2PU
|
|
__IQ1atan2PU:
|
|
IQNatan2PU GLOBAL_Q
|
|
.endif
|
|
|
|
;;###########################################################################
|
|
;; No More.
|
|
;;###########################################################################
|