arm_sqrt_q15.c

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