arm_sqrt_q15.c

00001 /* ----------------------------------------------------------------------
00002  * Project:      CMSIS DSP Library
00003  * Title:        arm_sqrt_q15.c
00004  * Description:  Q15 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 /**
00034  * @ingroup groupFastMath
00035  */
00036 
00037 /**
00038  * @addtogroup SQRT
00039  * @{
00040  */
00041 
00042   /**
00043    * @brief  Q15 square root function.
00044    * @param[in]   in     input value.  The range of the input value is [0 +1) or 0x0000 to 0x7FFF.
00045    * @param[out]  *pOut  square root of input value.
00046    * @return The function returns ARM_MATH_SUCCESS if the input value is positive
00047    * and ARM_MATH_ARGUMENT_ERROR if the input is negative.  For
00048    * negative inputs, the function returns *pOut = 0.
00049    */
00050 
00051 arm_status arm_sqrt_q15(
00052   q15_t in,
00053   q15_t * pOut)
00054 {
00055   q15_t number, temp1, var1, signBits1, half;
00056   q31_t bits_val1;
00057   float32_t temp_float1;
00058   union
00059   {
00060     q31_t fracval;
00061     float32_t floatval;
00062   } tempconv;
00063 
00064   number = in;
00065 
00066   /* If the input is a positive number then compute the signBits. */
00067   if (number > 0)
00068   {
00069     signBits1 = __CLZ(number) - 17;
00070 
00071     /* Shift by the number of signBits1 */
00072     if ((signBits1 % 2) == 0)
00073     {
00074       number = number << signBits1;
00075     }
00076     else
00077     {
00078       number = number << (signBits1 - 1);
00079     }
00080 
00081     /* Calculate half value of the number */
00082     half = number >> 1;
00083     /* Store the number for later use */
00084     temp1 = number;
00085 
00086     /* Convert to float */
00087     temp_float1 = number * 3.051757812500000e-005f;
00088     /*Store as integer */
00089     tempconv.floatval = temp_float1;
00090     bits_val1 = tempconv.fracval;
00091     /* Subtract the shifted value from the magic number to give intial guess */
00092     bits_val1 = 0x5f3759df - (bits_val1 >> 1);  /* gives initial guess */
00093     /* Store as float */
00094     tempconv.fracval = bits_val1;
00095     temp_float1 = tempconv.floatval;
00096     /* Convert to integer format */
00097     var1 = (q31_t) (temp_float1 * 16384);
00098 
00099     /* 1st iteration */
00100     var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
00101                                      ((q15_t)
00102                                       ((((q15_t)
00103                                          (((q31_t) var1 * var1) >> 15)) *
00104                                         (q31_t) half) >> 15))) >> 15)) << 2;
00105     /* 2nd iteration */
00106     var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
00107                                      ((q15_t)
00108                                       ((((q15_t)
00109                                          (((q31_t) var1 * var1) >> 15)) *
00110                                         (q31_t) half) >> 15))) >> 15)) << 2;
00111     /* 3rd iteration */
00112     var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
00113                                      ((q15_t)
00114                                       ((((q15_t)
00115                                          (((q31_t) var1 * var1) >> 15)) *
00116                                         (q31_t) half) >> 15))) >> 15)) << 2;
00117 
00118     /* Multiply the inverse square root with the original value */
00119     var1 = ((q15_t) (((q31_t) temp1 * var1) >> 15)) << 1;
00120 
00121     /* Shift the output down accordingly */
00122     if ((signBits1 % 2) == 0)
00123     {
00124       var1 = var1 >> (signBits1 / 2);
00125     }
00126     else
00127     {
00128       var1 = var1 >> ((signBits1 - 1) / 2);
00129     }
00130     *pOut = var1;
00131 
00132     return (ARM_MATH_SUCCESS);
00133   }
00134   /* If the number is a negative number then store zero as its square root value */
00135   else
00136   {
00137     *pOut = 0;
00138     return (ARM_MATH_ARGUMENT_ERROR);
00139   }
00140 }
00141 
00142 /**
00143  * @} end of SQRT group
00144  */
00145