arm_sin_cos_q31.c

00001 /* ----------------------------------------------------------------------    
00002 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.    
00003 *    
00004 * $Date:        17. January 2013
00005 * $Revision:    V1.4.1
00006 *    
00007 * Project:      CMSIS DSP Library    
00008 * Title:        arm_sin_cos_q31.c    
00009 *    
00010 * Description:  Cosine & Sine calculation for Q31 values.   
00011 *    
00012 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
00013 *  
00014 * Redistribution and use in source and binary forms, with or without 
00015 * modification, are permitted provided that the following conditions
00016 * are met:
00017 *   - Redistributions of source code must retain the above copyright
00018 *     notice, this list of conditions and the following disclaimer.
00019 *   - Redistributions in binary form must reproduce the above copyright
00020 *     notice, this list of conditions and the following disclaimer in
00021 *     the documentation and/or other materials provided with the 
00022 *     distribution.
00023 *   - Neither the name of ARM LIMITED nor the names of its contributors
00024 *     may be used to endorse or promote products derived from this
00025 *     software without specific prior written permission.
00026 *
00027 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00028 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00029 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00030 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
00031 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00032 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00033 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00034 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00035 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00036 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00037 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00038 * POSSIBILITY OF SUCH DAMAGE.    
00039 * -------------------------------------------------------------------- */
00040 
00041 #include "arm_math.h"
00042 
00043 /**    
00044  * @ingroup groupController    
00045  */
00046 
00047  /**    
00048  * @addtogroup SinCos    
00049  * @{    
00050  */
00051 
00052 /**    
00053 * \par    
00054 * Sine Table is generated from following loop    
00055 * <pre>for(i = 0; i < 360; i++)    
00056 * {    
00057 *    sinTable[i]= sin((i-180) * PI/180.0);    
00058 * } </pre>   
00059 * Convert above coefficients to fixed point 1.31 format.    
00060 */
00061 
00062 static const int32_t sinTableQ31 [360] = {
00063 
00064   0x0, 0xfdc41e9b, 0xfb8869ce, 0xf94d0e2e, 0xf7123849, 0xf4d814a4, 0xf29ecfb2,
00065   0xf06695da,
00066   0xee2f9369, 0xebf9f498, 0xe9c5e582, 0xe7939223, 0xe5632654, 0xe334cdc9,
00067   0xe108b40d, 0xdedf047d,
00068   0xdcb7ea46, 0xda939061, 0xd8722192, 0xd653c860, 0xd438af17, 0xd220ffc0,
00069   0xd00ce422, 0xcdfc85bb,
00070   0xcbf00dbe, 0xc9e7a512, 0xc7e3744b, 0xc5e3a3a9, 0xc3e85b18, 0xc1f1c224,
00071   0xc0000000, 0xbe133b7c,
00072   0xbc2b9b05, 0xba4944a2, 0xb86c5df0, 0xb6950c1e, 0xb4c373ee, 0xb2f7b9af,
00073   0xb1320139, 0xaf726def,
00074   0xadb922b7, 0xac0641fb, 0xaa59eda4, 0xa8b4471a, 0xa7156f3c, 0xa57d8666,
00075   0xa3ecac65, 0xa263007d,
00076   0xa0e0a15f, 0x9f65ad2d, 0x9df24175, 0x9c867b2c, 0x9b2276b0, 0x99c64fc5,
00077   0x98722192, 0x9726069c,
00078   0x95e218c9, 0x94a6715d, 0x937328f5, 0x92485786, 0x9126145f, 0x900c7621,
00079   0x8efb92c2, 0x8df37f8b,
00080   0x8cf45113, 0x8bfe1b3f, 0x8b10f144, 0x8a2ce59f, 0x89520a1a, 0x88806fc4,
00081   0x87b826f7, 0x86f93f50,
00082   0x8643c7b3, 0x8597ce46, 0x84f56073, 0x845c8ae3, 0x83cd5982, 0x8347d77b,
00083   0x82cc0f36, 0x825a0a5b,
00084   0x81f1d1ce, 0x81936daf, 0x813ee55b, 0x80f43f69, 0x80b381ac, 0x807cb130,
00085   0x804fd23a, 0x802ce84c,
00086   0x8013f61d, 0x8004fda0, 0x80000000, 0x8004fda0, 0x8013f61d, 0x802ce84c,
00087   0x804fd23a, 0x807cb130,
00088   0x80b381ac, 0x80f43f69, 0x813ee55b, 0x81936daf, 0x81f1d1ce, 0x825a0a5b,
00089   0x82cc0f36, 0x8347d77b,
00090   0x83cd5982, 0x845c8ae3, 0x84f56073, 0x8597ce46, 0x8643c7b3, 0x86f93f50,
00091   0x87b826f7, 0x88806fc4,
00092   0x89520a1a, 0x8a2ce59f, 0x8b10f144, 0x8bfe1b3f, 0x8cf45113, 0x8df37f8b,
00093   0x8efb92c2, 0x900c7621,
00094   0x9126145f, 0x92485786, 0x937328f5, 0x94a6715d, 0x95e218c9, 0x9726069c,
00095   0x98722192, 0x99c64fc5,
00096   0x9b2276b0, 0x9c867b2c, 0x9df24175, 0x9f65ad2d, 0xa0e0a15f, 0xa263007d,
00097   0xa3ecac65, 0xa57d8666,
00098   0xa7156f3c, 0xa8b4471a, 0xaa59eda4, 0xac0641fb, 0xadb922b7, 0xaf726def,
00099   0xb1320139, 0xb2f7b9af,
00100   0xb4c373ee, 0xb6950c1e, 0xb86c5df0, 0xba4944a2, 0xbc2b9b05, 0xbe133b7c,
00101   0xc0000000, 0xc1f1c224,
00102   0xc3e85b18, 0xc5e3a3a9, 0xc7e3744b, 0xc9e7a512, 0xcbf00dbe, 0xcdfc85bb,
00103   0xd00ce422, 0xd220ffc0,
00104   0xd438af17, 0xd653c860, 0xd8722192, 0xda939061, 0xdcb7ea46, 0xdedf047d,
00105   0xe108b40d, 0xe334cdc9,
00106   0xe5632654, 0xe7939223, 0xe9c5e582, 0xebf9f498, 0xee2f9369, 0xf06695da,
00107   0xf29ecfb2, 0xf4d814a4,
00108   0xf7123849, 0xf94d0e2e, 0xfb8869ce, 0xfdc41e9b, 0x0, 0x23be165, 0x4779632,
00109   0x6b2f1d2,
00110   0x8edc7b7, 0xb27eb5c, 0xd61304e, 0xf996a26, 0x11d06c97, 0x14060b68,
00111   0x163a1a7e, 0x186c6ddd,
00112   0x1a9cd9ac, 0x1ccb3237, 0x1ef74bf3, 0x2120fb83, 0x234815ba, 0x256c6f9f,
00113   0x278dde6e, 0x29ac37a0,
00114   0x2bc750e9, 0x2ddf0040, 0x2ff31bde, 0x32037a45, 0x340ff242, 0x36185aee,
00115   0x381c8bb5, 0x3a1c5c57,
00116   0x3c17a4e8, 0x3e0e3ddc, 0x40000000, 0x41ecc484, 0x43d464fb, 0x45b6bb5e,
00117   0x4793a210, 0x496af3e2,
00118   0x4b3c8c12, 0x4d084651, 0x4ecdfec7, 0x508d9211, 0x5246dd49, 0x53f9be05,
00119   0x55a6125c, 0x574bb8e6,
00120   0x58ea90c4, 0x5a82799a, 0x5c13539b, 0x5d9cff83, 0x5f1f5ea1, 0x609a52d3,
00121   0x620dbe8b, 0x637984d4,
00122   0x64dd8950, 0x6639b03b, 0x678dde6e, 0x68d9f964, 0x6a1de737, 0x6b598ea3,
00123   0x6c8cd70b, 0x6db7a87a,
00124   0x6ed9eba1, 0x6ff389df, 0x71046d3e, 0x720c8075, 0x730baeed, 0x7401e4c1,
00125   0x74ef0ebc, 0x75d31a61,
00126   0x76adf5e6, 0x777f903c, 0x7847d909, 0x7906c0b0, 0x79bc384d, 0x7a6831ba,
00127   0x7b0a9f8d, 0x7ba3751d,
00128   0x7c32a67e, 0x7cb82885, 0x7d33f0ca, 0x7da5f5a5, 0x7e0e2e32, 0x7e6c9251,
00129   0x7ec11aa5, 0x7f0bc097,
00130   0x7f4c7e54, 0x7f834ed0, 0x7fb02dc6, 0x7fd317b4, 0x7fec09e3, 0x7ffb0260,
00131   0x7fffffff, 0x7ffb0260,
00132   0x7fec09e3, 0x7fd317b4, 0x7fb02dc6, 0x7f834ed0, 0x7f4c7e54, 0x7f0bc097,
00133   0x7ec11aa5, 0x7e6c9251,
00134   0x7e0e2e32, 0x7da5f5a5, 0x7d33f0ca, 0x7cb82885, 0x7c32a67e, 0x7ba3751d,
00135   0x7b0a9f8d, 0x7a6831ba,
00136   0x79bc384d, 0x7906c0b0, 0x7847d909, 0x777f903c, 0x76adf5e6, 0x75d31a61,
00137   0x74ef0ebc, 0x7401e4c1,
00138   0x730baeed, 0x720c8075, 0x71046d3e, 0x6ff389df, 0x6ed9eba1, 0x6db7a87a,
00139   0x6c8cd70b, 0x6b598ea3,
00140   0x6a1de737, 0x68d9f964, 0x678dde6e, 0x6639b03b, 0x64dd8950, 0x637984d4,
00141   0x620dbe8b, 0x609a52d3,
00142   0x5f1f5ea1, 0x5d9cff83, 0x5c13539b, 0x5a82799a, 0x58ea90c4, 0x574bb8e6,
00143   0x55a6125c, 0x53f9be05,
00144   0x5246dd49, 0x508d9211, 0x4ecdfec7, 0x4d084651, 0x4b3c8c12, 0x496af3e2,
00145   0x4793a210, 0x45b6bb5e,
00146   0x43d464fb, 0x41ecc484, 0x40000000, 0x3e0e3ddc, 0x3c17a4e8, 0x3a1c5c57,
00147   0x381c8bb5, 0x36185aee,
00148   0x340ff242, 0x32037a45, 0x2ff31bde, 0x2ddf0040, 0x2bc750e9, 0x29ac37a0,
00149   0x278dde6e, 0x256c6f9f,
00150   0x234815ba, 0x2120fb83, 0x1ef74bf3, 0x1ccb3237, 0x1a9cd9ac, 0x186c6ddd,
00151   0x163a1a7e, 0x14060b68,
00152   0x11d06c97, 0xf996a26, 0xd61304e, 0xb27eb5c, 0x8edc7b7, 0x6b2f1d2,
00153   0x4779632, 0x23be165,
00154 
00155 
00156 };
00157 
00158 /**    
00159 * \par    
00160 * Cosine Table is generated from following loop    
00161 * <pre>for(i = 0; i < 360; i++)    
00162 * {    
00163 *    cosTable[i]= cos((i-180) * PI/180.0);    
00164 * } </pre>   
00165 * \par    
00166 * Convert above coefficients to fixed point 1.31 format.    
00167 */
00168 static const int32_t cosTableQ31 [360] = {
00169   0x80000000, 0x8004fda0, 0x8013f61d, 0x802ce84c, 0x804fd23a, 0x807cb130,
00170   0x80b381ac, 0x80f43f69,
00171   0x813ee55b, 0x81936daf, 0x81f1d1ce, 0x825a0a5b, 0x82cc0f36, 0x8347d77b,
00172   0x83cd5982, 0x845c8ae3,
00173   0x84f56073, 0x8597ce46, 0x8643c7b3, 0x86f93f50, 0x87b826f7, 0x88806fc4,
00174   0x89520a1a, 0x8a2ce59f,
00175   0x8b10f144, 0x8bfe1b3f, 0x8cf45113, 0x8df37f8b, 0x8efb92c2, 0x900c7621,
00176   0x9126145f, 0x92485786,
00177   0x937328f5, 0x94a6715d, 0x95e218c9, 0x9726069c, 0x98722192, 0x99c64fc5,
00178   0x9b2276b0, 0x9c867b2c,
00179   0x9df24175, 0x9f65ad2d, 0xa0e0a15f, 0xa263007d, 0xa3ecac65, 0xa57d8666,
00180   0xa7156f3c, 0xa8b4471a,
00181   0xaa59eda4, 0xac0641fb, 0xadb922b7, 0xaf726def, 0xb1320139, 0xb2f7b9af,
00182   0xb4c373ee, 0xb6950c1e,
00183   0xb86c5df0, 0xba4944a2, 0xbc2b9b05, 0xbe133b7c, 0xc0000000, 0xc1f1c224,
00184   0xc3e85b18, 0xc5e3a3a9,
00185   0xc7e3744b, 0xc9e7a512, 0xcbf00dbe, 0xcdfc85bb, 0xd00ce422, 0xd220ffc0,
00186   0xd438af17, 0xd653c860,
00187   0xd8722192, 0xda939061, 0xdcb7ea46, 0xdedf047d, 0xe108b40d, 0xe334cdc9,
00188   0xe5632654, 0xe7939223,
00189   0xe9c5e582, 0xebf9f498, 0xee2f9369, 0xf06695da, 0xf29ecfb2, 0xf4d814a4,
00190   0xf7123849, 0xf94d0e2e,
00191   0xfb8869ce, 0xfdc41e9b, 0x0, 0x23be165, 0x4779632, 0x6b2f1d2, 0x8edc7b7,
00192   0xb27eb5c,
00193   0xd61304e, 0xf996a26, 0x11d06c97, 0x14060b68, 0x163a1a7e, 0x186c6ddd,
00194   0x1a9cd9ac, 0x1ccb3237,
00195   0x1ef74bf3, 0x2120fb83, 0x234815ba, 0x256c6f9f, 0x278dde6e, 0x29ac37a0,
00196   0x2bc750e9, 0x2ddf0040,
00197   0x2ff31bde, 0x32037a45, 0x340ff242, 0x36185aee, 0x381c8bb5, 0x3a1c5c57,
00198   0x3c17a4e8, 0x3e0e3ddc,
00199   0x40000000, 0x41ecc484, 0x43d464fb, 0x45b6bb5e, 0x4793a210, 0x496af3e2,
00200   0x4b3c8c12, 0x4d084651,
00201   0x4ecdfec7, 0x508d9211, 0x5246dd49, 0x53f9be05, 0x55a6125c, 0x574bb8e6,
00202   0x58ea90c4, 0x5a82799a,
00203   0x5c13539b, 0x5d9cff83, 0x5f1f5ea1, 0x609a52d3, 0x620dbe8b, 0x637984d4,
00204   0x64dd8950, 0x6639b03b,
00205   0x678dde6e, 0x68d9f964, 0x6a1de737, 0x6b598ea3, 0x6c8cd70b, 0x6db7a87a,
00206   0x6ed9eba1, 0x6ff389df,
00207   0x71046d3e, 0x720c8075, 0x730baeed, 0x7401e4c1, 0x74ef0ebc, 0x75d31a61,
00208   0x76adf5e6, 0x777f903c,
00209   0x7847d909, 0x7906c0b0, 0x79bc384d, 0x7a6831ba, 0x7b0a9f8d, 0x7ba3751d,
00210   0x7c32a67e, 0x7cb82885,
00211   0x7d33f0ca, 0x7da5f5a5, 0x7e0e2e32, 0x7e6c9251, 0x7ec11aa5, 0x7f0bc097,
00212   0x7f4c7e54, 0x7f834ed0,
00213   0x7fb02dc6, 0x7fd317b4, 0x7fec09e3, 0x7ffb0260, 0x7fffffff, 0x7ffb0260,
00214   0x7fec09e3, 0x7fd317b4,
00215   0x7fb02dc6, 0x7f834ed0, 0x7f4c7e54, 0x7f0bc097, 0x7ec11aa5, 0x7e6c9251,
00216   0x7e0e2e32, 0x7da5f5a5,
00217   0x7d33f0ca, 0x7cb82885, 0x7c32a67e, 0x7ba3751d, 0x7b0a9f8d, 0x7a6831ba,
00218   0x79bc384d, 0x7906c0b0,
00219   0x7847d909, 0x777f903c, 0x76adf5e6, 0x75d31a61, 0x74ef0ebc, 0x7401e4c1,
00220   0x730baeed, 0x720c8075,
00221   0x71046d3e, 0x6ff389df, 0x6ed9eba1, 0x6db7a87a, 0x6c8cd70b, 0x6b598ea3,
00222   0x6a1de737, 0x68d9f964,
00223   0x678dde6e, 0x6639b03b, 0x64dd8950, 0x637984d4, 0x620dbe8b, 0x609a52d3,
00224   0x5f1f5ea1, 0x5d9cff83,
00225   0x5c13539b, 0x5a82799a, 0x58ea90c4, 0x574bb8e6, 0x55a6125c, 0x53f9be05,
00226   0x5246dd49, 0x508d9211,
00227   0x4ecdfec7, 0x4d084651, 0x4b3c8c12, 0x496af3e2, 0x4793a210, 0x45b6bb5e,
00228   0x43d464fb, 0x41ecc484,
00229   0x40000000, 0x3e0e3ddc, 0x3c17a4e8, 0x3a1c5c57, 0x381c8bb5, 0x36185aee,
00230   0x340ff242, 0x32037a45,
00231   0x2ff31bde, 0x2ddf0040, 0x2bc750e9, 0x29ac37a0, 0x278dde6e, 0x256c6f9f,
00232   0x234815ba, 0x2120fb83,
00233   0x1ef74bf3, 0x1ccb3237, 0x1a9cd9ac, 0x186c6ddd, 0x163a1a7e, 0x14060b68,
00234   0x11d06c97, 0xf996a26,
00235   0xd61304e, 0xb27eb5c, 0x8edc7b7, 0x6b2f1d2, 0x4779632, 0x23be165, 0x0,
00236   0xfdc41e9b,
00237   0xfb8869ce, 0xf94d0e2e, 0xf7123849, 0xf4d814a4, 0xf29ecfb2, 0xf06695da,
00238   0xee2f9369, 0xebf9f498,
00239   0xe9c5e582, 0xe7939223, 0xe5632654, 0xe334cdc9, 0xe108b40d, 0xdedf047d,
00240   0xdcb7ea46, 0xda939061,
00241   0xd8722192, 0xd653c860, 0xd438af17, 0xd220ffc0, 0xd00ce422, 0xcdfc85bb,
00242   0xcbf00dbe, 0xc9e7a512,
00243   0xc7e3744b, 0xc5e3a3a9, 0xc3e85b18, 0xc1f1c224, 0xc0000000, 0xbe133b7c,
00244   0xbc2b9b05, 0xba4944a2,
00245   0xb86c5df0, 0xb6950c1e, 0xb4c373ee, 0xb2f7b9af, 0xb1320139, 0xaf726def,
00246   0xadb922b7, 0xac0641fb,
00247   0xaa59eda4, 0xa8b4471a, 0xa7156f3c, 0xa57d8666, 0xa3ecac65, 0xa263007d,
00248   0xa0e0a15f, 0x9f65ad2d,
00249   0x9df24175, 0x9c867b2c, 0x9b2276b0, 0x99c64fc5, 0x98722192, 0x9726069c,
00250   0x95e218c9, 0x94a6715d,
00251   0x937328f5, 0x92485786, 0x9126145f, 0x900c7621, 0x8efb92c2, 0x8df37f8b,
00252   0x8cf45113, 0x8bfe1b3f,
00253   0x8b10f144, 0x8a2ce59f, 0x89520a1a, 0x88806fc4, 0x87b826f7, 0x86f93f50,
00254   0x8643c7b3, 0x8597ce46,
00255   0x84f56073, 0x845c8ae3, 0x83cd5982, 0x8347d77b, 0x82cc0f36, 0x825a0a5b,
00256   0x81f1d1ce, 0x81936daf,
00257   0x813ee55b, 0x80f43f69, 0x80b381ac, 0x807cb130, 0x804fd23a, 0x802ce84c,
00258   0x8013f61d, 0x8004fda0,
00259 
00260 };
00261 
00262 
00263 /**    
00264  * @brief  Q31 sin_cos function.   
00265  * @param[in]  theta    scaled input value in degrees    
00266  * @param[out] *pSinVal points to the processed sine output.    
00267  * @param[out] *pCosVal points to the processed cosine output.    
00268  * @return none.   
00269  *    
00270  * The Q31 input value is in the range [-1 0.999999] and is mapped to a degree value in the range [-180 179].   
00271  *    
00272  */
00273 
00274 
00275 void arm_sin_cos_q31(
00276   q31_t theta,
00277   q31_t * pSinVal,
00278   q31_t * pCosVal)
00279 {
00280   q31_t x0;                                      /* Nearest input value */
00281   q31_t y0, y1;                                  /* Nearest output values */
00282   q31_t xSpacing = INPUT_SPACING;                /* Spaing between inputs */
00283   uint32_t i;                                    /* Index */
00284   q31_t oneByXSpacing;                           /* 1/ xSpacing value */
00285   q31_t out;                                     /* temporary variable */
00286   uint32_t sign_bits;                            /* No.of sign bits */
00287   uint32_t firstX = 0x80000000;                  /* First X value */
00288 
00289   /* Calculation of index */
00290   i = ((uint32_t) theta - firstX) / (uint32_t) xSpacing;
00291 
00292   /* Checking min and max index of table */
00293   if(i >= 359)
00294   {
00295     i = 358;
00296   }
00297 
00298   /* Calculation of first nearest input value */
00299   x0 = (q31_t) firstX + ((q31_t) i * xSpacing);
00300 
00301   /* Reading nearest sine output values from table */
00302   y0 = sinTableQ31 [i];
00303   y1 = sinTableQ31 [i + 1u];
00304 
00305   /* Calculation of 1/(x1-x0) */
00306   /* (x1-x0) is xSpacing which is fixed value */
00307   sign_bits = 8u;
00308   oneByXSpacing = 0x5A000000;
00309 
00310   /* Calculation of (theta - x0)/(x1-x0) */
00311   out =
00312     (((q31_t) (((q63_t) (theta - x0) * oneByXSpacing) >> 32)) << sign_bits);
00313 
00314   /* Calculation of y0 + (y1 - y0) * ((theta - x0)/(x1-x0)) */
00315   *pSinVal = __QADD(y0, ((q31_t) (((q63_t) (y1 - y0) * out) >> 30)));
00316 
00317   /* Reading nearest cosine output values from table */
00318   y0 = cosTableQ31 [i];
00319   y1 = cosTableQ31 [i + 1u];
00320 
00321   /* Calculation of y0 + (y1 - y0) * ((theta - x0)/(x1-x0)) */
00322   *pCosVal = __QADD(y0, ((q31_t) (((q63_t) (y1 - y0) * out) >> 30)));
00323 
00324 }
00325 
00326 /**    
00327  * @} end of SinCos group    
00328  */