| /* ---------------------------------------------------------------------- |
| * 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 |
| */ |