tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FastMathFunctions/arm_cos_q31.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_q31.c
   9 *
  10 * Description:  Fast cosine calculation for Q31 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 Q31 (1.31 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 Q31 (Fixed point):
  65  *      (cosTable[i] * pow(2, 31))
  66  * \par
  67  * Finally, round to the nearest integer value:
  68  *      cosTable[i] += (cosTable[i] > 0 ? 0.5 :-0.5);
  69  */
  70
  71
  72 static const q31_t cosTableQ31[259] = {
  73   0x7ff62182, 0x7fffffff, 0x7ff62182, 0x7fd8878e, 0x7fa736b4, 0x7f62368f,
  74   0x7f0991c4, 0x7e9d55fc,
  75   0x7e1d93ea, 0x7d8a5f40, 0x7ce3ceb2, 0x7c29fbee, 0x7b5d039e, 0x7a7d055b,
  76   0x798a23b1, 0x78848414,
  77   0x776c4edb, 0x7641af3d, 0x7504d345, 0x73b5ebd1, 0x72552c85, 0x70e2cbc6,
  78   0x6f5f02b2, 0x6dca0d14,
  79   0x6c242960, 0x6a6d98a4, 0x68a69e81, 0x66cf8120, 0x64e88926, 0x62f201ac,
  80   0x60ec3830, 0x5ed77c8a,
  81   0x5cb420e0, 0x5a82799a, 0x5842dd54, 0x55f5a4d2, 0x539b2af0, 0x5133cc94,
  82   0x4ebfe8a5, 0x4c3fdff4,
  83   0x49b41533, 0x471cece7, 0x447acd50, 0x41ce1e65, 0x3f1749b8, 0x3c56ba70,
  84   0x398cdd32, 0x36ba2014,
  85   0x33def287, 0x30fbc54d, 0x2e110a62, 0x2b1f34eb, 0x2826b928, 0x25280c5e,
  86   0x2223a4c5, 0x1f19f97b,
  87   0x1c0b826a, 0x18f8b83c, 0x15e21445, 0x12c8106f, 0xfab272b, 0xc8bd35e,
  88   0x96a9049, 0x647d97c,
  89   0x3242abf, 0x0, 0xfcdbd541, 0xf9b82684, 0xf6956fb7, 0xf3742ca2, 0xf054d8d5,
  90   0xed37ef91,
  91   0xea1debbb, 0xe70747c4, 0xe3f47d96, 0xe0e60685, 0xdddc5b3b, 0xdad7f3a2,
  92   0xd7d946d8, 0xd4e0cb15,
  93   0xd1eef59e, 0xcf043ab3, 0xcc210d79, 0xc945dfec, 0xc67322ce, 0xc3a94590,
  94   0xc0e8b648, 0xbe31e19b,
  95   0xbb8532b0, 0xb8e31319, 0xb64beacd, 0xb3c0200c, 0xb140175b, 0xaecc336c,
  96   0xac64d510, 0xaa0a5b2e,
  97   0xa7bd22ac, 0xa57d8666, 0xa34bdf20, 0xa1288376, 0x9f13c7d0, 0x9d0dfe54,
  98   0x9b1776da, 0x99307ee0,
  99   0x9759617f, 0x9592675c, 0x93dbd6a0, 0x9235f2ec, 0x90a0fd4e, 0x8f1d343a,
 100   0x8daad37b, 0x8c4a142f,
 101   0x8afb2cbb, 0x89be50c3, 0x8893b125, 0x877b7bec, 0x8675dc4f, 0x8582faa5,
 102   0x84a2fc62, 0x83d60412,
 103   0x831c314e, 0x8275a0c0, 0x81e26c16, 0x8162aa04, 0x80f66e3c, 0x809dc971,
 104   0x8058c94c, 0x80277872,
 105   0x8009de7e, 0x80000000, 0x8009de7e, 0x80277872, 0x8058c94c, 0x809dc971,
 106   0x80f66e3c, 0x8162aa04,
 107   0x81e26c16, 0x8275a0c0, 0x831c314e, 0x83d60412, 0x84a2fc62, 0x8582faa5,
 108   0x8675dc4f, 0x877b7bec,
 109   0x8893b125, 0x89be50c3, 0x8afb2cbb, 0x8c4a142f, 0x8daad37b, 0x8f1d343a,
 110   0x90a0fd4e, 0x9235f2ec,
 111   0x93dbd6a0, 0x9592675c, 0x9759617f, 0x99307ee0, 0x9b1776da, 0x9d0dfe54,
 112   0x9f13c7d0, 0xa1288376,
 113   0xa34bdf20, 0xa57d8666, 0xa7bd22ac, 0xaa0a5b2e, 0xac64d510, 0xaecc336c,
 114   0xb140175b, 0xb3c0200c,
 115   0xb64beacd, 0xb8e31319, 0xbb8532b0, 0xbe31e19b, 0xc0e8b648, 0xc3a94590,
 116   0xc67322ce, 0xc945dfec,
 117   0xcc210d79, 0xcf043ab3, 0xd1eef59e, 0xd4e0cb15, 0xd7d946d8, 0xdad7f3a2,
 118   0xdddc5b3b, 0xe0e60685,
 119   0xe3f47d96, 0xe70747c4, 0xea1debbb, 0xed37ef91, 0xf054d8d5, 0xf3742ca2,
 120   0xf6956fb7, 0xf9b82684,
 121   0xfcdbd541, 0x0, 0x3242abf, 0x647d97c, 0x96a9049, 0xc8bd35e, 0xfab272b,
 122   0x12c8106f,
 123   0x15e21445, 0x18f8b83c, 0x1c0b826a, 0x1f19f97b, 0x2223a4c5, 0x25280c5e,
 124   0x2826b928, 0x2b1f34eb,
 125   0x2e110a62, 0x30fbc54d, 0x33def287, 0x36ba2014, 0x398cdd32, 0x3c56ba70,
 126   0x3f1749b8, 0x41ce1e65,
 127   0x447acd50, 0x471cece7, 0x49b41533, 0x4c3fdff4, 0x4ebfe8a5, 0x5133cc94,
 128   0x539b2af0, 0x55f5a4d2,
 129   0x5842dd54, 0x5a82799a, 0x5cb420e0, 0x5ed77c8a, 0x60ec3830, 0x62f201ac,
 130   0x64e88926, 0x66cf8120,
 131   0x68a69e81, 0x6a6d98a4, 0x6c242960, 0x6dca0d14, 0x6f5f02b2, 0x70e2cbc6,
 132   0x72552c85, 0x73b5ebd1,
 133   0x7504d345, 0x7641af3d, 0x776c4edb, 0x78848414, 0x798a23b1, 0x7a7d055b,
 134   0x7b5d039e, 0x7c29fbee,
 135   0x7ce3ceb2, 0x7d8a5f40, 0x7e1d93ea, 0x7e9d55fc, 0x7f0991c4, 0x7f62368f,
 136   0x7fa736b4, 0x7fd8878e,
 137   0x7ff62182, 0x7fffffff, 0x7ff62182
 138 };
 139
 140 /**
 141  * @brief Fast approximation to the trigonometric cosine function for Q31 data.
 142  * @param[in] x Scaled input value in radians.
 143  * @return  cos(x).
 144  *
 145  * The Q31 input value is in the range [0 +0.9999] and is mapped to a radian
 146  * value in the range [0 2*pi).
 147  */
 148
 149 q31_t arm_cos_q31(
 150   q31_t x)
 151 {
 152   q31_t cosVal, in, in2;                         /* Temporary variables for input, output */
 153   q31_t wa, wb, wc, wd;                          /* Cubic interpolation coefficients */
 154   q31_t a, b, c, d;                              /* Four nearest output values */
 155   q31_t *tablePtr;                               /* Pointer to table */
 156   q31_t fract, fractCube, fractSquare;           /* Temporary values for fractional values */
 157   q31_t oneBy6 = 0x15555555;                     /* Fixed point value of 1/6 */
 158   q31_t tableSpacing = TABLE_SPACING_Q31;        /* Table spacing */
 159   q31_t temp;                                    /* Temporary variable for intermediate process */
 160   int32_t index;                                 /* Index variable */
 161
 162   in = x;
 163
 164   /* Calculate the nearest index */
 165   index = in / tableSpacing;
 166
 167   /* Calculate the nearest value of input */
 168   in2 = ((q31_t) index) * tableSpacing;
 169
 170   /* Calculation of fractional value */
 171   fract = (in - in2) << 8;
 172
 173   /* fractSquare = fract * fract */
 174   fractSquare = ((q31_t) (((q63_t) fract * fract) >> 32));
 175   fractSquare = fractSquare << 1;
 176
 177   /* fractCube = fract * fract * fract */
 178   fractCube = ((q31_t) (((q63_t) fractSquare * fract) >> 32));
 179   fractCube = fractCube << 1;
 180
 181   /* Checking min and max index of table */
 182   if(index < 0)
 183   {
 184     index = 0;
 185   }
 186   else if(index > 256)
 187   {
 188     index = 256;
 189   }
 190
 191   /* Initialise table pointer */
 192   tablePtr = (q31_t *) & cosTableQ31[index];
 193
 194   /* Cubic interpolation process */
 195   /* Calculation of wa */
 196   /* wa = -(oneBy6)*fractCube + (fractSquare >> 1u) - (0x2AAAAAAA)*fract; */
 197   wa = ((q31_t) (((q63_t) oneBy6 * fractCube) >> 32));
 198   temp = 0x2AAAAAAA;
 199   wa = (q31_t) ((((q63_t) wa << 32) + ((q63_t) temp * fract)) >> 32);
 200   wa = -(wa << 1u);
 201   wa += (fractSquare >> 1u);
 202
 203   /* Read first nearest value of output from the cos table */
 204   a = *tablePtr++;
 205
 206   /* cosVal = a*wa */
 207   cosVal = ((q31_t) (((q63_t) a * wa) >> 32));
 208
 209   /* q31(1.31) Fixed point value of 1 */
 210   temp = 0x7FFFFFFF;
 211
 212   /* Calculation of wb */
 213   wb = ((fractCube >> 1u) - (fractSquare + (fract >> 1u))) + temp;
 214   /* Read second nearest value of output from the cos table */
 215   b = *tablePtr++;
 216
 217   /*      cosVal += b*wb */
 218   cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) b * (wb))) >> 32);
 219
 220   /* Calculation of wc */
 221   wc = -fractCube + fractSquare;
 222   wc = (wc >> 1u) + fract;
 223   /* Read third nearest values of output value from the cos table */
 224   c = *tablePtr++;
 225
 226   /*      cosVal += c*wc */
 227   cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) c * (wc))) >> 32);
 228
 229   /* Calculation of wd */
 230   /* wd = (oneBy6)*fractCube - (oneBy6)*fract; */
 231   fractCube = fractCube - fract;
 232   wd = ((q31_t) (((q63_t) oneBy6 * fractCube) >> 32));
 233   wd = (wd << 1u);
 234
 235   /* Read fourth nearest value of output from the cos table */
 236   d = *tablePtr++;
 237
 238   /* cosVal += d*wd; */
 239   cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) d * (wd))) >> 32);
 240
 241
 242   /* convert cosVal in 2.30 format to 1.31 format */
 243   return (__QADD(cosVal, cosVal));
 244
 245 }
 246
 247 /**
 248  * @} end of cos group
 249  */