204 lines
8.5 KiB
NASM
204 lines
8.5 KiB
NASM
;;#############################################################################
|
|
;; FILE: CLAatan2.asm
|
|
;;
|
|
;; DESCRIPTION: CLA arctan2 function
|
|
;;
|
|
;;#############################################################################
|
|
;;!
|
|
;;! 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.
|
|
;;#############################################################################
|
|
|
|
.cdecls C,LIST,"CLAmath.h"
|
|
.include "CLAeabi.asm"
|
|
|
|
;;----------------------------------------------------------------------------
|
|
;; Description:
|
|
;; 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:
|
|
;;
|
|
;; _FPUatan2Table:
|
|
;; 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))
|
|
;; if( Y < 0 )
|
|
;; Angle = -Angle
|
|
;; Equation: z = atan(y/x)
|
|
;;
|
|
;; Regs Used: MR0, MR1, MR2, MR3, MAR0,MAR1
|
|
;;
|
|
;; Input: x , y two f32 values in memory
|
|
;;
|
|
;; Output: z f32 value in memory
|
|
;; MR0 = z f32 result
|
|
;;
|
|
;; Benchmark: Cycles = 44
|
|
;; Instructions = 44
|
|
;;
|
|
;; Scratchpad Usage: (Local Function Scratchpad Pointer (SP))
|
|
;;
|
|
;; |_______|<- MR3 (SP+4)
|
|
;; |_______|<- atan2 temporary variable 2 (SP+2)
|
|
;; |_______|<- atan2 temporary variable 1 (SP+0)
|
|
;;
|
|
;;----------------------------------------------------------------------------
|
|
|
|
|
|
.def _CLAatan2
|
|
.sect "Cla1Prog:_CLAatan2"
|
|
.align 2
|
|
.def __cla_CLAatan2_sp
|
|
__cla_CLAatan2_sp .usect ".scratchpad:Cla1Prog:_CLAatan2",6,0,1
|
|
|
|
_CLAatan2:
|
|
.asmfunc
|
|
.asg __cla_CLAatan2_sp + 0, _atan2_tmp1
|
|
.asg __cla_CLAatan2_sp + 2, _atan2_tmp2
|
|
.asg __cla_CLAatan2_sp + 4, _save_MR3
|
|
; Context Save
|
|
MMOV32 @_save_MR3, MR3
|
|
; MR0 = Y(fVal1) and MR1 = X(fVal2) is stored in the
|
|
; scratchpad memory
|
|
MMOV32 @_atan2_tmp1,MR0 ;Y
|
|
MMOV32 @_atan2_tmp2,MR1 ;X
|
|
; Perform Step (1):
|
|
MABSF32 MR3,MR0 ; MR3 = abs(Y)
|
|
MMOV32 MR2,MR3 ; Store abs(Y) in MR2
|
|
MABSF32 MR1,MR1 ; Store abs(X) in MR1
|
|
MMINF32 MR3,MR1 ; MR3 = numerator (A) = min(abs(Y),abs(X))
|
|
MMOV32 MR1,MR2,GT ; MR1 = denominator (B) = max(abs(Y),abs(X))
|
|
; Ratio = A/B
|
|
; Perform Step (2):
|
|
MEINVF32 MR2,MR1 ; MR2 = Ye = Estimate(1/Denominator) i.e 1/B
|
|
MTESTTF LEQ ; Set TF if 1.0 >= abs(X) , this will be used in step 5
|
|
MMPYF32 MR0,MR2,MR1 ; MR0 = Ye*B
|
|
MSUBF32 MR0,#2.0,MR0 ; MR0 = 2.0 - Ye*B
|
|
MMPYF32 MR2,MR2,MR0 ; MR2 = Ye = Ye*(2.0 - Ye*B) (first estimate)
|
|
MMPYF32 MR0,MR2,MR1 ; MR0 = Ye*B
|
|
MSUBF32 MR0,#2.0,MR0 ; MR0 = 2.0 - Ye*B
|
|
MMPYF32 MR2,MR2,MR0 ; MR2 = Ye = Ye*(2.0 - Ye*B) (second estimate)
|
|
MMPYF32 MR0,MR2,MR3 ; MR0 = Ratio = A*Ye = A/B
|
|
; Perform Step (3):
|
|
MMPYF32 MR2,MR0,#64.0 ; 64 = Elements In Table
|
|
MF32TOUI16 MR2,MR2 ; MR2 = int(64*ratio)
|
|
MADD32 MR2,MR2,MR2 ; MR2 = 2*MR2
|
|
MADD32 MR1,MR2,MR2 ; MR1 = 4*MR2
|
|
MADD32 MR2,MR2,MR1 ; MR2 = 6*MR2 this is the index value for the stored data array
|
|
MMOV16 MAR0,MR2,#_CLAatan2Table+4 ; MAR0 points to A2, this will be used in step 4
|
|
MMOVI16 MAR1,#_CLAatan2HalfPITable+2 ; MAR1 points to pi/2, this will be used in step 5
|
|
MNOP
|
|
MNOP
|
|
; Perform Step (4):
|
|
; arctan(Ratio) = A0 + Ratio(A1 + A2*Ratio)
|
|
MMOV32 MR1,*MAR0[#-2]++ ; MR1 = A2
|
|
MMPYF32 MR1,MR1,MR0 ; MR1 = A2*Ratio
|
|
|| MMOV32 MR3,*MAR0[#-2]++ ; MR3 = A1
|
|
MADDF32 MR3,MR3,MR1 ; MR3 = A1 + A2*Ratio
|
|
|| MMOV32 MR1,*MAR0 ; MR1 = A0
|
|
MMPYF32 MR3,MR3,MR0 ; MR3 = Ratio*(A1 + A2*Ratio)
|
|
MADDF32 MR3,MR1,MR3 ; MR3 = A0 + Ratio*(A1 + A2*Ratio)
|
|
|| MMOV32 MR2,@_atan2_tmp2 ; MR2 = X (set/clear NF,ZF for use below)
|
|
; Perform Step (5):
|
|
MMOV32 MR1,*MAR1,UNC ; MR1 = pi/2 (no flag change)
|
|
MNEGF32 MR0,MR1,UNC ; MR0 = -pi/2 (no flag change)
|
|
MMOV32 MR3,MR2,EQ ; if (X == 0), MR3 = 0
|
|
MNEGF32 MR3,MR3,GEQ ; if (X >= 0) MR3 flip sign of atan(Ratio)
|
|
MNEGF32 MR3,MR3,TF ; if (abs(X) >= abs(Y)) flip sign of atan(Ratio)
|
|
|
|
MNEGF32 MR0,MR0,LT ; if (X < 0) MR0 = pi/2
|
|
MADDF32 MR0,MR0,MR1 ; MR0 = MR0+pi/2
|
|
; if(X < 0) MR0 = pi
|
|
; if(X > 0) MR0 = 0
|
|
|| MMOV32 MR2,@_atan2_tmp1 ; MR2 = Y (set/clear NF,ZF)
|
|
MMOV32 MR0,MR1,NTF ; if(abs(X) < abs(Y) R3H = pi/2
|
|
MADDF32 MR3,MR3,MR0 ; MR3 = Angle
|
|
|
|
; Context Restore and Final Operations
|
|
MRCNDD UNC
|
|
MNEGF32 MR3,MR3,LT ; if (Y < 0) Angle = -Angle
|
|
MMOV32 MR0,MR3 ; Store Y = atan2(X)
|
|
MMOV32 MR3,@_save_MR3
|
|
.unasg _atan2_tmp1
|
|
.unasg _atan2_tmp2
|
|
.unasg _save_MR3
|
|
.endasmfunc
|
|
|
|
;; End of File
|