tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FastMathFunctions/arm_sin_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_sin_q15.c
   9 *
  10 * Description:  Fast sine 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 sin
  49  * @{
  50  */
  51
  52
  53 /**
  54 * \par
  55  * Table values are in Q15 (1.15 fixed-point format) and generation is done in
  56  * three steps.  First,  generate sin values in floating point:
  57  * <pre>
  58  * tableSize = 256;
  59  * for(n = -1; n < (tableSize + 1); n++)
  60  * {
  61  *      sinTable[n+1]= sin(2*pi*n/tableSize);
  62  * } </pre>
  63  * where pi value is  3.14159265358979
  64  * \par
  65  * Second, convert floating-point to Q15 (fixed-point):
  66  *      (sinTable[i] * pow(2, 15))
  67  * \par
  68  * Finally, round to the nearest integer value:
  69  *      sinTable[i] += (sinTable[i] > 0 ? 0.5 :-0.5);
  70 */
  71
  72 static const q15_t sinTableQ15[259] = {
  73   0xfcdc, 0x0, 0x324, 0x648, 0x96b, 0xc8c, 0xfab, 0x12c8,
  74   0x15e2, 0x18f9, 0x1c0c, 0x1f1a, 0x2224, 0x2528, 0x2827, 0x2b1f,
  75   0x2e11, 0x30fc, 0x33df, 0x36ba, 0x398d, 0x3c57, 0x3f17, 0x41ce,
  76   0x447b, 0x471d, 0x49b4, 0x4c40, 0x4ec0, 0x5134, 0x539b, 0x55f6,
  77   0x5843, 0x5a82, 0x5cb4, 0x5ed7, 0x60ec, 0x62f2, 0x64e9, 0x66d0,
  78   0x68a7, 0x6a6e, 0x6c24, 0x6dca, 0x6f5f, 0x70e3, 0x7255, 0x73b6,
  79   0x7505, 0x7642, 0x776c, 0x7885, 0x798a, 0x7a7d, 0x7b5d, 0x7c2a,
  80   0x7ce4, 0x7d8a, 0x7e1e, 0x7e9d, 0x7f0a, 0x7f62, 0x7fa7, 0x7fd9,
  81   0x7ff6, 0x7fff, 0x7ff6, 0x7fd9, 0x7fa7, 0x7f62, 0x7f0a, 0x7e9d,
  82   0x7e1e, 0x7d8a, 0x7ce4, 0x7c2a, 0x7b5d, 0x7a7d, 0x798a, 0x7885,
  83   0x776c, 0x7642, 0x7505, 0x73b6, 0x7255, 0x70e3, 0x6f5f, 0x6dca,
  84   0x6c24, 0x6a6e, 0x68a7, 0x66d0, 0x64e9, 0x62f2, 0x60ec, 0x5ed7,
  85   0x5cb4, 0x5a82, 0x5843, 0x55f6, 0x539b, 0x5134, 0x4ec0, 0x4c40,
  86   0x49b4, 0x471d, 0x447b, 0x41ce, 0x3f17, 0x3c57, 0x398d, 0x36ba,
  87   0x33df, 0x30fc, 0x2e11, 0x2b1f, 0x2827, 0x2528, 0x2224, 0x1f1a,
  88   0x1c0c, 0x18f9, 0x15e2, 0x12c8, 0xfab, 0xc8c, 0x96b, 0x648,
  89   0x324, 0x0, 0xfcdc, 0xf9b8, 0xf695, 0xf374, 0xf055, 0xed38,
  90   0xea1e, 0xe707, 0xe3f4, 0xe0e6, 0xdddc, 0xdad8, 0xd7d9, 0xd4e1,
  91   0xd1ef, 0xcf04, 0xcc21, 0xc946, 0xc673, 0xc3a9, 0xc0e9, 0xbe32,
  92   0xbb85, 0xb8e3, 0xb64c, 0xb3c0, 0xb140, 0xaecc, 0xac65, 0xaa0a,
  93   0xa7bd, 0xa57e, 0xa34c, 0xa129, 0x9f14, 0x9d0e, 0x9b17, 0x9930,
  94   0x9759, 0x9592, 0x93dc, 0x9236, 0x90a1, 0x8f1d, 0x8dab, 0x8c4a,
  95   0x8afb, 0x89be, 0x8894, 0x877b, 0x8676, 0x8583, 0x84a3, 0x83d6,
  96   0x831c, 0x8276, 0x81e2, 0x8163, 0x80f6, 0x809e, 0x8059, 0x8027,
  97   0x800a, 0x8000, 0x800a, 0x8027, 0x8059, 0x809e, 0x80f6, 0x8163,
  98   0x81e2, 0x8276, 0x831c, 0x83d6, 0x84a3, 0x8583, 0x8676, 0x877b,
  99   0x8894, 0x89be, 0x8afb, 0x8c4a, 0x8dab, 0x8f1d, 0x90a1, 0x9236,
 100   0x93dc, 0x9592, 0x9759, 0x9930, 0x9b17, 0x9d0e, 0x9f14, 0xa129,
 101   0xa34c, 0xa57e, 0xa7bd, 0xaa0a, 0xac65, 0xaecc, 0xb140, 0xb3c0,
 102   0xb64c, 0xb8e3, 0xbb85, 0xbe32, 0xc0e9, 0xc3a9, 0xc673, 0xc946,
 103   0xcc21, 0xcf04, 0xd1ef, 0xd4e1, 0xd7d9, 0xdad8, 0xdddc, 0xe0e6,
 104   0xe3f4, 0xe707, 0xea1e, 0xed38, 0xf055, 0xf374, 0xf695, 0xf9b8,
 105   0xfcdc, 0x0, 0x324
 106 };
 107
 108
 109 /**
 110  * @brief Fast approximation to the trigonometric sine function for Q15 data.
 111  * @param[in] x Scaled input value in radians.
 112  * @return  sin(x).
 113  *
 114  * The Q15 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*pi).
 115  */
 116
 117 q15_t arm_sin_q15(
 118   q15_t x)
 119 {
 120   q31_t sinVal;                                  /* Temporary variables output */
 121   q15_t *tablePtr;                               /* Pointer to table */
 122   q15_t fract, in, in2;                          /* Temporary variables for input, output */
 123   q31_t wa, wb, wc, wd;                          /* Cubic interpolation coefficients */
 124   q15_t a, b, c, d;                              /* Four nearest output values */
 125   q15_t fractCube, fractSquare;                  /* Temporary values 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 *) & sinTableQ15[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 += ((q31_t) fractSquare >> 1u);
 167
 168   /* Read first nearest value of output from the sin table */
 169   a = *tablePtr++;
 170
 171   /* sinVal = a * wa */
 172   sinVal = a * wa;
 173
 174   /* Calculation of wb */
 175   wb = (((q31_t) fractCube >> 1u) - (q31_t) fractSquare) -
 176     (((q31_t) fract >> 1u) - 0x7FFF);
 177
 178   /* Read second nearest value of output from the sin table */
 179   b = *tablePtr++;
 180
 181   /*      sinVal += b*wb */
 182   sinVal += b * wb;
 183
 184
 185   /* Calculation of wc */
 186   wc = -(q31_t) fractCube + fractSquare;
 187   wc = (wc >> 1u) + fract;
 188
 189   /* Read third nearest value of output from the sin table */
 190   c = *tablePtr++;
 191
 192   /* sinVal += c*wc */
 193   sinVal += c * wc;
 194
 195   /* Calculation of wd */
 196   /* wd = (oneBy6)*fractCube - (oneBy6)*fract; */
 197   fractCube = fractCube - fract;
 198   wd = ((q15_t) (((q31_t) oneBy6 * fractCube) >> 15));
 199
 200   /* Read fourth nearest value of output from the sin table */
 201   d = *tablePtr++;
 202
 203   /* sinVal += d*wd; */
 204   sinVal += d * wd;
 205
 206   /* Convert output value in 1.15(q15) format and saturate */
 207   sinVal = __SSAT((sinVal >> 15), 16);
 208
 209   /* Return the output value in 1.15(q15) format */
 210   return ((q15_t) sinVal);
 211
 212 }
 213
 214 /**
 215  * @} end of sin group
 216  */