arm_sin_cos_q31.c

00001 /* ----------------------------------------------------------------------  
00002 * Copyright (C) 2010 ARM Limited. All rights reserved.  
00003 *  
00004 * $Date:        29. November 2010  
00005 * $Revision:    V1.0.3  
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
00013 *  
00014 * Version 1.0.3 2010/11/29 
00015 *    Re-organized the CMSIS folders and updated documentation.  
00016 *   
00017 * Version 1.0.2 2010/11/11  
00018 *    Documentation updated.   
00019 *  
00020 * Version 1.0.1 2010/10/05   
00021 *    Production release and review comments incorporated.  
00022 *  
00023 * Version 1.0.0 2010/09/20   
00024 *    Production release and review comments incorporated.  
00025 * -------------------------------------------------------------------- */ 
00026  
00027 #include "arm_math.h" 
00028  
00029 /**  
00030  * @ingroup groupController  
00031  */ 
00032  
00033  /**  
00034  * @addtogroup SinCos  
00035  * @{  
00036  */ 
00037  
00038 /**  
00039 * \par  
00040 * Sine Table is generated from following loop  
00041 * <pre>for(i = 0; i < 360; i++)  
00042 * {  
00043 *    sinTable[i]= sin((i-180) * PI/180.0);  
00044 * } </pre> 
00045 * Convert above coefficients to fixed point 1.31 format.  
00046 */ 
00047  
00048 static const int32_t sinTableQ31 [360] = { 
00049  
00050   0x0, 0xfdc41e9b, 0xfb8869ce, 0xf94d0e2e, 0xf7123849, 0xf4d814a4, 0xf29ecfb2, 
00051   0xf06695da, 
00052   0xee2f9369, 0xebf9f498, 0xe9c5e582, 0xe7939223, 0xe5632654, 0xe334cdc9, 
00053   0xe108b40d, 0xdedf047d, 
00054   0xdcb7ea46, 0xda939061, 0xd8722192, 0xd653c860, 0xd438af17, 0xd220ffc0, 
00055   0xd00ce422, 0xcdfc85bb, 
00056   0xcbf00dbe, 0xc9e7a512, 0xc7e3744b, 0xc5e3a3a9, 0xc3e85b18, 0xc1f1c224, 
00057   0xc0000000, 0xbe133b7c, 
00058   0xbc2b9b05, 0xba4944a2, 0xb86c5df0, 0xb6950c1e, 0xb4c373ee, 0xb2f7b9af, 
00059   0xb1320139, 0xaf726def, 
00060   0xadb922b7, 0xac0641fb, 0xaa59eda4, 0xa8b4471a, 0xa7156f3c, 0xa57d8666, 
00061   0xa3ecac65, 0xa263007d, 
00062   0xa0e0a15f, 0x9f65ad2d, 0x9df24175, 0x9c867b2c, 0x9b2276b0, 0x99c64fc5, 
00063   0x98722192, 0x9726069c, 
00064   0x95e218c9, 0x94a6715d, 0x937328f5, 0x92485786, 0x9126145f, 0x900c7621, 
00065   0x8efb92c2, 0x8df37f8b, 
00066   0x8cf45113, 0x8bfe1b3f, 0x8b10f144, 0x8a2ce59f, 0x89520a1a, 0x88806fc4, 
00067   0x87b826f7, 0x86f93f50, 
00068   0x8643c7b3, 0x8597ce46, 0x84f56073, 0x845c8ae3, 0x83cd5982, 0x8347d77b, 
00069   0x82cc0f36, 0x825a0a5b, 
00070   0x81f1d1ce, 0x81936daf, 0x813ee55b, 0x80f43f69, 0x80b381ac, 0x807cb130, 
00071   0x804fd23a, 0x802ce84c, 
00072   0x8013f61d, 0x8004fda0, 0x80000000, 0x8004fda0, 0x8013f61d, 0x802ce84c, 
00073   0x804fd23a, 0x807cb130, 
00074   0x80b381ac, 0x80f43f69, 0x813ee55b, 0x81936daf, 0x81f1d1ce, 0x825a0a5b, 
00075   0x82cc0f36, 0x8347d77b, 
00076   0x83cd5982, 0x845c8ae3, 0x84f56073, 0x8597ce46, 0x8643c7b3, 0x86f93f50, 
00077   0x87b826f7, 0x88806fc4, 
00078   0x89520a1a, 0x8a2ce59f, 0x8b10f144, 0x8bfe1b3f, 0x8cf45113, 0x8df37f8b, 
00079   0x8efb92c2, 0x900c7621, 
00080   0x9126145f, 0x92485786, 0x937328f5, 0x94a6715d, 0x95e218c9, 0x9726069c, 
00081   0x98722192, 0x99c64fc5, 
00082   0x9b2276b0, 0x9c867b2c, 0x9df24175, 0x9f65ad2d, 0xa0e0a15f, 0xa263007d, 
00083   0xa3ecac65, 0xa57d8666, 
00084   0xa7156f3c, 0xa8b4471a, 0xaa59eda4, 0xac0641fb, 0xadb922b7, 0xaf726def, 
00085   0xb1320139, 0xb2f7b9af, 
00086   0xb4c373ee, 0xb6950c1e, 0xb86c5df0, 0xba4944a2, 0xbc2b9b05, 0xbe133b7c, 
00087   0xc0000000, 0xc1f1c224, 
00088   0xc3e85b18, 0xc5e3a3a9, 0xc7e3744b, 0xc9e7a512, 0xcbf00dbe, 0xcdfc85bb, 
00089   0xd00ce422, 0xd220ffc0, 
00090   0xd438af17, 0xd653c860, 0xd8722192, 0xda939061, 0xdcb7ea46, 0xdedf047d, 
00091   0xe108b40d, 0xe334cdc9, 
00092   0xe5632654, 0xe7939223, 0xe9c5e582, 0xebf9f498, 0xee2f9369, 0xf06695da, 
00093   0xf29ecfb2, 0xf4d814a4, 
00094   0xf7123849, 0xf94d0e2e, 0xfb8869ce, 0xfdc41e9b, 0x0, 0x23be165, 0x4779632, 
00095   0x6b2f1d2, 
00096   0x8edc7b7, 0xb27eb5c, 0xd61304e, 0xf996a26, 0x11d06c97, 0x14060b68, 
00097   0x163a1a7e, 0x186c6ddd, 
00098   0x1a9cd9ac, 0x1ccb3237, 0x1ef74bf3, 0x2120fb83, 0x234815ba, 0x256c6f9f, 
00099   0x278dde6e, 0x29ac37a0, 
00100   0x2bc750e9, 0x2ddf0040, 0x2ff31bde, 0x32037a45, 0x340ff242, 0x36185aee, 
00101   0x381c8bb5, 0x3a1c5c57, 
00102   0x3c17a4e8, 0x3e0e3ddc, 0x40000000, 0x41ecc484, 0x43d464fb, 0x45b6bb5e, 
00103   0x4793a210, 0x496af3e2, 
00104   0x4b3c8c12, 0x4d084651, 0x4ecdfec7, 0x508d9211, 0x5246dd49, 0x53f9be05, 
00105   0x55a6125c, 0x574bb8e6, 
00106   0x58ea90c4, 0x5a82799a, 0x5c13539b, 0x5d9cff83, 0x5f1f5ea1, 0x609a52d3, 
00107   0x620dbe8b, 0x637984d4, 
00108   0x64dd8950, 0x6639b03b, 0x678dde6e, 0x68d9f964, 0x6a1de737, 0x6b598ea3, 
00109   0x6c8cd70b, 0x6db7a87a, 
00110   0x6ed9eba1, 0x6ff389df, 0x71046d3e, 0x720c8075, 0x730baeed, 0x7401e4c1, 
00111   0x74ef0ebc, 0x75d31a61, 
00112   0x76adf5e6, 0x777f903c, 0x7847d909, 0x7906c0b0, 0x79bc384d, 0x7a6831ba, 
00113   0x7b0a9f8d, 0x7ba3751d, 
00114   0x7c32a67e, 0x7cb82885, 0x7d33f0ca, 0x7da5f5a5, 0x7e0e2e32, 0x7e6c9251, 
00115   0x7ec11aa5, 0x7f0bc097, 
00116   0x7f4c7e54, 0x7f834ed0, 0x7fb02dc6, 0x7fd317b4, 0x7fec09e3, 0x7ffb0260, 
00117   0x7fffffff, 0x7ffb0260, 
00118   0x7fec09e3, 0x7fd317b4, 0x7fb02dc6, 0x7f834ed0, 0x7f4c7e54, 0x7f0bc097, 
00119   0x7ec11aa5, 0x7e6c9251, 
00120   0x7e0e2e32, 0x7da5f5a5, 0x7d33f0ca, 0x7cb82885, 0x7c32a67e, 0x7ba3751d, 
00121   0x7b0a9f8d, 0x7a6831ba, 
00122   0x79bc384d, 0x7906c0b0, 0x7847d909, 0x777f903c, 0x76adf5e6, 0x75d31a61, 
00123   0x74ef0ebc, 0x7401e4c1, 
00124   0x730baeed, 0x720c8075, 0x71046d3e, 0x6ff389df, 0x6ed9eba1, 0x6db7a87a, 
00125   0x6c8cd70b, 0x6b598ea3, 
00126   0x6a1de737, 0x68d9f964, 0x678dde6e, 0x6639b03b, 0x64dd8950, 0x637984d4, 
00127   0x620dbe8b, 0x609a52d3, 
00128   0x5f1f5ea1, 0x5d9cff83, 0x5c13539b, 0x5a82799a, 0x58ea90c4, 0x574bb8e6, 
00129   0x55a6125c, 0x53f9be05, 
00130   0x5246dd49, 0x508d9211, 0x4ecdfec7, 0x4d084651, 0x4b3c8c12, 0x496af3e2, 
00131   0x4793a210, 0x45b6bb5e, 
00132   0x43d464fb, 0x41ecc484, 0x40000000, 0x3e0e3ddc, 0x3c17a4e8, 0x3a1c5c57, 
00133   0x381c8bb5, 0x36185aee, 
00134   0x340ff242, 0x32037a45, 0x2ff31bde, 0x2ddf0040, 0x2bc750e9, 0x29ac37a0, 
00135   0x278dde6e, 0x256c6f9f, 
00136   0x234815ba, 0x2120fb83, 0x1ef74bf3, 0x1ccb3237, 0x1a9cd9ac, 0x186c6ddd, 
00137   0x163a1a7e, 0x14060b68, 
00138   0x11d06c97, 0xf996a26, 0xd61304e, 0xb27eb5c, 0x8edc7b7, 0x6b2f1d2, 
00139   0x4779632, 0x23be165, 
00140  
00141  
00142 }; 
00143  
00144 /**  
00145 * \par  
00146 * Cosine Table is generated from following loop  
00147 * <pre>for(i = 0; i < 360; i++)  
00148 * {  
00149 *    cosTable[i]= cos((i-180) * PI/180.0);  
00150 * } </pre> 
00151 * \par  
00152 * Convert above coefficients to fixed point 1.31 format.  
00153 */ 
00154 static const int32_t cosTableQ31 [360] = { 
00155   0x80000000, 0x8004fda0, 0x8013f61d, 0x802ce84c, 0x804fd23a, 0x807cb130, 
00156   0x80b381ac, 0x80f43f69, 
00157   0x813ee55b, 0x81936daf, 0x81f1d1ce, 0x825a0a5b, 0x82cc0f36, 0x8347d77b, 
00158   0x83cd5982, 0x845c8ae3, 
00159   0x84f56073, 0x8597ce46, 0x8643c7b3, 0x86f93f50, 0x87b826f7, 0x88806fc4, 
00160   0x89520a1a, 0x8a2ce59f, 
00161   0x8b10f144, 0x8bfe1b3f, 0x8cf45113, 0x8df37f8b, 0x8efb92c2, 0x900c7621, 
00162   0x9126145f, 0x92485786, 
00163   0x937328f5, 0x94a6715d, 0x95e218c9, 0x9726069c, 0x98722192, 0x99c64fc5, 
00164   0x9b2276b0, 0x9c867b2c, 
00165   0x9df24175, 0x9f65ad2d, 0xa0e0a15f, 0xa263007d, 0xa3ecac65, 0xa57d8666, 
00166   0xa7156f3c, 0xa8b4471a, 
00167   0xaa59eda4, 0xac0641fb, 0xadb922b7, 0xaf726def, 0xb1320139, 0xb2f7b9af, 
00168   0xb4c373ee, 0xb6950c1e, 
00169   0xb86c5df0, 0xba4944a2, 0xbc2b9b05, 0xbe133b7c, 0xc0000000, 0xc1f1c224, 
00170   0xc3e85b18, 0xc5e3a3a9, 
00171   0xc7e3744b, 0xc9e7a512, 0xcbf00dbe, 0xcdfc85bb, 0xd00ce422, 0xd220ffc0, 
00172   0xd438af17, 0xd653c860, 
00173   0xd8722192, 0xda939061, 0xdcb7ea46, 0xdedf047d, 0xe108b40d, 0xe334cdc9, 
00174   0xe5632654, 0xe7939223, 
00175   0xe9c5e582, 0xebf9f498, 0xee2f9369, 0xf06695da, 0xf29ecfb2, 0xf4d814a4, 
00176   0xf7123849, 0xf94d0e2e, 
00177   0xfb8869ce, 0xfdc41e9b, 0x0, 0x23be165, 0x4779632, 0x6b2f1d2, 0x8edc7b7, 
00178   0xb27eb5c, 
00179   0xd61304e, 0xf996a26, 0x11d06c97, 0x14060b68, 0x163a1a7e, 0x186c6ddd, 
00180   0x1a9cd9ac, 0x1ccb3237, 
00181   0x1ef74bf3, 0x2120fb83, 0x234815ba, 0x256c6f9f, 0x278dde6e, 0x29ac37a0, 
00182   0x2bc750e9, 0x2ddf0040, 
00183   0x2ff31bde, 0x32037a45, 0x340ff242, 0x36185aee, 0x381c8bb5, 0x3a1c5c57, 
00184   0x3c17a4e8, 0x3e0e3ddc, 
00185   0x40000000, 0x41ecc484, 0x43d464fb, 0x45b6bb5e, 0x4793a210, 0x496af3e2, 
00186   0x4b3c8c12, 0x4d084651, 
00187   0x4ecdfec7, 0x508d9211, 0x5246dd49, 0x53f9be05, 0x55a6125c, 0x574bb8e6, 
00188   0x58ea90c4, 0x5a82799a, 
00189   0x5c13539b, 0x5d9cff83, 0x5f1f5ea1, 0x609a52d3, 0x620dbe8b, 0x637984d4, 
00190   0x64dd8950, 0x6639b03b, 
00191   0x678dde6e, 0x68d9f964, 0x6a1de737, 0x6b598ea3, 0x6c8cd70b, 0x6db7a87a, 
00192   0x6ed9eba1, 0x6ff389df, 
00193   0x71046d3e, 0x720c8075, 0x730baeed, 0x7401e4c1, 0x74ef0ebc, 0x75d31a61, 
00194   0x76adf5e6, 0x777f903c, 
00195   0x7847d909, 0x7906c0b0, 0x79bc384d, 0x7a6831ba, 0x7b0a9f8d, 0x7ba3751d, 
00196   0x7c32a67e, 0x7cb82885, 
00197   0x7d33f0ca, 0x7da5f5a5, 0x7e0e2e32, 0x7e6c9251, 0x7ec11aa5, 0x7f0bc097, 
00198   0x7f4c7e54, 0x7f834ed0, 
00199   0x7fb02dc6, 0x7fd317b4, 0x7fec09e3, 0x7ffb0260, 0x7fffffff, 0x7ffb0260, 
00200   0x7fec09e3, 0x7fd317b4, 
00201   0x7fb02dc6, 0x7f834ed0, 0x7f4c7e54, 0x7f0bc097, 0x7ec11aa5, 0x7e6c9251, 
00202   0x7e0e2e32, 0x7da5f5a5, 
00203   0x7d33f0ca, 0x7cb82885, 0x7c32a67e, 0x7ba3751d, 0x7b0a9f8d, 0x7a6831ba, 
00204   0x79bc384d, 0x7906c0b0, 
00205   0x7847d909, 0x777f903c, 0x76adf5e6, 0x75d31a61, 0x74ef0ebc, 0x7401e4c1, 
00206   0x730baeed, 0x720c8075, 
00207   0x71046d3e, 0x6ff389df, 0x6ed9eba1, 0x6db7a87a, 0x6c8cd70b, 0x6b598ea3, 
00208   0x6a1de737, 0x68d9f964, 
00209   0x678dde6e, 0x6639b03b, 0x64dd8950, 0x637984d4, 0x620dbe8b, 0x609a52d3, 
00210   0x5f1f5ea1, 0x5d9cff83, 
00211   0x5c13539b, 0x5a82799a, 0x58ea90c4, 0x574bb8e6, 0x55a6125c, 0x53f9be05, 
00212   0x5246dd49, 0x508d9211, 
00213   0x4ecdfec7, 0x4d084651, 0x4b3c8c12, 0x496af3e2, 0x4793a210, 0x45b6bb5e, 
00214   0x43d464fb, 0x41ecc484, 
00215   0x40000000, 0x3e0e3ddc, 0x3c17a4e8, 0x3a1c5c57, 0x381c8bb5, 0x36185aee, 
00216   0x340ff242, 0x32037a45, 
00217   0x2ff31bde, 0x2ddf0040, 0x2bc750e9, 0x29ac37a0, 0x278dde6e, 0x256c6f9f, 
00218   0x234815ba, 0x2120fb83, 
00219   0x1ef74bf3, 0x1ccb3237, 0x1a9cd9ac, 0x186c6ddd, 0x163a1a7e, 0x14060b68, 
00220   0x11d06c97, 0xf996a26, 
00221   0xd61304e, 0xb27eb5c, 0x8edc7b7, 0x6b2f1d2, 0x4779632, 0x23be165, 0x0, 
00222   0xfdc41e9b, 
00223   0xfb8869ce, 0xf94d0e2e, 0xf7123849, 0xf4d814a4, 0xf29ecfb2, 0xf06695da, 
00224   0xee2f9369, 0xebf9f498, 
00225   0xe9c5e582, 0xe7939223, 0xe5632654, 0xe334cdc9, 0xe108b40d, 0xdedf047d, 
00226   0xdcb7ea46, 0xda939061, 
00227   0xd8722192, 0xd653c860, 0xd438af17, 0xd220ffc0, 0xd00ce422, 0xcdfc85bb, 
00228   0xcbf00dbe, 0xc9e7a512, 
00229   0xc7e3744b, 0xc5e3a3a9, 0xc3e85b18, 0xc1f1c224, 0xc0000000, 0xbe133b7c, 
00230   0xbc2b9b05, 0xba4944a2, 
00231   0xb86c5df0, 0xb6950c1e, 0xb4c373ee, 0xb2f7b9af, 0xb1320139, 0xaf726def, 
00232   0xadb922b7, 0xac0641fb, 
00233   0xaa59eda4, 0xa8b4471a, 0xa7156f3c, 0xa57d8666, 0xa3ecac65, 0xa263007d, 
00234   0xa0e0a15f, 0x9f65ad2d, 
00235   0x9df24175, 0x9c867b2c, 0x9b2276b0, 0x99c64fc5, 0x98722192, 0x9726069c, 
00236   0x95e218c9, 0x94a6715d, 
00237   0x937328f5, 0x92485786, 0x9126145f, 0x900c7621, 0x8efb92c2, 0x8df37f8b, 
00238   0x8cf45113, 0x8bfe1b3f, 
00239   0x8b10f144, 0x8a2ce59f, 0x89520a1a, 0x88806fc4, 0x87b826f7, 0x86f93f50, 
00240   0x8643c7b3, 0x8597ce46, 
00241   0x84f56073, 0x845c8ae3, 0x83cd5982, 0x8347d77b, 0x82cc0f36, 0x825a0a5b, 
00242   0x81f1d1ce, 0x81936daf, 
00243   0x813ee55b, 0x80f43f69, 0x80b381ac, 0x807cb130, 0x804fd23a, 0x802ce84c, 
00244   0x8013f61d, 0x8004fda0, 
00245  
00246 }; 
00247  
00248  
00249 /**  
00250  * @brief  Q31 sin_cos function. 
00251  * @param[in]  theta    scaled input value in degrees  
00252  * @param[out] *pSinVal points to the processed sine output.  
00253  * @param[out] *pCosVal points to the processed cosine output.  
00254  * @return none. 
00255  *  
00256  * The Q31 input value is in the range [-1 +1) and is mapped to a degree value in the range [-180 180). 
00257  *  
00258  */ 
00259  
00260  
00261 void arm_sin_cos_q31( 
00262   q31_t theta, 
00263   q31_t * pSinVal, 
00264   q31_t * pCosVal) 
00265 { 
00266   q31_t x0;                                      /* Nearest input value */ 
00267   q31_t y0, y1;                                  /* Nearest output values */ 
00268   q31_t xSpacing = INPUT_SPACING;                /* Spaing between inputs */ 
00269   uint32_t i;                                    /* Index */ 
00270   q31_t oneByXSpacing;                           /* 1/ xSpacing value */ 
00271   q31_t out;                                     /* temporary variable */ 
00272   uint32_t sign_bits;                            /* No.of sign bits */ 
00273   uint32_t firstX = 0x80000000;                  /* First X value */ 
00274  
00275   /* Calculation of index */ 
00276   i = ((uint32_t) theta - firstX) / (uint32_t) xSpacing; 
00277  
00278   /* Calculation of first nearest input value */ 
00279   x0 = (q31_t) firstX + ((q31_t) i * xSpacing); 
00280  
00281   /* Reading nearest sine output values from table */ 
00282   y0 = sinTableQ31 [i]; 
00283   y1 = sinTableQ31 [i + 1u]; 
00284  
00285   /* Calculation of 1/(x1-x0) */ 
00286   /* (x1-x0) is xSpacing which is fixed value */ 
00287   sign_bits = 8u; 
00288   oneByXSpacing = 0x5A000000; 
00289  
00290   /* Calculation of (theta - x0)/(x1-x0) */ 
00291   out = (((q31_t) (((q63_t) (theta - x0) * oneByXSpacing) >> 32)) << sign_bits); 
00292  
00293   /* Calculation of y0 + (y1 - y0) * ((theta - x0)/(x1-x0)) */ 
00294   *pSinVal = y0 + ((q31_t) (((q63_t) (y1 - y0) * out) >> 30)); 
00295  
00296   /* Reading nearest cosine output values from table */ 
00297   y0 = cosTableQ31 [i]; 
00298   y1 = cosTableQ31 [i + 1u]; 
00299  
00300   /* Calculation of y0 + (y1 - y0) * ((theta - x0)/(x1-x0)) */ 
00301   *pCosVal = y0 + ((q31_t) (((q63_t) (y1 - y0) * out) >> 30)); 
00302  
00303 } 
00304  
00305 /**  
00306  * @} end of SinCos group  
00307  */