tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FastMathFunctions/arm_cos_q15.c

   1 /* ----------------------------------------------------------------------
   2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
   3 *
   4 * $Date:        17. January 2013
   5 * $Revision:    V1.4.1
   6 *
   7 * Project:          CMSIS DSP Library
   8 * Title:                arm_cos_q15.c
   9 *
  10 * Description:  Fast cosine calculation for Q15 values.
  11 *
  12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  13 *
  14 * Redistribution and use in source and binary forms, with or without
  15 * modification, are permitted provided that the following conditions
  16 * are met:
  17 *   - Redistributions of source code must retain the above copyright
  18 *     notice, this list of conditions and the following disclaimer.
  19 *   - Redistributions in binary form must reproduce the above copyright
  20 *     notice, this list of conditions and the following disclaimer in
  21 *     the documentation and/or other materials provided with the
  22 *     distribution.
  23 *   - Neither the name of ARM LIMITED nor the names of its contributors
  24 *     may be used to endorse or promote products derived from this
  25 *     software without specific prior written permission.
  26 *
  27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  38 * POSSIBILITY OF SUCH DAMAGE.
  39 * -------------------------------------------------------------------- */
  40
  41 #include "arm_math.h"
  42
  43 /**
  44  * @ingroup groupFastMath
  45  */
  46
  47  /**
  48  * @addtogroup cos
  49  * @{
  50  */
  51
  52 /**
  53 * \par
  54  * Table values are in Q15 (1.15 fixed-point format) and generation is done in
  55  * three steps.  First,  generate cos values in floating point:
  56  * <pre>
  57  * tableSize = 256;
  58  * for(n = -1; n < (tableSize + 1); n++)
  59  * {
  60  *      cosTable[n+1]= cos(2*pi*n/tableSize);
  61  * } </pre>
  62  * where pi value is  3.14159265358979
  63  * \par
  64  * Second, convert floating-point to Q15 (fixed-point):
  65  *      (cosTable[i] * pow(2, 15))
  66  * \par
  67  * Finally, round to the nearest integer value:
  68  *      cosTable[i] += (cosTable[i] > 0 ? 0.5 :-0.5);
  69 */
  70
  71 static const q15_t cosTableQ15[259] = {
  72   0x7ff6, 0x7fff, 0x7ff6, 0x7fd9, 0x7fa7, 0x7f62, 0x7f0a, 0x7e9d,
  73   0x7e1e, 0x7d8a, 0x7ce4, 0x7c2a, 0x7b5d, 0x7a7d, 0x798a, 0x7885,
  74   0x776c, 0x7642, 0x7505, 0x73b6, 0x7255, 0x70e3, 0x6f5f, 0x6dca,
  75   0x6c24, 0x6a6e, 0x68a7, 0x66d0, 0x64e9, 0x62f2, 0x60ec, 0x5ed7,
  76   0x5cb4, 0x5a82, 0x5843, 0x55f6, 0x539b, 0x5134, 0x4ec0, 0x4c40,
  77   0x49b4, 0x471d, 0x447b, 0x41ce, 0x3f17, 0x3c57, 0x398d, 0x36ba,
  78   0x33df, 0x30fc, 0x2e11, 0x2b1f, 0x2827, 0x2528, 0x2224, 0x1f1a,
  79   0x1c0c, 0x18f9, 0x15e2, 0x12c8, 0xfab, 0xc8c, 0x96b, 0x648,
  80   0x324, 0x0, 0xfcdc, 0xf9b8, 0xf695, 0xf374, 0xf055, 0xed38,
  81   0xea1e, 0xe707, 0xe3f4, 0xe0e6, 0xdddc, 0xdad8, 0xd7d9, 0xd4e1,
  82   0xd1ef, 0xcf04, 0xcc21, 0xc946, 0xc673, 0xc3a9, 0xc0e9, 0xbe32,
  83   0xbb85, 0xb8e3, 0xb64c, 0xb3c0, 0xb140, 0xaecc, 0xac65, 0xaa0a,
  84   0xa7bd, 0xa57e, 0xa34c, 0xa129, 0x9f14, 0x9d0e, 0x9b17, 0x9930,
  85   0x9759, 0x9592, 0x93dc, 0x9236, 0x90a1, 0x8f1d, 0x8dab, 0x8c4a,
  86   0x8afb, 0x89be, 0x8894, 0x877b, 0x8676, 0x8583, 0x84a3, 0x83d6,
  87   0x831c, 0x8276, 0x81e2, 0x8163, 0x80f6, 0x809e, 0x8059, 0x8027,
  88   0x800a, 0x8000, 0x800a, 0x8027, 0x8059, 0x809e, 0x80f6, 0x8163,
  89   0x81e2, 0x8276, 0x831c, 0x83d6, 0x84a3, 0x8583, 0x8676, 0x877b,
  90   0x8894, 0x89be, 0x8afb, 0x8c4a, 0x8dab, 0x8f1d, 0x90a1, 0x9236,
  91   0x93dc, 0x9592, 0x9759, 0x9930, 0x9b17, 0x9d0e, 0x9f14, 0xa129,
  92   0xa34c, 0xa57e, 0xa7bd, 0xaa0a, 0xac65, 0xaecc, 0xb140, 0xb3c0,
  93   0xb64c, 0xb8e3, 0xbb85, 0xbe32, 0xc0e9, 0xc3a9, 0xc673, 0xc946,
  94   0xcc21, 0xcf04, 0xd1ef, 0xd4e1, 0xd7d9, 0xdad8, 0xdddc, 0xe0e6,
  95   0xe3f4, 0xe707, 0xea1e, 0xed38, 0xf055, 0xf374, 0xf695, 0xf9b8,
  96   0xfcdc, 0x0, 0x324, 0x648, 0x96b, 0xc8c, 0xfab, 0x12c8,
  97   0x15e2, 0x18f9, 0x1c0c, 0x1f1a, 0x2224, 0x2528, 0x2827, 0x2b1f,
  98   0x2e11, 0x30fc, 0x33df, 0x36ba, 0x398d, 0x3c57, 0x3f17, 0x41ce,
  99   0x447b, 0x471d, 0x49b4, 0x4c40, 0x4ec0, 0x5134, 0x539b, 0x55f6,
 100   0x5843, 0x5a82, 0x5cb4, 0x5ed7, 0x60ec, 0x62f2, 0x64e9, 0x66d0,
 101   0x68a7, 0x6a6e, 0x6c24, 0x6dca, 0x6f5f, 0x70e3, 0x7255, 0x73b6,
 102   0x7505, 0x7642, 0x776c, 0x7885, 0x798a, 0x7a7d, 0x7b5d, 0x7c2a,
 103   0x7ce4, 0x7d8a, 0x7e1e, 0x7e9d, 0x7f0a, 0x7f62, 0x7fa7, 0x7fd9,
 104   0x7ff6, 0x7fff, 0x7ff6
 105 };
 106
 107
 108 /**
 109  * @brief Fast approximation to the trigonometric cosine function for Q15 data.
 110  * @param[in] x Scaled input value in radians.
 111  * @return  cos(x).
 112  *
 113  * The Q15 input value is in the range [0 +0.9999] and is mapped to a radian
 114  * value in the range [0 2*pi).
 115  */
 116
 117 q15_t arm_cos_q15(
 118   q15_t x)
 119 {
 120   q31_t cosVal;                                  /* Temporary variable for output */
 121   q15_t *tablePtr;                               /* Pointer to table */
 122   q15_t in, in2;                                 /* Temporary variables for input */
 123   q31_t wa, wb, wc, wd;                          /* Cubic interpolation coefficients */
 124   q15_t a, b, c, d;                              /* Four nearest output values */
 125   q15_t fract, fractCube, fractSquare;           /* Variables for fractional value */
 126   q15_t oneBy6 = 0x1555;                         /* Fixed point value of 1/6 */
 127   q15_t tableSpacing = TABLE_SPACING_Q15;        /* Table spacing */
 128   int32_t index;                                 /* Index variable */
 129
 130   in = x;
 131
 132   /* Calculate the nearest index */
 133   index = (int32_t) in / tableSpacing;
 134
 135   /* Calculate the nearest value of input */
 136   in2 = (q15_t) index *tableSpacing;
 137
 138   /* Calculation of fractional value */
 139   fract = (in - in2) << 8;
 140
 141   /* fractSquare = fract * fract */
 142   fractSquare = (q15_t) ((fract * fract) >> 15);
 143
 144   /* fractCube = fract * fract * fract */
 145   fractCube = (q15_t) ((fractSquare * fract) >> 15);
 146
 147   /* Checking min and max index of table */
 148   if(index < 0)
 149   {
 150     index = 0;
 151   }
 152   else if(index > 256)
 153   {
 154     index = 256;
 155   }
 156
 157   /* Initialise table pointer */
 158   tablePtr = (q15_t *) & cosTableQ15[index];
 159
 160   /* Cubic interpolation process */
 161   /* Calculation of wa */
 162   /* wa = -(oneBy6)*fractCube + (fractSquare >> 1u) - (0x2AAA)*fract; */
 163   wa = (q31_t) oneBy6 *fractCube;
 164   wa += (q31_t) 0x2AAA *fract;
 165   wa = -(wa >> 15);
 166   wa += (fractSquare >> 1u);
 167
 168   /* Read first nearest value of output from the cos table */
 169   a = *tablePtr++;
 170
 171   /* cosVal = a * wa */
 172   cosVal = a * wa;
 173
 174   /* Calculation of wb */
 175   wb = (((fractCube >> 1u) - fractSquare) - (fract >> 1u)) + 0x7FFF;
 176
 177   /* Read second nearest value of output from the cos table */
 178   b = *tablePtr++;
 179
 180   /*      cosVal += b*wb */
 181   cosVal += b * wb;
 182
 183   /* Calculation of wc */
 184   wc = -(q31_t) fractCube + fractSquare;
 185   wc = (wc >> 1u) + fract;
 186
 187   /* Read third nearest value of output from the cos table */
 188   c = *tablePtr++;
 189
 190   /* cosVal += c*wc */
 191   cosVal += c * wc;
 192
 193   /* Calculation of wd */
 194   /* wd = (oneBy6)*fractCube - (oneBy6)*fract; */
 195   fractCube = fractCube - fract;
 196   wd = ((q15_t) (((q31_t) oneBy6 * fractCube) >> 15));
 197
 198   /* Read fourth nearest value of output from the cos table */
 199   d = *tablePtr++;
 200
 201   /* cosVal += d*wd; */
 202   cosVal += d * wd;
 203
 204   /* Convert output value in 1.15(q15) format and saturate */
 205   cosVal = __SSAT((cosVal >> 15), 16);
 206
 207   /* Return the output value in 1.15(q15) format */
 208   return ((q15_t) cosVal);
 209
 210 }
 211
 212 /**
 213  * @} end of cos group
 214  */