arm_sqrt_q31.c

00001 /* ----------------------------------------------------------------------     
00002 * Copyright (C) 2010-2014 ARM Limited. All rights reserved.  
00003 *     
00004 * $Date:        12. March 2014
00005 * $Revision:    V1.4.3
00006 *     
00007 * Project:      CMSIS DSP Library  
00008 * Title:        arm_sqrt_q31.c     
00009 *     
00010 * Description:  Q31 square root function.    
00011 *     
00012 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
00013 *  
00014 * Redistribution and use in source and binary forms, with or without 
00015 * modification, are permitted provided that the following conditions
00016 * are met:
00017 *   - Redistributions of source code must retain the above copyright
00018 *     notice, this list of conditions and the following disclaimer.
00019 *   - Redistributions in binary form must reproduce the above copyright
00020 *     notice, this list of conditions and the following disclaimer in
00021 *     the documentation and/or other materials provided with the 
00022 *     distribution.
00023 *   - Neither the name of ARM LIMITED nor the names of its contributors
00024 *     may be used to endorse or promote products derived from this
00025 *     software without specific prior written permission.
00026 *
00027 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00028 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00029 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00030 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
00031 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00032 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00033 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00034 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00035 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00036 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00037 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00038 * POSSIBILITY OF SUCH DAMAGE. 
00039 * -------------------------------------------------------------------- */
00040 #include "arm_math.h"
00041 #include "arm_common_tables.h"
00042 
00043 /**     
00044  * @ingroup groupFastMath     
00045  */
00046 
00047 /**     
00048  * @addtogroup SQRT     
00049  * @{     
00050  */
00051 
00052 /**    
00053  * @brief Q31 square root function.    
00054  * @param[in]   in    input value.  The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF.    
00055  * @param[out]  *pOut square root of input value.    
00056  * @return The function returns ARM_MATH_SUCCESS if the input value is positive
00057  * and ARM_MATH_ARGUMENT_ERROR if the input is negative.  For
00058  * negative inputs, the function returns *pOut = 0.
00059  */
00060 
00061 arm_status arm_sqrt_q31(
00062   q31_t in,
00063   q31_t * pOut)
00064 {
00065   q31_t number, temp1, bits_val1, var1, signBits1, half;
00066   float32_t temp_float1;
00067   union
00068   {
00069       q31_t fracval;
00070       float32_t floatval;
00071   } tempconv;
00072 
00073   number = in;
00074 
00075   /* If the input is a positive number then compute the signBits. */
00076   if(number > 0)
00077   {
00078     signBits1 = __CLZ(number) - 1;
00079 
00080     /* Shift by the number of signBits1 */
00081     if((signBits1 % 2) == 0)
00082     {
00083       number = number << signBits1;
00084     }
00085     else
00086     {
00087       number = number << (signBits1 - 1);
00088     }
00089 
00090     /* Calculate half value of the number */
00091     half = number >> 1;
00092     /* Store the number for later use */
00093     temp1 = number;
00094 
00095     /*Convert to float */
00096     temp_float1 = number * 4.6566128731e-010f;
00097     /*Store as integer */
00098     tempconv.floatval = temp_float1;
00099     bits_val1 = tempconv.fracval;
00100     /* Subtract the shifted value from the magic number to give intial guess */
00101     bits_val1 = 0x5f3759df - (bits_val1 >> 1);  // gives initial guess  
00102     /* Store as float */
00103     tempconv.fracval = bits_val1;
00104     temp_float1 = tempconv.floatval;
00105     /* Convert to integer format */
00106     var1 = (q31_t) (temp_float1 * 1073741824);
00107 
00108     /* 1st iteration */
00109     var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
00110                                      ((q31_t)
00111                                       ((((q31_t)
00112                                          (((q63_t) var1 * var1) >> 31)) *
00113                                         (q63_t) half) >> 31))) >> 31)) << 2;
00114     /* 2nd iteration */
00115     var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
00116                                      ((q31_t)
00117                                       ((((q31_t)
00118                                          (((q63_t) var1 * var1) >> 31)) *
00119                                         (q63_t) half) >> 31))) >> 31)) << 2;
00120     /* 3rd iteration */
00121     var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
00122                                      ((q31_t)
00123                                       ((((q31_t)
00124                                          (((q63_t) var1 * var1) >> 31)) *
00125                                         (q63_t) half) >> 31))) >> 31)) << 2;
00126 
00127     /* Multiply the inverse square root with the original value */
00128     var1 = ((q31_t) (((q63_t) temp1 * var1) >> 31)) << 1;
00129 
00130     /* Shift the output down accordingly */
00131     if((signBits1 % 2) == 0)
00132     {
00133       var1 = var1 >> (signBits1 / 2);
00134     }
00135     else
00136     {
00137       var1 = var1 >> ((signBits1 - 1) / 2);
00138     }
00139     *pOut = var1;
00140 
00141     return (ARM_MATH_SUCCESS);
00142   }
00143   /* If the number is a negative number then store zero as its square root value */
00144   else
00145   {
00146     *pOut = 0;
00147     return (ARM_MATH_ARGUMENT_ERROR);
00148   }
00149 }
00150 
00151 /**     
00152  * @} end of SQRT group     
00153  */