tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_sin_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_sin_cos_q31.c
   9 *
  10 * Description:  Cosine & 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 groupController
  45  */
  46
  47  /**
  48  * @addtogroup SinCos
  49  * @{
  50  */
  51
  52 /**
  53 * \par
  54 * Sine Table is generated from following loop
  55 * <pre>for(i = 0; i < 360; i++)
  56 * {
  57 *    sinTable[i]= sin((i-180) * PI/180.0);
  58 * } </pre>
  59 * Convert above coefficients to fixed point 1.31 format.
  60 */
  61
  62 static const int32_t sinTableQ31[360] = {
  63
  64   0x0, 0xfdc41e9b, 0xfb8869ce, 0xf94d0e2e, 0xf7123849, 0xf4d814a4, 0xf29ecfb2,
  65   0xf06695da,
  66   0xee2f9369, 0xebf9f498, 0xe9c5e582, 0xe7939223, 0xe5632654, 0xe334cdc9,
  67   0xe108b40d, 0xdedf047d,
  68   0xdcb7ea46, 0xda939061, 0xd8722192, 0xd653c860, 0xd438af17, 0xd220ffc0,
  69   0xd00ce422, 0xcdfc85bb,
  70   0xcbf00dbe, 0xc9e7a512, 0xc7e3744b, 0xc5e3a3a9, 0xc3e85b18, 0xc1f1c224,
  71   0xc0000000, 0xbe133b7c,
  72   0xbc2b9b05, 0xba4944a2, 0xb86c5df0, 0xb6950c1e, 0xb4c373ee, 0xb2f7b9af,
  73   0xb1320139, 0xaf726def,
  74   0xadb922b7, 0xac0641fb, 0xaa59eda4, 0xa8b4471a, 0xa7156f3c, 0xa57d8666,
  75   0xa3ecac65, 0xa263007d,
  76   0xa0e0a15f, 0x9f65ad2d, 0x9df24175, 0x9c867b2c, 0x9b2276b0, 0x99c64fc5,
  77   0x98722192, 0x9726069c,
  78   0x95e218c9, 0x94a6715d, 0x937328f5, 0x92485786, 0x9126145f, 0x900c7621,
  79   0x8efb92c2, 0x8df37f8b,
  80   0x8cf45113, 0x8bfe1b3f, 0x8b10f144, 0x8a2ce59f, 0x89520a1a, 0x88806fc4,
  81   0x87b826f7, 0x86f93f50,
  82   0x8643c7b3, 0x8597ce46, 0x84f56073, 0x845c8ae3, 0x83cd5982, 0x8347d77b,
  83   0x82cc0f36, 0x825a0a5b,
  84   0x81f1d1ce, 0x81936daf, 0x813ee55b, 0x80f43f69, 0x80b381ac, 0x807cb130,
  85   0x804fd23a, 0x802ce84c,
  86   0x8013f61d, 0x8004fda0, 0x80000000, 0x8004fda0, 0x8013f61d, 0x802ce84c,
  87   0x804fd23a, 0x807cb130,
  88   0x80b381ac, 0x80f43f69, 0x813ee55b, 0x81936daf, 0x81f1d1ce, 0x825a0a5b,
  89   0x82cc0f36, 0x8347d77b,
  90   0x83cd5982, 0x845c8ae3, 0x84f56073, 0x8597ce46, 0x8643c7b3, 0x86f93f50,
  91   0x87b826f7, 0x88806fc4,
  92   0x89520a1a, 0x8a2ce59f, 0x8b10f144, 0x8bfe1b3f, 0x8cf45113, 0x8df37f8b,
  93   0x8efb92c2, 0x900c7621,
  94   0x9126145f, 0x92485786, 0x937328f5, 0x94a6715d, 0x95e218c9, 0x9726069c,
  95   0x98722192, 0x99c64fc5,
  96   0x9b2276b0, 0x9c867b2c, 0x9df24175, 0x9f65ad2d, 0xa0e0a15f, 0xa263007d,
  97   0xa3ecac65, 0xa57d8666,
  98   0xa7156f3c, 0xa8b4471a, 0xaa59eda4, 0xac0641fb, 0xadb922b7, 0xaf726def,
  99   0xb1320139, 0xb2f7b9af,
 100   0xb4c373ee, 0xb6950c1e, 0xb86c5df0, 0xba4944a2, 0xbc2b9b05, 0xbe133b7c,
 101   0xc0000000, 0xc1f1c224,
 102   0xc3e85b18, 0xc5e3a3a9, 0xc7e3744b, 0xc9e7a512, 0xcbf00dbe, 0xcdfc85bb,
 103   0xd00ce422, 0xd220ffc0,
 104   0xd438af17, 0xd653c860, 0xd8722192, 0xda939061, 0xdcb7ea46, 0xdedf047d,
 105   0xe108b40d, 0xe334cdc9,
 106   0xe5632654, 0xe7939223, 0xe9c5e582, 0xebf9f498, 0xee2f9369, 0xf06695da,
 107   0xf29ecfb2, 0xf4d814a4,
 108   0xf7123849, 0xf94d0e2e, 0xfb8869ce, 0xfdc41e9b, 0x0, 0x23be165, 0x4779632,
 109   0x6b2f1d2,
 110   0x8edc7b7, 0xb27eb5c, 0xd61304e, 0xf996a26, 0x11d06c97, 0x14060b68,
 111   0x163a1a7e, 0x186c6ddd,
 112   0x1a9cd9ac, 0x1ccb3237, 0x1ef74bf3, 0x2120fb83, 0x234815ba, 0x256c6f9f,
 113   0x278dde6e, 0x29ac37a0,
 114   0x2bc750e9, 0x2ddf0040, 0x2ff31bde, 0x32037a45, 0x340ff242, 0x36185aee,
 115   0x381c8bb5, 0x3a1c5c57,
 116   0x3c17a4e8, 0x3e0e3ddc, 0x40000000, 0x41ecc484, 0x43d464fb, 0x45b6bb5e,
 117   0x4793a210, 0x496af3e2,
 118   0x4b3c8c12, 0x4d084651, 0x4ecdfec7, 0x508d9211, 0x5246dd49, 0x53f9be05,
 119   0x55a6125c, 0x574bb8e6,
 120   0x58ea90c4, 0x5a82799a, 0x5c13539b, 0x5d9cff83, 0x5f1f5ea1, 0x609a52d3,
 121   0x620dbe8b, 0x637984d4,
 122   0x64dd8950, 0x6639b03b, 0x678dde6e, 0x68d9f964, 0x6a1de737, 0x6b598ea3,
 123   0x6c8cd70b, 0x6db7a87a,
 124   0x6ed9eba1, 0x6ff389df, 0x71046d3e, 0x720c8075, 0x730baeed, 0x7401e4c1,
 125   0x74ef0ebc, 0x75d31a61,
 126   0x76adf5e6, 0x777f903c, 0x7847d909, 0x7906c0b0, 0x79bc384d, 0x7a6831ba,
 127   0x7b0a9f8d, 0x7ba3751d,
 128   0x7c32a67e, 0x7cb82885, 0x7d33f0ca, 0x7da5f5a5, 0x7e0e2e32, 0x7e6c9251,
 129   0x7ec11aa5, 0x7f0bc097,
 130   0x7f4c7e54, 0x7f834ed0, 0x7fb02dc6, 0x7fd317b4, 0x7fec09e3, 0x7ffb0260,
 131   0x7fffffff, 0x7ffb0260,
 132   0x7fec09e3, 0x7fd317b4, 0x7fb02dc6, 0x7f834ed0, 0x7f4c7e54, 0x7f0bc097,
 133   0x7ec11aa5, 0x7e6c9251,
 134   0x7e0e2e32, 0x7da5f5a5, 0x7d33f0ca, 0x7cb82885, 0x7c32a67e, 0x7ba3751d,
 135   0x7b0a9f8d, 0x7a6831ba,
 136   0x79bc384d, 0x7906c0b0, 0x7847d909, 0x777f903c, 0x76adf5e6, 0x75d31a61,
 137   0x74ef0ebc, 0x7401e4c1,
 138   0x730baeed, 0x720c8075, 0x71046d3e, 0x6ff389df, 0x6ed9eba1, 0x6db7a87a,
 139   0x6c8cd70b, 0x6b598ea3,
 140   0x6a1de737, 0x68d9f964, 0x678dde6e, 0x6639b03b, 0x64dd8950, 0x637984d4,
 141   0x620dbe8b, 0x609a52d3,
 142   0x5f1f5ea1, 0x5d9cff83, 0x5c13539b, 0x5a82799a, 0x58ea90c4, 0x574bb8e6,
 143   0x55a6125c, 0x53f9be05,
 144   0x5246dd49, 0x508d9211, 0x4ecdfec7, 0x4d084651, 0x4b3c8c12, 0x496af3e2,
 145   0x4793a210, 0x45b6bb5e,
 146   0x43d464fb, 0x41ecc484, 0x40000000, 0x3e0e3ddc, 0x3c17a4e8, 0x3a1c5c57,
 147   0x381c8bb5, 0x36185aee,
 148   0x340ff242, 0x32037a45, 0x2ff31bde, 0x2ddf0040, 0x2bc750e9, 0x29ac37a0,
 149   0x278dde6e, 0x256c6f9f,
 150   0x234815ba, 0x2120fb83, 0x1ef74bf3, 0x1ccb3237, 0x1a9cd9ac, 0x186c6ddd,
 151   0x163a1a7e, 0x14060b68,
 152   0x11d06c97, 0xf996a26, 0xd61304e, 0xb27eb5c, 0x8edc7b7, 0x6b2f1d2,
 153   0x4779632, 0x23be165,
 154
 155
 156 };
 157
 158 /**
 159 * \par
 160 * Cosine Table is generated from following loop
 161 * <pre>for(i = 0; i < 360; i++)
 162 * {
 163 *    cosTable[i]= cos((i-180) * PI/180.0);
 164 * } </pre>
 165 * \par
 166 * Convert above coefficients to fixed point 1.31 format.
 167 */
 168 static const int32_t cosTableQ31[360] = {
 169   0x80000000, 0x8004fda0, 0x8013f61d, 0x802ce84c, 0x804fd23a, 0x807cb130,
 170   0x80b381ac, 0x80f43f69,
 171   0x813ee55b, 0x81936daf, 0x81f1d1ce, 0x825a0a5b, 0x82cc0f36, 0x8347d77b,
 172   0x83cd5982, 0x845c8ae3,
 173   0x84f56073, 0x8597ce46, 0x8643c7b3, 0x86f93f50, 0x87b826f7, 0x88806fc4,
 174   0x89520a1a, 0x8a2ce59f,
 175   0x8b10f144, 0x8bfe1b3f, 0x8cf45113, 0x8df37f8b, 0x8efb92c2, 0x900c7621,
 176   0x9126145f, 0x92485786,
 177   0x937328f5, 0x94a6715d, 0x95e218c9, 0x9726069c, 0x98722192, 0x99c64fc5,
 178   0x9b2276b0, 0x9c867b2c,
 179   0x9df24175, 0x9f65ad2d, 0xa0e0a15f, 0xa263007d, 0xa3ecac65, 0xa57d8666,
 180   0xa7156f3c, 0xa8b4471a,
 181   0xaa59eda4, 0xac0641fb, 0xadb922b7, 0xaf726def, 0xb1320139, 0xb2f7b9af,
 182   0xb4c373ee, 0xb6950c1e,
 183   0xb86c5df0, 0xba4944a2, 0xbc2b9b05, 0xbe133b7c, 0xc0000000, 0xc1f1c224,
 184   0xc3e85b18, 0xc5e3a3a9,
 185   0xc7e3744b, 0xc9e7a512, 0xcbf00dbe, 0xcdfc85bb, 0xd00ce422, 0xd220ffc0,
 186   0xd438af17, 0xd653c860,
 187   0xd8722192, 0xda939061, 0xdcb7ea46, 0xdedf047d, 0xe108b40d, 0xe334cdc9,
 188   0xe5632654, 0xe7939223,
 189   0xe9c5e582, 0xebf9f498, 0xee2f9369, 0xf06695da, 0xf29ecfb2, 0xf4d814a4,
 190   0xf7123849, 0xf94d0e2e,
 191   0xfb8869ce, 0xfdc41e9b, 0x0, 0x23be165, 0x4779632, 0x6b2f1d2, 0x8edc7b7,
 192   0xb27eb5c,
 193   0xd61304e, 0xf996a26, 0x11d06c97, 0x14060b68, 0x163a1a7e, 0x186c6ddd,
 194   0x1a9cd9ac, 0x1ccb3237,
 195   0x1ef74bf3, 0x2120fb83, 0x234815ba, 0x256c6f9f, 0x278dde6e, 0x29ac37a0,
 196   0x2bc750e9, 0x2ddf0040,
 197   0x2ff31bde, 0x32037a45, 0x340ff242, 0x36185aee, 0x381c8bb5, 0x3a1c5c57,
 198   0x3c17a4e8, 0x3e0e3ddc,
 199   0x40000000, 0x41ecc484, 0x43d464fb, 0x45b6bb5e, 0x4793a210, 0x496af3e2,
 200   0x4b3c8c12, 0x4d084651,
 201   0x4ecdfec7, 0x508d9211, 0x5246dd49, 0x53f9be05, 0x55a6125c, 0x574bb8e6,
 202   0x58ea90c4, 0x5a82799a,
 203   0x5c13539b, 0x5d9cff83, 0x5f1f5ea1, 0x609a52d3, 0x620dbe8b, 0x637984d4,
 204   0x64dd8950, 0x6639b03b,
 205   0x678dde6e, 0x68d9f964, 0x6a1de737, 0x6b598ea3, 0x6c8cd70b, 0x6db7a87a,
 206   0x6ed9eba1, 0x6ff389df,
 207   0x71046d3e, 0x720c8075, 0x730baeed, 0x7401e4c1, 0x74ef0ebc, 0x75d31a61,
 208   0x76adf5e6, 0x777f903c,
 209   0x7847d909, 0x7906c0b0, 0x79bc384d, 0x7a6831ba, 0x7b0a9f8d, 0x7ba3751d,
 210   0x7c32a67e, 0x7cb82885,
 211   0x7d33f0ca, 0x7da5f5a5, 0x7e0e2e32, 0x7e6c9251, 0x7ec11aa5, 0x7f0bc097,
 212   0x7f4c7e54, 0x7f834ed0,
 213   0x7fb02dc6, 0x7fd317b4, 0x7fec09e3, 0x7ffb0260, 0x7fffffff, 0x7ffb0260,
 214   0x7fec09e3, 0x7fd317b4,
 215   0x7fb02dc6, 0x7f834ed0, 0x7f4c7e54, 0x7f0bc097, 0x7ec11aa5, 0x7e6c9251,
 216   0x7e0e2e32, 0x7da5f5a5,
 217   0x7d33f0ca, 0x7cb82885, 0x7c32a67e, 0x7ba3751d, 0x7b0a9f8d, 0x7a6831ba,
 218   0x79bc384d, 0x7906c0b0,
 219   0x7847d909, 0x777f903c, 0x76adf5e6, 0x75d31a61, 0x74ef0ebc, 0x7401e4c1,
 220   0x730baeed, 0x720c8075,
 221   0x71046d3e, 0x6ff389df, 0x6ed9eba1, 0x6db7a87a, 0x6c8cd70b, 0x6b598ea3,
 222   0x6a1de737, 0x68d9f964,
 223   0x678dde6e, 0x6639b03b, 0x64dd8950, 0x637984d4, 0x620dbe8b, 0x609a52d3,
 224   0x5f1f5ea1, 0x5d9cff83,
 225   0x5c13539b, 0x5a82799a, 0x58ea90c4, 0x574bb8e6, 0x55a6125c, 0x53f9be05,
 226   0x5246dd49, 0x508d9211,
 227   0x4ecdfec7, 0x4d084651, 0x4b3c8c12, 0x496af3e2, 0x4793a210, 0x45b6bb5e,
 228   0x43d464fb, 0x41ecc484,
 229   0x40000000, 0x3e0e3ddc, 0x3c17a4e8, 0x3a1c5c57, 0x381c8bb5, 0x36185aee,
 230   0x340ff242, 0x32037a45,
 231   0x2ff31bde, 0x2ddf0040, 0x2bc750e9, 0x29ac37a0, 0x278dde6e, 0x256c6f9f,
 232   0x234815ba, 0x2120fb83,
 233   0x1ef74bf3, 0x1ccb3237, 0x1a9cd9ac, 0x186c6ddd, 0x163a1a7e, 0x14060b68,
 234   0x11d06c97, 0xf996a26,
 235   0xd61304e, 0xb27eb5c, 0x8edc7b7, 0x6b2f1d2, 0x4779632, 0x23be165, 0x0,
 236   0xfdc41e9b,
 237   0xfb8869ce, 0xf94d0e2e, 0xf7123849, 0xf4d814a4, 0xf29ecfb2, 0xf06695da,
 238   0xee2f9369, 0xebf9f498,
 239   0xe9c5e582, 0xe7939223, 0xe5632654, 0xe334cdc9, 0xe108b40d, 0xdedf047d,
 240   0xdcb7ea46, 0xda939061,
 241   0xd8722192, 0xd653c860, 0xd438af17, 0xd220ffc0, 0xd00ce422, 0xcdfc85bb,
 242   0xcbf00dbe, 0xc9e7a512,
 243   0xc7e3744b, 0xc5e3a3a9, 0xc3e85b18, 0xc1f1c224, 0xc0000000, 0xbe133b7c,
 244   0xbc2b9b05, 0xba4944a2,
 245   0xb86c5df0, 0xb6950c1e, 0xb4c373ee, 0xb2f7b9af, 0xb1320139, 0xaf726def,
 246   0xadb922b7, 0xac0641fb,
 247   0xaa59eda4, 0xa8b4471a, 0xa7156f3c, 0xa57d8666, 0xa3ecac65, 0xa263007d,
 248   0xa0e0a15f, 0x9f65ad2d,
 249   0x9df24175, 0x9c867b2c, 0x9b2276b0, 0x99c64fc5, 0x98722192, 0x9726069c,
 250   0x95e218c9, 0x94a6715d,
 251   0x937328f5, 0x92485786, 0x9126145f, 0x900c7621, 0x8efb92c2, 0x8df37f8b,
 252   0x8cf45113, 0x8bfe1b3f,
 253   0x8b10f144, 0x8a2ce59f, 0x89520a1a, 0x88806fc4, 0x87b826f7, 0x86f93f50,
 254   0x8643c7b3, 0x8597ce46,
 255   0x84f56073, 0x845c8ae3, 0x83cd5982, 0x8347d77b, 0x82cc0f36, 0x825a0a5b,
 256   0x81f1d1ce, 0x81936daf,
 257   0x813ee55b, 0x80f43f69, 0x80b381ac, 0x807cb130, 0x804fd23a, 0x802ce84c,
 258   0x8013f61d, 0x8004fda0,
 259
 260 };
 261
 262
 263 /**
 264  * @brief  Q31 sin_cos function.
 265  * @param[in]  theta    scaled input value in degrees
 266  * @param[out] *pSinVal points to the processed sine output.
 267  * @param[out] *pCosVal points to the processed cosine output.
 268  * @return none.
 269  *
 270  * The Q31 input value is in the range [-1 0.999999] and is mapped to a degree value in the range [-180 179].
 271  *
 272  */
 273
 274
 275 void arm_sin_cos_q31(
 276   q31_t theta,
 277   q31_t * pSinVal,
 278   q31_t * pCosVal)
 279 {
 280   q31_t x0;                                      /* Nearest input value */
 281   q31_t y0, y1;                                  /* Nearest output values */
 282   q31_t xSpacing = INPUT_SPACING;                /* Spaing between inputs */
 283   uint32_t i;                                    /* Index */
 284   q31_t oneByXSpacing;                           /* 1/ xSpacing value */
 285   q31_t out;                                     /* temporary variable */
 286   uint32_t sign_bits;                            /* No.of sign bits */
 287   uint32_t firstX = 0x80000000;                  /* First X value */
 288
 289   /* Calculation of index */
 290   i = ((uint32_t) theta - firstX) / (uint32_t) xSpacing;
 291
 292   /* Checking min and max index of table */
 293   if(i >= 359)
 294   {
 295     i = 358;
 296   }
 297
 298   /* Calculation of first nearest input value */
 299   x0 = (q31_t) firstX + ((q31_t) i * xSpacing);
 300
 301   /* Reading nearest sine output values from table */
 302   y0 = sinTableQ31[i];
 303   y1 = sinTableQ31[i + 1u];
 304
 305   /* Calculation of 1/(x1-x0) */
 306   /* (x1-x0) is xSpacing which is fixed value */
 307   sign_bits = 8u;
 308   oneByXSpacing = 0x5A000000;
 309
 310   /* Calculation of (theta - x0)/(x1-x0) */
 311   out =
 312     (((q31_t) (((q63_t) (theta - x0) * oneByXSpacing) >> 32)) << sign_bits);
 313
 314   /* Calculation of y0 + (y1 - y0) * ((theta - x0)/(x1-x0)) */
 315   *pSinVal = __QADD(y0, ((q31_t) (((q63_t) (y1 - y0) * out) >> 30)));
 316
 317   /* Reading nearest cosine output values from table */
 318   y0 = cosTableQ31[i];
 319   y1 = cosTableQ31[i + 1u];
 320
 321   /* Calculation of y0 + (y1 - y0) * ((theta - x0)/(x1-x0)) */
 322   *pCosVal = __QADD(y0, ((q31_t) (((q63_t) (y1 - y0) * out) >> 30)));
 323
 324 }
 325
 326 /**
 327  * @} end of SinCos group
 328  */