1  /* ----------------------------------------------------------------------
  2   * Project:      CMSIS DSP Library
  3   * Title:        arm_atan2_q31.c
  4   * Description:  float32 Arc tangent of y/x
  5   *
  6   * $Date:        22 April 2022
  7   * $Revision:    V1.10.0
  8   *
  9   * Target Processor: Cortex-M and Cortex-A cores
 10   * -------------------------------------------------------------------- */
 11  /*
 12   * Copyright (C) 2010-2022 ARM Limited or its affiliates. All rights reserved.
 13   *
 14   * SPDX-License-Identifier: Apache-2.0
 15   *
 16   * Licensed under the Apache License, Version 2.0 (the License); you may
 17   * not use this file except in compliance with the License.
 18   * You may obtain a copy of the License at
 19   *
 20   * www.apache.org/licenses/LICENSE-2.0
 21   *
 22   * Unless required by applicable law or agreed to in writing, software
 23   * distributed under the License is distributed on an AS IS BASIS, WITHOUT
 24   * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 25   * See the License for the specific language governing permissions and
 26   * limitations under the License.
 27   */
 28  
 29  #include "dsp/fast_math_functions.h"        
 30  #include "dsp/utils.h"        
 31  
 32  /*
 33  
 34  atan for argument between in [0, 1.0]
 35  
 36  */
 37  
 38  
 39  /* Q2.29 */
 40  #define ATANHALF_Q29 0xed63383
 41  #define PIHALF_Q29 0x3243f6a9
 42  #define PIQ29 0x6487ed51
 43  
 44  #define ATAN2_NB_COEFS_Q31 13
 45  
 46  static const q31_t atan2_coefs_q31[ATAN2_NB_COEFS_Q31]={0x00000000
 47  ,0x7ffffffe
 48  ,0x000001b6
 49  ,0xd555158e
 50  ,0x00036463
 51  ,0x1985f617
 52  ,0x001992ae
 53  ,0xeed53a7f
 54  ,0xf8f15245
 55  ,0x2215a3a4
 56  ,0xe0fab004
 57  ,0x0cdd4825
 58  ,0xfddbc054
 59  };
 60  
 61  
 62  __STATIC_FORCEINLINE q31_t arm_atan_limited_q31(q31_t x)
 63  {
 64      q63_t res=(q63_t)atan2_coefs_q31[ATAN2_NB_COEFS_Q31-1];
 65      int i=1;
 66      for(i=1;i<ATAN2_NB_COEFS_Q31;i++)
 67      {
 68          res = ((q63_t) x * res) >> 31U;
 69          res = res + ((q63_t) atan2_coefs_q31[ATAN2_NB_COEFS_Q31-1-i]) ;
 70      }
 71  
 72      return(clip_q63_to_q31(res>>2));
 73  }
 74  
 75  
 76  __STATIC_FORCEINLINE q31_t arm_atan_q31(q31_t y,q31_t x)
 77  {
 78     int sign=0;
 79     q31_t res=0;
 80  
 81     if (y<0)
 82     {
 83       arm_negate_q31(&y,&y,1);
 84       sign=1-sign;
 85     }
 86  
 87     if (x < 0)
 88     {
 89        sign=1 - sign;
 90        arm_negate_q31(&x,&x,1);
 91     }
 92  
 93     if (y > x)
 94     {
 95      q31_t ratio;
 96      int16_t shift;
 97  
 98      arm_divide_q31(x,y,&ratio,&shift);
 99  
100      arm_shift_q31(&ratio,shift,&ratio,1);
101     
102      res = PIHALF_Q29 - arm_atan_limited_q31(ratio);
103        
104     }
105     else
106     {
107      q31_t ratio;
108      int16_t shift;
109  
110      arm_divide_q31(y,x,&ratio,&shift);
111  
112      arm_shift_q31(&ratio,shift,&ratio,1);
113  
114      res = arm_atan_limited_q31(ratio);
115  
116     }
117  
118  
119     if (sign)
120     {
121       arm_negate_q31(&res,&res,1);
122     }
123  
124     return(res);
125  }
126  
127  
128  /**
129    @ingroup groupFastMath
130   */
131  
132  
133  /**
134    @addtogroup atan2
135    @{
136   */
137  
138  /**
139    @brief       Arc Tangent of y/x using sign of y and x to get right quadrant
140    @param[in]   y  y coordinate
141    @param[in]   x  x coordinate
142    @param[out]  result  Result in Q2.29
143    @return  error status.
144   
145    @par         Compute the Arc tangent of y/x:
146                     The sign of y and x are used to determine the right quadrant
147                     and compute the right angle.
148  */
149  
150  
151  arm_status arm_atan2_q31(q31_t y,q31_t x,q31_t *result)
152  {
153      if (x > 0)
154      {
155          *result=arm_atan_q31(y,x);
156          return(ARM_MATH_SUCCESS);
157      }
158      if (x < 0)
159      {
160          if (y > 0)
161          {
162             *result=arm_atan_q31(y,x) + PIQ29;
163          }
164          else if (y < 0)
165          {
166             *result=arm_atan_q31(y,x) - PIQ29;
167          }
168          else
169          {
170              if (y<0)
171              {
172                 *result= -PIQ29;
173              }
174              else
175              {
176                 *result= PIQ29;
177              }
178          }
179          return(ARM_MATH_SUCCESS);
180      }
181      if (x == 0)
182      {
183          if (y > 0)
184          {
185              *result=PIHALF_Q29;
186              return(ARM_MATH_SUCCESS);
187          }
188          if (y < 0)
189          {
190              *result=-PIHALF_Q29;
191              return(ARM_MATH_SUCCESS);
192          }
193      }
194      
195  
196      return(ARM_MATH_NANINF);
197  
198  }
199  
200  /**
201    @} end of atan2 group
202   */