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