diff options
Diffstat (limited to 'fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions')
8 files changed, 843 insertions, 0 deletions
diff --git a/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_f32.c b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_f32.c new file mode 100644 index 0000000..44efbd5 --- /dev/null +++ b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_f32.c @@ -0,0 +1,115 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_cos_f32.c
+ * Description: Fast cosine calculation for floating-point values
+ *
+ * $Date: 27. January 2017
+ * $Revision: V.1.5.1
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2017 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 and in the range [0 2*pi) 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 <code>(1.0f-fract)*a + fract*b;</code>
+ *
+ * where
+ * <pre>
+ * b=Table[index+0];
+ * c=Table[index+1];
+ * </pre>
+ */
+
+ /**
+ * @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 variables for input, output */
+ 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 the 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) & 0x1ff;
+
+ /* 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 the output value */
+ return (cosVal);
+}
+
+/**
+ * @} end of cos group
+ */
diff --git a/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q15.c b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q15.c new file mode 100644 index 0000000..036c5d7 --- /dev/null +++ b/fw/cdc-dials/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: 27. January 2017
+ * $Revision: V.1.5.1
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2017 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 variables for input, output */
+ int32_t index; /* Index variables */
+ q15_t a, b; /* Four 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 cosVal << 1;
+}
+
+/**
+ * @} end of cos group
+ */
diff --git a/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q31.c b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_cos_q31.c new file mode 100644 index 0000000..105addb --- /dev/null +++ b/fw/cdc-dials/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: 27. January 2017
+ * $Revision: V.1.5.1
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2017 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 variables for input, output */
+ int32_t index; /* Index variables */
+ q31_t a, b; /* Four 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 cosVal << 1;
+}
+
+/**
+ * @} end of cos group
+ */
diff --git a/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_f32.c b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_f32.c new file mode 100644 index 0000000..3fb5153 --- /dev/null +++ b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_f32.c @@ -0,0 +1,123 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_sin_f32.c
+ * Description: Fast sine calculation for floating-point values
+ *
+ * $Date: 27. January 2017
+ * $Revision: V.1.5.1
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2017 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"
+#include <math.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 and in the range [0 2*pi) 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 <code>(1.0f-fract)*a + fract*b;</code>
+ *
+ * where
+ * <pre>
+ * b=Table[index+0];
+ * c=Table[index+1];
+ * </pre>
+ */
+
+/**
+ * @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 variables for input, output */
+ uint16_t index; /* Index variable */
+ float32_t a, b; /* Two nearest output values */
+ int32_t n;
+ float32_t findex;
+
+ /* Special case for small negative inputs */
+ if ((x < 0.0f) && (x >= -1.9e-7f)) {
+ return x;
+ }
+
+ /* input x is in radians */
+ /* Scale the 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 (x < 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) & 0x1ff;
+
+ /* 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 the output value */
+ return (sinVal);
+}
+
+/**
+ * @} end of sin group
+ */
diff --git a/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q15.c b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q15.c new file mode 100644 index 0000000..9eecaa9 --- /dev/null +++ b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q15.c @@ -0,0 +1,76 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_sin_q15.c
+ * Description: Fast sine calculation for Q15 values
+ *
+ * $Date: 27. January 2017
+ * $Revision: V.1.5.1
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2017 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 variables for input, output */
+ int32_t index; /* Index variables */
+ q15_t a, b; /* Four 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 sinVal << 1;
+}
+
+/**
+ * @} end of sin group
+ */
diff --git a/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q31.c b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q31.c new file mode 100644 index 0000000..2119016 --- /dev/null +++ b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sin_q31.c @@ -0,0 +1,75 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_sin_q31.c
+ * Description: Fast sine calculation for Q31 values
+ *
+ * $Date: 27. January 2017
+ * $Revision: V.1.5.1
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2017 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 variables */
+ q31_t a, b; /* Four 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 sinVal << 1;
+}
+
+/**
+ * @} end of sin group
+ */
diff --git a/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q15.c b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q15.c new file mode 100644 index 0000000..83e4ddd --- /dev/null +++ b/fw/cdc-dials/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: 27. January 2017
+ * $Revision: V.1.5.1
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2017 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 square root of input value.
+ * @return The function returns ARM_MATH_SUCCESS if the input value is positive
+ * and ARM_MATH_ARGUMENT_ERROR if the input is negative. For
+ * negative inputs, the function returns *pOut = 0.
+ */
+
+arm_status arm_sqrt_q15(
+ q15_t in,
+ q15_t * pOut)
+{
+ q15_t number, temp1, var1, signBits1, half;
+ q31_t bits_val1;
+ 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/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q31.c b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q31.c new file mode 100644 index 0000000..de8c35f --- /dev/null +++ b/fw/cdc-dials/Drivers/CMSIS/DSP/Source/FastMathFunctions/arm_sqrt_q31.c @@ -0,0 +1,142 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_sqrt_q31.c
+ * Description: Q31 square root function
+ *
+ * $Date: 27. January 2017
+ * $Revision: V.1.5.1
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2017 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 square root of input value.
+ * @return The function returns ARM_MATH_SUCCESS if the input value is positive
+ * and ARM_MATH_ARGUMENT_ERROR if the input is negative. For
+ * negative inputs, the function returns *pOut = 0.
+ */
+
+arm_status arm_sqrt_q31(
+ q31_t in,
+ q31_t * pOut)
+{
+ q31_t number, temp1, bits_val1, 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
+ */
|