From 86608c6770cf08c138a2bdab5855072f64be09ef Mon Sep 17 00:00:00 2001 From: joshua Date: Sat, 30 Dec 2023 23:54:31 -0500 Subject: initial commit --- .../DSP/Source/FastMathFunctions/CMakeLists.txt | 51 ++++++++ .../Source/FastMathFunctions/FastMathFunctions.c | 37 ++++++ .../DSP/Source/FastMathFunctions/arm_cos_f32.c | 122 +++++++++++++++++ .../DSP/Source/FastMathFunctions/arm_cos_q15.c | 84 ++++++++++++ .../DSP/Source/FastMathFunctions/arm_cos_q31.c | 84 ++++++++++++ .../DSP/Source/FastMathFunctions/arm_sin_f32.c | 122 +++++++++++++++++ .../DSP/Source/FastMathFunctions/arm_sin_q15.c | 77 +++++++++++ .../DSP/Source/FastMathFunctions/arm_sin_q31.c | 77 +++++++++++ .../DSP/Source/FastMathFunctions/arm_sqrt_q15.c | 144 +++++++++++++++++++++ .../DSP/Source/FastMathFunctions/arm_sqrt_q31.c | 144 +++++++++++++++++++++ 10 files changed, 942 insertions(+) create mode 100644 Drivers/CMSIS/DSP/Source/FastMathFunctions/CMakeLists.txt create mode 100644 Drivers/CMSIS/DSP/Source/FastMathFunctions/FastMathFunctions.c create mode 100644 Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_f32.c create mode 100644 Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q15.c create mode 100644 Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q31.c create mode 100644 Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_f32.c create mode 100644 Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q15.c create mode 100644 Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q31.c create mode 100644 Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q15.c create mode 100644 Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q31.c (limited to 'Drivers/CMSIS/DSP/Source/FastMathFunctions') diff --git a/Drivers/CMSIS/DSP/Source/FastMathFunctions/CMakeLists.txt b/Drivers/CMSIS/DSP/Source/FastMathFunctions/CMakeLists.txt new file mode 100644 index 0000000..c42a05b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FastMathFunctions/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required (VERSION 3.6) + +project(CMSISDSPFastMath) + + +file(GLOB SRC "./*_*.c") + +add_library(CMSISDSPFastMath STATIC) + +include(interpol) +interpol(CMSISDSPFastMath) + +if (CONFIGTABLE AND ALLFAST) + target_compile_definitions(CMSISDSPFastMath PUBLIC ARM_ALL_FAST_TABLES) +endif() + +if (NOT CONFIGTABLE OR ALLFAST OR ARM_COS_F32) +target_sources(CMSISDSPFastMath PRIVATE arm_cos_f32.c) +endif() + +if (NOT CONFIGTABLE OR ALLFAST OR ARM_COS_Q15) +target_sources(CMSISDSPFastMath PRIVATE arm_cos_q15.c) +endif() + +if (NOT CONFIGTABLE OR ALLFAST OR ARM_COS_Q31) +target_sources(CMSISDSPFastMath PRIVATE arm_cos_q31.c) +endif() + +if (NOT CONFIGTABLE OR ALLFAST OR ARM_SIN_F32) +target_sources(CMSISDSPFastMath PRIVATE arm_sin_f32.c) +endif() + +if (NOT CONFIGTABLE OR ALLFAST OR ARM_SIN_Q15) +target_sources(CMSISDSPFastMath PRIVATE arm_sin_q15.c) +endif() + +if (NOT CONFIGTABLE OR ALLFAST OR ARM_SIN_Q31) +target_sources(CMSISDSPFastMath PRIVATE arm_sin_q31.c) +endif() + +target_sources(CMSISDSPFastMath PRIVATE arm_sqrt_q15.c) +target_sources(CMSISDSPFastMath PRIVATE arm_sqrt_q31.c) + + +configdsp(CMSISDSPFastMath ..) + +### Includes +target_include_directories(CMSISDSPFastMath PUBLIC "${DSP}/../../Include") + + + diff --git a/Drivers/CMSIS/DSP/Source/FastMathFunctions/FastMathFunctions.c b/Drivers/CMSIS/DSP/Source/FastMathFunctions/FastMathFunctions.c new file mode 100644 index 0000000..a856678 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FastMathFunctions/FastMathFunctions.c @@ -0,0 +1,37 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: FastMathFunctions.c + * Description: Combination of all fast math function source files. + * + * $Date: 18. March 2019 + * $Revision: V1.0.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2019 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 "arm_cos_f32.c" +#include "arm_cos_q15.c" +#include "arm_cos_q31.c" +#include "arm_sin_f32.c" +#include "arm_sin_q15.c" +#include "arm_sin_q31.c" +#include "arm_sqrt_q15.c" +#include "arm_sqrt_q31.c" + diff --git a/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_f32.c b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_f32.c new file mode 100644 index 0000000..78207d8 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_f32.c @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cos_f32.c + * Description: Fast cosine calculation for floating-point values + * + * $Date: 18. March 2019 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2019 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 "arm_math.h" +#include "arm_common_tables.h" + +/** + @ingroup groupFastMath + */ + +/** + @defgroup cos Cosine + + Computes the trigonometric cosine function using a combination of table lookup + and linear interpolation. There are separate functions for + Q15, Q31, and floating-point data types. + The input to the floating-point version is in radians while the + fixed-point Q15 and Q31 have a scaled input with the range + [0 +0.9999] mapping to [0 2*pi). The fixed-point range is chosen so that a + value of 2*pi wraps around to 0. + + The implementation is based on table lookup using 256 values together with linear interpolation. + The steps used are: + -# Calculation of the nearest integer table index + -# Compute the fractional portion (fract) of the table index. + -# The final result equals (1.0f-fract)*a + fract*b; + + where + 
+     b = Table[index];
+     c = Table[index+1];
+
+ */ + +/** + @addtogroup cos + @{ + */ + +/** + @brief Fast approximation to the trigonometric cosine function for floating-point data. + @param[in] x input value in radians + @return cos(x) + */ + +float32_t arm_cos_f32( + float32_t x) +{ + float32_t cosVal, fract, in; /* Temporary input, output variables */ + uint16_t index; /* Index variable */ + float32_t a, b; /* Two nearest output values */ + int32_t n; + float32_t findex; + + /* input x is in radians */ + /* Scale input to [0 1] range from [0 2*PI] , divide input by 2*pi, add 0.25 (pi/2) to read sine table */ + in = x * 0.159154943092f + 0.25f; + + /* Calculation of floor value of input */ + n = (int32_t) in; + + /* Make negative values towards -infinity */ + if (in < 0.0f) + { + n--; + } + + /* Map input value to [0 1] */ + in = in - (float32_t) n; + + /* Calculation of index of the table */ + findex = (float32_t)FAST_MATH_TABLE_SIZE * in; + index = (uint16_t)findex; + + /* when "in" is exactly 1, we need to rotate the index down to 0 */ + if (index >= FAST_MATH_TABLE_SIZE) { + index = 0; + findex -= (float32_t)FAST_MATH_TABLE_SIZE; + } + + /* fractional value calculation */ + fract = findex - (float32_t) index; + + /* Read two nearest values of input value from the cos table */ + a = sinTable_f32[index]; + b = sinTable_f32[index+1]; + + /* Linear interpolation process */ + cosVal = (1.0f - fract) * a + fract * b; + + /* Return output value */ + return (cosVal); +} + +/** + @} end of cos group + */ diff --git a/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q15.c b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q15.c new file mode 100644 index 0000000..aef47da --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q15.c @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cos_q15.c + * Description: Fast cosine calculation for Q15 values + * + * $Date: 18. March 2019 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2019 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 "arm_math.h" +#include "arm_common_tables.h" + +/** + @ingroup groupFastMath + */ + +/** + @addtogroup cos + @{ + */ + +/** + @brief Fast approximation to the trigonometric cosine function for Q15 data. + @param[in] x Scaled input value in radians + @return cos(x) + + The Q15 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*PI). + */ + +q15_t arm_cos_q15( + q15_t x) +{ + q15_t cosVal; /* Temporary input, output variables */ + int32_t index; /* Index variable */ + q15_t a, b; /* Two nearest output values */ + q15_t fract; /* Temporary values for fractional values */ + + /* add 0.25 (pi/2) to read sine table */ + x = (uint16_t)x + 0x2000; + if (x < 0) + { /* convert negative numbers to corresponding positive ones */ + x = (uint16_t)x + 0x8000; + } + + /* Calculate the nearest index */ + index = (uint32_t)x >> FAST_MATH_Q15_SHIFT; + + /* Calculation of fractional value */ + fract = (x - (index << FAST_MATH_Q15_SHIFT)) << 9; + + /* Read two nearest values of input value from the sin table */ + a = sinTable_q15[index]; + b = sinTable_q15[index+1]; + + /* Linear interpolation process */ + cosVal = (q31_t) (0x8000 - fract) * a >> 16; + cosVal = (q15_t) ((((q31_t) cosVal << 16) + ((q31_t) fract * b)) >> 16); + + /* Return output value */ + return (cosVal << 1); +} + +/** + @} end of cos group + */ diff --git a/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q31.c b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q31.c new file mode 100644 index 0000000..fa42eee --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q31.c @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cos_q31.c + * Description: Fast cosine calculation for Q31 values + * + * $Date: 18. March 2019 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2019 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 "arm_math.h" +#include "arm_common_tables.h" + +/** + @ingroup groupFastMath + */ + +/** + @addtogroup cos + @{ + */ + +/** + @brief Fast approximation to the trigonometric cosine function for Q31 data. + @param[in] x Scaled input value in radians + @return cos(x) + + The Q31 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*PI). + */ + +q31_t arm_cos_q31( + q31_t x) +{ + q31_t cosVal; /* Temporary input, output variables */ + int32_t index; /* Index variable */ + q31_t a, b; /* Two nearest output values */ + q31_t fract; /* Temporary values for fractional values */ + + /* add 0.25 (pi/2) to read sine table */ + x = (uint32_t)x + 0x20000000; + if (x < 0) + { /* convert negative numbers to corresponding positive ones */ + x = (uint32_t)x + 0x80000000; + } + + /* Calculate the nearest index */ + index = (uint32_t)x >> FAST_MATH_Q31_SHIFT; + + /* Calculation of fractional value */ + fract = (x - (index << FAST_MATH_Q31_SHIFT)) << 9; + + /* Read two nearest values of input value from the sin table */ + a = sinTable_q31[index]; + b = sinTable_q31[index+1]; + + /* Linear interpolation process */ + cosVal = (q63_t) (0x80000000 - fract) * a >> 32; + cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) fract * b)) >> 32); + + /* Return output value */ + return (cosVal << 1); +} + +/** + @} end of cos group + */ diff --git a/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_f32.c b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_f32.c new file mode 100644 index 0000000..674765e --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_f32.c @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sin_f32.c + * Description: Fast sine calculation for floating-point values + * + * $Date: 18. March 2019 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2019 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 "arm_math.h" +#include "arm_common_tables.h" + +/** + @ingroup groupFastMath + */ + +/** + @defgroup sin Sine + + Computes the trigonometric sine function using a combination of table lookup + and linear interpolation. There are separate functions for + Q15, Q31, and floating-point data types. + The input to the floating-point version is in radians while the + fixed-point Q15 and Q31 have a scaled input with the range + [0 +0.9999] mapping to [0 2*pi). The fixed-point range is chosen so that a + value of 2*pi wraps around to 0. + + The implementation is based on table lookup using 256 values together with linear interpolation. + The steps used are: + -# Calculation of the nearest integer table index + -# Compute the fractional portion (fract) of the table index. + -# The final result equals (1.0f-fract)*a + fract*b; + + where + 
+     b = Table[index];
+     c = Table[index+1];
+
+ */ + +/** + @addtogroup sin + @{ + */ + +/** + @brief Fast approximation to the trigonometric sine function for floating-point data. + @param[in] x input value in radians. + @return sin(x) + */ + +float32_t arm_sin_f32( + float32_t x) +{ + float32_t sinVal, fract, in; /* Temporary input, output variables */ + uint16_t index; /* Index variable */ + float32_t a, b; /* Two nearest output values */ + int32_t n; + float32_t findex; + + /* input x is in radians */ + /* Scale input to [0 1] range from [0 2*PI] , divide input by 2*pi */ + in = x * 0.159154943092f; + + /* Calculation of floor value of input */ + n = (int32_t) in; + + /* Make negative values towards -infinity */ + if (in < 0.0f) + { + n--; + } + + /* Map input value to [0 1] */ + in = in - (float32_t) n; + + /* Calculation of index of the table */ + findex = (float32_t)FAST_MATH_TABLE_SIZE * in; + index = (uint16_t)findex; + + /* when "in" is exactly 1, we need to rotate the index down to 0 */ + if (index >= FAST_MATH_TABLE_SIZE) { + index = 0; + findex -= (float32_t)FAST_MATH_TABLE_SIZE; + } + + /* fractional value calculation */ + fract = findex - (float32_t) index; + + /* Read two nearest values of input value from the sin table */ + a = sinTable_f32[index]; + b = sinTable_f32[index+1]; + + /* Linear interpolation process */ + sinVal = (1.0f - fract) * a + fract * b; + + /* Return output value */ + return (sinVal); +} + +/** + @} end of sin group + */ diff --git a/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q15.c b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q15.c new file mode 100644 index 0000000..edc8b38 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q15.c @@ -0,0 +1,77 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sin_q15.c + * Description: Fast sine calculation for Q15 values + * + * $Date: 18. March 2019 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2019 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 "arm_math.h" +#include "arm_common_tables.h" + +/** + @ingroup groupFastMath + */ + +/** + @addtogroup sin + @{ + */ + +/** + @brief Fast approximation to the trigonometric sine function for Q15 data. + @param[in] x Scaled input value in radians + @return sin(x) + + The Q15 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*PI). + */ + +q15_t arm_sin_q15( + q15_t x) +{ + q15_t sinVal; /* Temporary input, output variables */ + int32_t index; /* Index variable */ + q15_t a, b; /* Two nearest output values */ + q15_t fract; /* Temporary values for fractional values */ + + /* Calculate the nearest index */ + index = (uint32_t)x >> FAST_MATH_Q15_SHIFT; + + /* Calculation of fractional value */ + fract = (x - (index << FAST_MATH_Q15_SHIFT)) << 9; + + /* Read two nearest values of input value from the sin table */ + a = sinTable_q15[index]; + b = sinTable_q15[index+1]; + + /* Linear interpolation process */ + sinVal = (q31_t) (0x8000 - fract) * a >> 16; + sinVal = (q15_t) ((((q31_t) sinVal << 16) + ((q31_t) fract * b)) >> 16); + + /* Return output value */ + return (sinVal << 1); +} + +/** + @} end of sin group + */ diff --git a/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q31.c b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q31.c new file mode 100644 index 0000000..849cc11 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q31.c @@ -0,0 +1,77 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sin_q31.c + * Description: Fast sine calculation for Q31 values + * + * $Date: 18. March 2019 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2019 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 "arm_math.h" +#include "arm_common_tables.h" + +/** + @ingroup groupFastMath + */ + +/** + @addtogroup sin + @{ + */ + +/** + @brief Fast approximation to the trigonometric sine function for Q31 data. + @param[in] x Scaled input value in radians + @return sin(x) + + The Q31 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*PI). + */ + +q31_t arm_sin_q31( + q31_t x) +{ + q31_t sinVal; /* Temporary variables for input, output */ + int32_t index; /* Index variable */ + q31_t a, b; /* Two nearest output values */ + q31_t fract; /* Temporary values for fractional values */ + + /* Calculate the nearest index */ + index = (uint32_t)x >> FAST_MATH_Q31_SHIFT; + + /* Calculation of fractional value */ + fract = (x - (index << FAST_MATH_Q31_SHIFT)) << 9; + + /* Read two nearest values of input value from the sin table */ + a = sinTable_q31[index]; + b = sinTable_q31[index+1]; + + /* Linear interpolation process */ + sinVal = (q63_t) (0x80000000 - fract) * a >> 32; + sinVal = (q31_t) ((((q63_t) sinVal << 32) + ((q63_t) fract * b)) >> 32); + + /* Return output value */ + return (sinVal << 1); +} + +/** + @} end of sin group + */ diff --git a/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q15.c b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q15.c new file mode 100644 index 0000000..1dc7bc4 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q15.c @@ -0,0 +1,144 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sqrt_q15.c + * Description: Q15 square root function + * + * $Date: 18. March 2019 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2019 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 "arm_math.h" +#include "arm_common_tables.h" + +/** + @ingroup groupFastMath + */ + +/** + @addtogroup SQRT + @{ + */ + +/** + @brief Q15 square root function. + @param[in] in input value. The range of the input value is [0 +1) or 0x0000 to 0x7FFF + @param[out] pOut points to square root of input value + @return execution status + - \ref ARM_MATH_SUCCESS : input value is positive + - \ref ARM_MATH_ARGUMENT_ERROR : input value is negative; *pOut is set to 0 + */ + +arm_status arm_sqrt_q15( + q15_t in, + q15_t * pOut) +{ + q31_t bits_val1; + q15_t number, temp1, var1, signBits1, half; + float32_t temp_float1; + union + { + q31_t fracval; + float32_t floatval; + } tempconv; + + number = in; + + /* If the input is a positive number then compute the signBits. */ + if (number > 0) + { + signBits1 = __CLZ(number) - 17; + + /* Shift by the number of signBits1 */ + if ((signBits1 % 2) == 0) + { + number = number << signBits1; + } + else + { + number = number << (signBits1 - 1); + } + + /* Calculate half value of the number */ + half = number >> 1; + /* Store the number for later use */ + temp1 = number; + + /* Convert to float */ + temp_float1 = number * 3.051757812500000e-005f; + /* Store as integer */ + tempconv.floatval = temp_float1; + bits_val1 = tempconv.fracval; + /* Subtract the shifted value from the magic number to give intial guess */ + bits_val1 = 0x5f3759df - (bits_val1 >> 1); /* gives initial guess */ + /* Store as float */ + tempconv.fracval = bits_val1; + temp_float1 = tempconv.floatval; + /* Convert to integer format */ + var1 = (q31_t) (temp_float1 * 16384); + + /* 1st iteration */ + var1 = ((q15_t) ((q31_t) var1 * (0x3000 - + ((q15_t) + ((((q15_t) + (((q31_t) var1 * var1) >> 15)) * + (q31_t) half) >> 15))) >> 15)) << 2; + /* 2nd iteration */ + var1 = ((q15_t) ((q31_t) var1 * (0x3000 - + ((q15_t) + ((((q15_t) + (((q31_t) var1 * var1) >> 15)) * + (q31_t) half) >> 15))) >> 15)) << 2; + /* 3rd iteration */ + var1 = ((q15_t) ((q31_t) var1 * (0x3000 - + ((q15_t) + ((((q15_t) + (((q31_t) var1 * var1) >> 15)) * + (q31_t) half) >> 15))) >> 15)) << 2; + + /* Multiply the inverse square root with the original value */ + var1 = ((q15_t) (((q31_t) temp1 * var1) >> 15)) << 1; + + /* Shift the output down accordingly */ + if ((signBits1 % 2) == 0) + { + var1 = var1 >> (signBits1 / 2); + } + else + { + var1 = var1 >> ((signBits1 - 1) / 2); + } + *pOut = var1; + + return (ARM_MATH_SUCCESS); + } + /* If the number is a negative number then store zero as its square root value */ + else + { + *pOut = 0; + + return (ARM_MATH_ARGUMENT_ERROR); + } +} + +/** + @} end of SQRT group + */ diff --git a/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q31.c b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q31.c new file mode 100644 index 0000000..9f2a731 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q31.c @@ -0,0 +1,144 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sqrt_q31.c + * Description: Q31 square root function + * + * $Date: 18. March 2019 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2019 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 "arm_math.h" +#include "arm_common_tables.h" + +/** + @ingroup groupFastMath + */ + +/** + @addtogroup SQRT + @{ + */ + +/** + @brief Q31 square root function. + @param[in] in input value. The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF + @param[out] pOut points to square root of input value + @return execution status + - \ref ARM_MATH_SUCCESS : input value is positive + - \ref ARM_MATH_ARGUMENT_ERROR : input value is negative; *pOut is set to 0 + */ + +arm_status arm_sqrt_q31( + q31_t in, + q31_t * pOut) +{ + q31_t bits_val1; + q31_t number, temp1, var1, signBits1, half; + float32_t temp_float1; + union + { + q31_t fracval; + float32_t floatval; + } tempconv; + + number = in; + + /* If the input is a positive number then compute the signBits. */ + if (number > 0) + { + signBits1 = __CLZ(number) - 1; + + /* Shift by the number of signBits1 */ + if ((signBits1 % 2) == 0) + { + number = number << signBits1; + } + else + { + number = number << (signBits1 - 1); + } + + /* Calculate half value of the number */ + half = number >> 1; + /* Store the number for later use */ + temp1 = number; + + /* Convert to float */ + temp_float1 = number * 4.6566128731e-010f; + /* Store as integer */ + tempconv.floatval = temp_float1; + bits_val1 = tempconv.fracval; + /* Subtract the shifted value from the magic number to give intial guess */ + bits_val1 = 0x5f3759df - (bits_val1 >> 1); /* gives initial guess */ + /* Store as float */ + tempconv.fracval = bits_val1; + temp_float1 = tempconv.floatval; + /* Convert to integer format */ + var1 = (q31_t) (temp_float1 * 1073741824); + + /* 1st iteration */ + var1 = ((q31_t) ((q63_t) var1 * (0x30000000 - + ((q31_t) + ((((q31_t) + (((q63_t) var1 * var1) >> 31)) * + (q63_t) half) >> 31))) >> 31)) << 2; + /* 2nd iteration */ + var1 = ((q31_t) ((q63_t) var1 * (0x30000000 - + ((q31_t) + ((((q31_t) + (((q63_t) var1 * var1) >> 31)) * + (q63_t) half) >> 31))) >> 31)) << 2; + /* 3rd iteration */ + var1 = ((q31_t) ((q63_t) var1 * (0x30000000 - + ((q31_t) + ((((q31_t) + (((q63_t) var1 * var1) >> 31)) * + (q63_t) half) >> 31))) >> 31)) << 2; + + /* Multiply the inverse square root with the original value */ + var1 = ((q31_t) (((q63_t) temp1 * var1) >> 31)) << 1; + + /* Shift the output down accordingly */ + if ((signBits1 % 2) == 0) + { + var1 = var1 >> (signBits1 / 2); + } + else + { + var1 = var1 >> ((signBits1 - 1) / 2); + } + *pOut = var1; + + return (ARM_MATH_SUCCESS); + } + /* If the number is a negative number then store zero as its square root value */ + else + { + *pOut = 0; + + return (ARM_MATH_ARGUMENT_ERROR); + } +} + +/** + @} end of SQRT group + */ -- cgit v1.2.3