arm_sqrt_q31.c

00001 /* ----------------------------------------------------------------------
00002  * Project:      CMSIS DSP Library
00003  * Title:        arm_sqrt_q31.c
00004  * Description:  Q31 square root function
00005  *
00006  * $Date:        27. January 2017
00007  * $Revision:    V.1.5.1
00008  *
00009  * Target Processor: Cortex-M cores
00010  * -------------------------------------------------------------------- */
00011 /*
00012  * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
00013  *
00014  * SPDX-License-Identifier: Apache-2.0
00015  *
00016  * Licensed under the Apache License, Version 2.0 (the License); you may
00017  * not use this file except in compliance with the License.
00018  * You may obtain a copy of the License at
00019  *
00020  * www.apache.org/licenses/LICENSE-2.0
00021  *
00022  * Unless required by applicable law or agreed to in writing, software
00023  * distributed under the License is distributed on an AS IS BASIS, WITHOUT
00024  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00025  * See the License for the specific language governing permissions and
00026  * limitations under the License.
00027  */
00028 
00029 #include "arm_math.h"
00030 #include "arm_common_tables.h"
00031 
00032 /**
00033  * @ingroup groupFastMath
00034  */
00035 
00036 /**
00037  * @addtogroup SQRT
00038  * @{
00039  */
00040 
00041 /**
00042  * @brief Q31 square root function.
00043  * @param[in]   in    input value.  The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF.
00044  * @param[out]  *pOut square root of input value.
00045  * @return The function returns ARM_MATH_SUCCESS if the input value is positive
00046  * and ARM_MATH_ARGUMENT_ERROR if the input is negative.  For
00047  * negative inputs, the function returns *pOut = 0.
00048  */
00049 
00050 arm_status arm_sqrt_q31(
00051   q31_t in,
00052   q31_t * pOut)
00053 {
00054   q31_t number, temp1, bits_val1, var1, signBits1, half;
00055   float32_t temp_float1;
00056   union
00057   {
00058       q31_t fracval;
00059       float32_t floatval;
00060   } tempconv;
00061 
00062   number = in;
00063 
00064   /* If the input is a positive number then compute the signBits. */
00065   if (number > 0)
00066   {
00067     signBits1 = __CLZ(number) - 1;
00068 
00069     /* Shift by the number of signBits1 */
00070     if ((signBits1 % 2) == 0)
00071     {
00072       number = number << signBits1;
00073     }
00074     else
00075     {
00076       number = number << (signBits1 - 1);
00077     }
00078 
00079     /* Calculate half value of the number */
00080     half = number >> 1;
00081     /* Store the number for later use */
00082     temp1 = number;
00083 
00084     /*Convert to float */
00085     temp_float1 = number * 4.6566128731e-010f;
00086     /*Store as integer */
00087     tempconv.floatval = temp_float1;
00088     bits_val1 = tempconv.fracval;
00089     /* Subtract the shifted value from the magic number to give intial guess */
00090     bits_val1 = 0x5f3759df - (bits_val1 >> 1);  /* gives initial guess */
00091     /* Store as float */
00092     tempconv.fracval = bits_val1;
00093     temp_float1 = tempconv.floatval;
00094     /* Convert to integer format */
00095     var1 = (q31_t) (temp_float1 * 1073741824);
00096 
00097     /* 1st iteration */
00098     var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
00099                                      ((q31_t)
00100                                       ((((q31_t)
00101                                          (((q63_t) var1 * var1) >> 31)) *
00102                                         (q63_t) half) >> 31))) >> 31)) << 2;
00103     /* 2nd iteration */
00104     var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
00105                                      ((q31_t)
00106                                       ((((q31_t)
00107                                          (((q63_t) var1 * var1) >> 31)) *
00108                                         (q63_t) half) >> 31))) >> 31)) << 2;
00109     /* 3rd iteration */
00110     var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
00111                                      ((q31_t)
00112                                       ((((q31_t)
00113                                          (((q63_t) var1 * var1) >> 31)) *
00114                                         (q63_t) half) >> 31))) >> 31)) << 2;
00115 
00116     /* Multiply the inverse square root with the original value */
00117     var1 = ((q31_t) (((q63_t) temp1 * var1) >> 31)) << 1;
00118 
00119     /* Shift the output down accordingly */
00120     if ((signBits1 % 2) == 0)
00121     {
00122       var1 = var1 >> (signBits1 / 2);
00123     }
00124     else
00125     {
00126       var1 = var1 >> ((signBits1 - 1) / 2);
00127     }
00128     *pOut = var1;
00129 
00130     return (ARM_MATH_SUCCESS);
00131   }
00132   /* If the number is a negative number then store zero as its square root value */
00133   else
00134   {
00135     *pOut = 0;
00136     return (ARM_MATH_ARGUMENT_ERROR);
00137   }
00138 }
00139 
00140 /**
00141  * @} end of SQRT group
00142  */
00143