arm_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_cos_q31.c  
00009 *  
00010 * Description:  Fast cosine 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 groupFastMath  
00031  */ 
00032  
00033  /**  
00034  * @addtogroup cos  
00035  * @{  
00036  */ 
00037  
00038 /**  
00039  * \par  
00040  * Table Values are in Q31(1.31 Fixed point format) and generation is done in three steps  
00041  * First Generate cos values in floating point:  
00042  * tableSize = 256;    
00043  * <pre>for(n = -1; n < (tableSize + 1); n++)  
00044  * {  
00045  *  cosTable[n+1]= cos(2*pi*n/tableSize);  
00046  * } </pre>   
00047  * where pi value is  3.14159265358979  
00048  * \par  
00049  * Secondly Convert Floating point to Q31(Fixed point):  
00050  *  (cosTable[i] * pow(2, 31))  
00051  * \par  
00052  * Finally Rounding to nearest integer is done  
00053  *  cosTable[i] += (cosTable[i] > 0 ? 0.5 :-0.5);  
00054  */ 
00055  
00056  
00057 static const q31_t cosTableQ31 [259] = { 
00058   0x7ff62182, 0x7fffffff, 0x7ff62182, 0x7fd8878e, 0x7fa736b4, 0x7f62368f, 
00059     0x7f0991c4, 0x7e9d55fc, 
00060   0x7e1d93ea, 0x7d8a5f40, 0x7ce3ceb2, 0x7c29fbee, 0x7b5d039e, 0x7a7d055b, 
00061     0x798a23b1, 0x78848414, 
00062   0x776c4edb, 0x7641af3d, 0x7504d345, 0x73b5ebd1, 0x72552c85, 0x70e2cbc6, 
00063     0x6f5f02b2, 0x6dca0d14, 
00064   0x6c242960, 0x6a6d98a4, 0x68a69e81, 0x66cf8120, 0x64e88926, 0x62f201ac, 
00065     0x60ec3830, 0x5ed77c8a, 
00066   0x5cb420e0, 0x5a82799a, 0x5842dd54, 0x55f5a4d2, 0x539b2af0, 0x5133cc94, 
00067     0x4ebfe8a5, 0x4c3fdff4, 
00068   0x49b41533, 0x471cece7, 0x447acd50, 0x41ce1e65, 0x3f1749b8, 0x3c56ba70, 
00069     0x398cdd32, 0x36ba2014, 
00070   0x33def287, 0x30fbc54d, 0x2e110a62, 0x2b1f34eb, 0x2826b928, 0x25280c5e, 
00071     0x2223a4c5, 0x1f19f97b, 
00072   0x1c0b826a, 0x18f8b83c, 0x15e21445, 0x12c8106f, 0xfab272b, 0xc8bd35e, 
00073     0x96a9049, 0x647d97c, 
00074   0x3242abf, 0x0, 0xfcdbd541, 0xf9b82684, 0xf6956fb7, 0xf3742ca2, 0xf054d8d5, 
00075     0xed37ef91, 
00076   0xea1debbb, 0xe70747c4, 0xe3f47d96, 0xe0e60685, 0xdddc5b3b, 0xdad7f3a2, 
00077     0xd7d946d8, 0xd4e0cb15, 
00078   0xd1eef59e, 0xcf043ab3, 0xcc210d79, 0xc945dfec, 0xc67322ce, 0xc3a94590, 
00079     0xc0e8b648, 0xbe31e19b, 
00080   0xbb8532b0, 0xb8e31319, 0xb64beacd, 0xb3c0200c, 0xb140175b, 0xaecc336c, 
00081     0xac64d510, 0xaa0a5b2e, 
00082   0xa7bd22ac, 0xa57d8666, 0xa34bdf20, 0xa1288376, 0x9f13c7d0, 0x9d0dfe54, 
00083     0x9b1776da, 0x99307ee0, 
00084   0x9759617f, 0x9592675c, 0x93dbd6a0, 0x9235f2ec, 0x90a0fd4e, 0x8f1d343a, 
00085     0x8daad37b, 0x8c4a142f, 
00086   0x8afb2cbb, 0x89be50c3, 0x8893b125, 0x877b7bec, 0x8675dc4f, 0x8582faa5, 
00087     0x84a2fc62, 0x83d60412, 
00088   0x831c314e, 0x8275a0c0, 0x81e26c16, 0x8162aa04, 0x80f66e3c, 0x809dc971, 
00089     0x8058c94c, 0x80277872, 
00090   0x8009de7e, 0x80000000, 0x8009de7e, 0x80277872, 0x8058c94c, 0x809dc971, 
00091     0x80f66e3c, 0x8162aa04, 
00092   0x81e26c16, 0x8275a0c0, 0x831c314e, 0x83d60412, 0x84a2fc62, 0x8582faa5, 
00093     0x8675dc4f, 0x877b7bec, 
00094   0x8893b125, 0x89be50c3, 0x8afb2cbb, 0x8c4a142f, 0x8daad37b, 0x8f1d343a, 
00095     0x90a0fd4e, 0x9235f2ec, 
00096   0x93dbd6a0, 0x9592675c, 0x9759617f, 0x99307ee0, 0x9b1776da, 0x9d0dfe54, 
00097     0x9f13c7d0, 0xa1288376, 
00098   0xa34bdf20, 0xa57d8666, 0xa7bd22ac, 0xaa0a5b2e, 0xac64d510, 0xaecc336c, 
00099     0xb140175b, 0xb3c0200c, 
00100   0xb64beacd, 0xb8e31319, 0xbb8532b0, 0xbe31e19b, 0xc0e8b648, 0xc3a94590, 
00101     0xc67322ce, 0xc945dfec, 
00102   0xcc210d79, 0xcf043ab3, 0xd1eef59e, 0xd4e0cb15, 0xd7d946d8, 0xdad7f3a2, 
00103     0xdddc5b3b, 0xe0e60685, 
00104   0xe3f47d96, 0xe70747c4, 0xea1debbb, 0xed37ef91, 0xf054d8d5, 0xf3742ca2, 
00105     0xf6956fb7, 0xf9b82684, 
00106   0xfcdbd541, 0x0, 0x3242abf, 0x647d97c, 0x96a9049, 0xc8bd35e, 0xfab272b, 
00107     0x12c8106f, 
00108   0x15e21445, 0x18f8b83c, 0x1c0b826a, 0x1f19f97b, 0x2223a4c5, 0x25280c5e, 
00109     0x2826b928, 0x2b1f34eb, 
00110   0x2e110a62, 0x30fbc54d, 0x33def287, 0x36ba2014, 0x398cdd32, 0x3c56ba70, 
00111     0x3f1749b8, 0x41ce1e65, 
00112   0x447acd50, 0x471cece7, 0x49b41533, 0x4c3fdff4, 0x4ebfe8a5, 0x5133cc94, 
00113     0x539b2af0, 0x55f5a4d2, 
00114   0x5842dd54, 0x5a82799a, 0x5cb420e0, 0x5ed77c8a, 0x60ec3830, 0x62f201ac, 
00115     0x64e88926, 0x66cf8120, 
00116   0x68a69e81, 0x6a6d98a4, 0x6c242960, 0x6dca0d14, 0x6f5f02b2, 0x70e2cbc6, 
00117     0x72552c85, 0x73b5ebd1, 
00118   0x7504d345, 0x7641af3d, 0x776c4edb, 0x78848414, 0x798a23b1, 0x7a7d055b, 
00119     0x7b5d039e, 0x7c29fbee, 
00120   0x7ce3ceb2, 0x7d8a5f40, 0x7e1d93ea, 0x7e9d55fc, 0x7f0991c4, 0x7f62368f, 
00121     0x7fa736b4, 0x7fd8878e, 
00122   0x7ff62182, 0x7fffffff, 0x7ff62182 
00123 }; 
00124  
00125 /** 
00126  * @brief Fast approximation to the trigonometric cosine function for Q31 data. 
00127  * @param[in] x Scaled input value in radians. 
00128  * @return  cos(x). 
00129  * 
00130  * The Q31 input value is in the range [0 +1) and is mapped to a radian value in the range [0 2*pi). 
00131  */ 
00132  
00133 q31_t arm_cos_q31( 
00134   q31_t x) 
00135 { 
00136   q31_t cosVal, fract, in, in2;                  /* Temporary variables for input, output */ 
00137   q31_t wa, wb, wc, wd;                          /* Cubic interpolation coefficients */ 
00138   q31_t a, b, c, d;                              /* Four nearest output values */ 
00139   q31_t *tablePtr;                               /* Pointer to table */ 
00140   q31_t fractCube, fractSquare;                  /* Temporary values for fractional value */ 
00141   q31_t oneBy6 = 0x15555555;                     /* Fixed point value of 1/6 */ 
00142   q31_t tableSpacing = TABLE_SPACING_Q31;        /* Table spacing */ 
00143   q31_t temp;                                    /* Temporary variable for intermediate process */ 
00144   uint32_t index;                                /* Index variable */ 
00145  
00146   in = x; 
00147  
00148   /* Calculate the nearest index */ 
00149   index = in / tableSpacing; 
00150  
00151   /* Calculate the nearest value of input */ 
00152   in2 = ((q31_t) index) * tableSpacing; 
00153  
00154   /* Calculation of fractional value */ 
00155   fract = (in - in2) << 8; 
00156  
00157   /* fractSquare = fract * fract */ 
00158   fractSquare = ((q31_t) (((q63_t) fract * fract) >> 32)); 
00159   fractSquare = fractSquare << 1; 
00160  
00161   /* fractCube = fract * fract * fract */ 
00162   fractCube = ((q31_t) (((q63_t) fractSquare * fract) >> 32)); 
00163   fractCube = fractCube << 1; 
00164  
00165   /* Initialise table pointer */ 
00166   tablePtr = (q31_t *) & cosTableQ31 [index]; 
00167  
00168   /* Cubic interpolation process */ 
00169   /* Calculation of wa */ 
00170   /* wa = -(oneBy6)*fractCube + (fractSquare >> 1u) - (0x2AAAAAAA)*fract; */ 
00171   wa = ((q31_t) (((q63_t) oneBy6 * fractCube) >> 32)); 
00172   temp = 0x2AAAAAAA; 
00173   wa = (q31_t) ((((q63_t) wa << 32) + ((q63_t) temp * fract)) >> 32); 
00174   wa = -(wa << 1u); 
00175   wa += (fractSquare >> 1u); 
00176  
00177   /* Read first nearest value of output from the cos table */ 
00178   a = *tablePtr++; 
00179  
00180   /* cosVal = a*wa */ 
00181   cosVal = ((q31_t) (((q63_t) a * wa) >> 32)); 
00182  
00183   /* q31(1.31) Fixed point value of 1 */ 
00184   temp = 0x7FFFFFFF; 
00185  
00186   /* Calculation of wb */ 
00187   wb = ((fractCube >> 1u) - (fractSquare + (fract >> 1u))) + temp; 
00188   /* Read second nearest value of output from the cos table */ 
00189   b = *tablePtr++; 
00190  
00191   /*      cosVal += b*wb */ 
00192   cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) b * (wb))) >> 32); 
00193  
00194   /* Calculation of wc */ 
00195   wc = -fractCube + fractSquare; 
00196   wc = (wc >> 1u) + fract; 
00197   /* Read third nearest values of output value from the cos table */ 
00198   c = *tablePtr++; 
00199  
00200   /*      cosVal += c*wc */ 
00201   cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) c * (wc))) >> 32); 
00202  
00203   /* Calculation of wd */ 
00204   /* wd = (oneBy6)*fractCube - (oneBy6)*fract; */ 
00205   fractCube = fractCube - fract; 
00206   wd = ((q31_t) (((q63_t) oneBy6 * fractCube) >> 32)); 
00207   wd = (wd << 1u); 
00208  
00209   /* Read fourth nearest value of output from the cos table */ 
00210   d = *tablePtr++; 
00211  
00212   /* cosVal += d*wd; */ 
00213   cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) d * (wd))) >> 32); 
00214  
00215   /* convert cosVal in 2.30 format to 1.31 format */ 
00216   return (cosVal << 1u); 
00217  
00218 } 
00219  
00220 /**  
00221  * @} end of cos group  
00222  */