git.gag.com Git - fw/stlink/blob - exampleF4/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q15.c

   1 /* ----------------------------------------------------------------------
   2 * Copyright (C) 2010 ARM Limited. All rights reserved.
   3 *
   4 * $Date:        15. July 2011
   5 * $Revision:    V1.0.10
   6 *
   7 * Project:          CMSIS DSP Library
   8 * Title:                arm_sqrt_q15.c
   9 *
  10 * Description:  Q15 square root function.
  11 *
  12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  13 *
  14 * Version 1.0.10 2011/7/15
  15 *    Big Endian support added and Merged M0 and M3/M4 Source code.
  16 *
  17 * Version 1.0.3 2010/11/29
  18 *    Re-organized the CMSIS folders and updated documentation.
  19 *
  20 * Version 1.0.2 2010/11/11
  21 *    Documentation updated.
  22 *
  23 * Version 1.0.1 2010/10/05
  24 *    Production release and review comments incorporated.
  25 *
  26 * Version 1.0.0 2010/09/20
  27 *    Production release and review comments incorporated.
  28 * -------------------------------------------------------------------- */
  29
  30 #include "arm_math.h"
  31 #include "arm_common_tables.h"
  32
  33
  34 /**
  35  * @ingroup groupFastMath
  36  */
  37
  38 /**
  39  * @addtogroup SQRT
  40  * @{
  41  */
  42
  43   /**
  44    * @brief  Q15 square root function.
  45    * @param[in]   in     input value.  The range of the input value is [0 +1) or 0x0000 to 0x7FFF.
  46    * @param[out]  *pOut  square root of input value.
  47    * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if
  48    * <code>in</code> is negative value and returns zero output for negative values.
  49    */
  50
  51 arm_status arm_sqrt_q15(
  52   q15_t in,
  53   q15_t * pOut)
  54 {
  55   q31_t prevOut;
  56   q15_t oneByOut;
  57   uint32_t sign_bits;
  58
  59 #ifndef ARM_MATH_CM0
  60
  61   /* Run the below code for Cortex-M4 and Cortex-M3 */
  62
  63   q31_t out;
  64
  65   if(in > 0)
  66   {
  67     /* run for ten iterations */
  68
  69     /* Take initial guess as half of the input and first iteration */
  70     out = ((q31_t) in >> 1u) + 0x3FFF;
  71
  72     /* Calculation of reciprocal of out */
  73     /* oneByOut contains reciprocal of out which is in 2.14 format
  74        and oneByOut should be upscaled by signBits */
  75     sign_bits = arm_recip_q15((q15_t) out, &oneByOut, armRecipTableQ15);
  76
  77     /* 0.5 * (out) */
  78     out = out >> 1u;
  79     /* prevOut = 0.5 * out + (in * (oneByOut << signBits))) */
  80     prevOut = out + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits);
  81
  82     /* Third iteration */
  83     sign_bits = arm_recip_q15((q15_t) prevOut, &oneByOut, armRecipTableQ15);
  84     prevOut = prevOut >> 1u;
  85     out = prevOut + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits);
  86
  87     sign_bits = arm_recip_q15((q15_t) out, &oneByOut, armRecipTableQ15);
  88     out = out >> 1u;
  89     prevOut = out + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits);
  90
  91     /* Fifth iteration */
  92     sign_bits = arm_recip_q15((q15_t) prevOut, &oneByOut, armRecipTableQ15);
  93     prevOut = prevOut >> 1u;
  94     out = prevOut + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits);
  95
  96     sign_bits = arm_recip_q15((q15_t) out, &oneByOut, armRecipTableQ15);
  97     out = out >> 1u;
  98     prevOut = out + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits);
  99
 100     /* Seventh iteration */
 101     sign_bits = arm_recip_q15((q15_t) prevOut, &oneByOut, armRecipTableQ15);
 102     prevOut = prevOut >> 1u;
 103     out = prevOut + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits);
 104
 105     sign_bits = arm_recip_q15((q15_t) out, &oneByOut, armRecipTableQ15);
 106     out = out >> 1u;
 107     prevOut = out + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits);
 108
 109     sign_bits = arm_recip_q15((q15_t) prevOut, &oneByOut, armRecipTableQ15);
 110     prevOut = prevOut >> 1u;
 111     out = prevOut + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits);
 112
 113     /* tenth iteration */
 114     sign_bits = arm_recip_q15((q15_t) out, &oneByOut, armRecipTableQ15);
 115     out = out >> 1u;
 116     *pOut = out + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits);
 117
 118     return (ARM_MATH_SUCCESS);
 119   }
 120
 121 #else
 122
 123   /* Run the below code for Cortex-M0 */
 124
 125   q31_t out, loopVar;                            /* Temporary variable for output, loop variable */
 126   if(in > 0)
 127   {
 128     /* run for ten iterations */
 129
 130     /* Take initial guess as half of the input and first iteration */
 131     out = ((q31_t) in >> 1u) + 0x3FFF;
 132
 133     /* Calculation of reciprocal of out */
 134
 135     /* oneByOut contains reciprocal of out which is in 2.14 format
 136        and oneByOut should be upscaled by sign bits */
 137     sign_bits = arm_recip_q15((q15_t) out, &oneByOut, armRecipTableQ15);
 138
 139     /* 0.5 * (out) */
 140     out = out >> 1u;
 141     /* prevOut = 0.5 * out + (in * oneByOut) << signbits))) */
 142     prevOut = out + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits);
 143
 144     /* loop for third iteration to tenth iteration */
 145
 146     for (loopVar = 1; loopVar <= 8; loopVar++)
 147     {
 148
 149       sign_bits = arm_recip_q15((q15_t) prevOut, &oneByOut, armRecipTableQ15);
 150       /* 0.5 * (prevOut) */
 151       prevOut = prevOut >> 1u;
 152       /* prevOut = 0.5 * prevOut+ (in * oneByOut) << signbits))) */
 153       out =
 154         prevOut + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits);
 155       /* prevOut      = out */
 156       prevOut = out;
 157
 158     }
 159     /* output is moved to pOut pointer */
 160     *pOut = prevOut;
 161
 162     return (ARM_MATH_SUCCESS);
 163   }
 164
 165 #endif /* #ifndef ARM_MATH_CM0 */
 166
 167   else
 168   {
 169
 170     *pOut = 0;
 171     return (ARM_MATH_ARGUMENT_ERROR);
 172   }
 173
 174 }
 175
 176 /**
 177  * @} end of SQRT group
 178  */