172 lines
3.7 KiB
C
172 lines
3.7 KiB
C
/* ----------------------------------------------------------------------
|
|
* Project: CMSIS DSP Library
|
|
* Title: arm_atan2_f16.c
|
|
* Description: float16 Arc tangent of y/x
|
|
*
|
|
* $Date: 22 April 2022
|
|
* $Revision: V1.10.0
|
|
*
|
|
* Target Processor: Cortex-M and Cortex-A cores
|
|
* -------------------------------------------------------------------- */
|
|
/*
|
|
* Copyright (C) 2010-2022 ARM Limited or its affiliates. All rights reserved.
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the License); you may
|
|
* not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
|
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#include "dsp/fast_math_functions_f16.h"
|
|
|
|
#if defined(ARM_FLOAT16_SUPPORTED)
|
|
|
|
/*
|
|
|
|
atan for argument between in [0, 1.0]
|
|
|
|
|
|
*/
|
|
|
|
#define PIF16 3.14f16
|
|
#define PI16HALF 1.571f16
|
|
|
|
#define ATANHALFF16 0.463648f16
|
|
|
|
#define ATAN2_NB_COEFS_F16 5
|
|
|
|
static const float16_t atan2_coefs_f16[ATAN2_NB_COEFS_F16]={0.f16
|
|
,1.f16
|
|
,0.f16
|
|
,-0.367f16
|
|
,0.152f16
|
|
};
|
|
|
|
__STATIC_FORCEINLINE float16_t arm_atan_limited_f16(float16_t x)
|
|
{
|
|
float16_t res=atan2_coefs_f16[ATAN2_NB_COEFS_F16-1];
|
|
int i=1;
|
|
for(i=1;i<ATAN2_NB_COEFS_F16;i++)
|
|
{
|
|
res = (_Float16)x*(_Float16)res + (_Float16)atan2_coefs_f16[ATAN2_NB_COEFS_F16-1-i];
|
|
}
|
|
|
|
|
|
return(res);
|
|
}
|
|
|
|
__STATIC_FORCEINLINE float16_t arm_atan_f16(float16_t x)
|
|
{
|
|
int sign=0;
|
|
float16_t res=0.0f16;
|
|
|
|
if ((_Float16)x < 0.0f16)
|
|
{
|
|
sign=1;
|
|
x=-(_Float16)x;
|
|
}
|
|
|
|
if ((_Float16)x > 1.0f16)
|
|
{
|
|
x = 1.0f16 / (_Float16)x;
|
|
res = (_Float16)PI16HALF - (_Float16)arm_atan_limited_f16(x);
|
|
}
|
|
else
|
|
{
|
|
res += (_Float16)arm_atan_limited_f16(x);
|
|
}
|
|
|
|
|
|
if (sign)
|
|
{
|
|
res = -(_Float16)res;
|
|
}
|
|
|
|
return(res);
|
|
}
|
|
|
|
/**
|
|
@ingroup groupFastMath
|
|
*/
|
|
|
|
|
|
/**
|
|
@addtogroup atan2
|
|
@{
|
|
*/
|
|
|
|
/**
|
|
@brief Arc Tangent of y/x using sign of y and x to get right quadrant
|
|
@param[in] y y coordinate
|
|
@param[in] x x coordinate
|
|
@param[out] result Result
|
|
@return error status.
|
|
|
|
@par Compute the Arc tangent of y/x:
|
|
The sign of y and x are used to determine the right quadrant
|
|
and compute the right angle.
|
|
|
|
*/
|
|
arm_status arm_atan2_f16(float16_t y,float16_t x,float16_t *result)
|
|
{
|
|
if ((_Float16)x > 0.0f16)
|
|
{
|
|
*result=arm_atan_f16((_Float16)y/(_Float16)x);
|
|
return(ARM_MATH_SUCCESS);
|
|
}
|
|
if ((_Float16)x < 0.0f16)
|
|
{
|
|
if ((_Float16)y > 0.0f16)
|
|
{
|
|
*result=(_Float16)arm_atan_f16((_Float16)y/(_Float16)x) + (_Float16)PIF16;
|
|
}
|
|
else if ((_Float16)y < 0.0f16)
|
|
{
|
|
*result=(_Float16)arm_atan_f16((_Float16)y/(_Float16)x) - (_Float16)PIF16;
|
|
}
|
|
else
|
|
{
|
|
if (signbit(y))
|
|
{
|
|
*result= -(_Float16)PIF16;
|
|
}
|
|
else
|
|
{
|
|
*result= PIF16;
|
|
}
|
|
}
|
|
return(ARM_MATH_SUCCESS);
|
|
}
|
|
if ((_Float16)x == 0.0f16)
|
|
{
|
|
if ((_Float16)y > 0.0f16)
|
|
{
|
|
*result=PI16HALF;
|
|
return(ARM_MATH_SUCCESS);
|
|
}
|
|
if ((_Float16)y < 0.0f16)
|
|
{
|
|
*result=-(_Float16)PI16HALF;
|
|
return(ARM_MATH_SUCCESS);
|
|
}
|
|
}
|
|
|
|
|
|
return(ARM_MATH_NANINF);
|
|
|
|
}
|
|
|
|
#endif /* #if defined(ARM_FLOAT16_SUPPORTED) */
|
|
/**
|
|
@} end of atan2 group
|
|
*/
|