arm_cos_q15.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_q15.c  
00009 *  
00010 * Description:  Fast cosine calculation for Q15 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 Q15(1.15 Fixed point format) and generation is done in three steps  
00041 * \par  
00042 * First Generate cos values in floating point:  
00043 * tableSize = 256;   
00044 * <pre>for(n = -1; n < (tableSize + 1); n++)  
00045 * {  
00046 *   cosTable[n+1]= cos(2*pi*n/tableSize);  
00047 * }</pre> 
00048 * where pi value is  3.14159265358979  
00049 * \par  
00050 * Secondly Convert Floating point to Q15(Fixed point):  
00051 *   round(cosTable[i] * pow(2, 15))  
00052 * \par  
00053 * Finally Rounding to nearest integer is done  
00054 *   cosTable[i] += (cosTable[i] > 0 ? 0.5 :-0.5);  
00055 */ 
00056  
00057 static const q15_t cosTableQ15 [259] = { 
00058   0x7ff6, 0x7fff, 0x7ff6, 0x7fd9, 0x7fa7, 0x7f62, 0x7f0a, 0x7e9d, 
00059   0x7e1e, 0x7d8a, 0x7ce4, 0x7c2a, 0x7b5d, 0x7a7d, 0x798a, 0x7885, 
00060   0x776c, 0x7642, 0x7505, 0x73b6, 0x7255, 0x70e3, 0x6f5f, 0x6dca, 
00061   0x6c24, 0x6a6e, 0x68a7, 0x66d0, 0x64e9, 0x62f2, 0x60ec, 0x5ed7, 
00062   0x5cb4, 0x5a82, 0x5843, 0x55f6, 0x539b, 0x5134, 0x4ec0, 0x4c40, 
00063   0x49b4, 0x471d, 0x447b, 0x41ce, 0x3f17, 0x3c57, 0x398d, 0x36ba, 
00064   0x33df, 0x30fc, 0x2e11, 0x2b1f, 0x2827, 0x2528, 0x2224, 0x1f1a, 
00065   0x1c0c, 0x18f9, 0x15e2, 0x12c8, 0xfab, 0xc8c, 0x96b, 0x648, 
00066   0x324, 0x0, 0xfcdc, 0xf9b8, 0xf695, 0xf374, 0xf055, 0xed38, 
00067   0xea1e, 0xe707, 0xe3f4, 0xe0e6, 0xdddc, 0xdad8, 0xd7d9, 0xd4e1, 
00068   0xd1ef, 0xcf04, 0xcc21, 0xc946, 0xc673, 0xc3a9, 0xc0e9, 0xbe32, 
00069   0xbb85, 0xb8e3, 0xb64c, 0xb3c0, 0xb140, 0xaecc, 0xac65, 0xaa0a, 
00070   0xa7bd, 0xa57e, 0xa34c, 0xa129, 0x9f14, 0x9d0e, 0x9b17, 0x9930, 
00071   0x9759, 0x9592, 0x93dc, 0x9236, 0x90a1, 0x8f1d, 0x8dab, 0x8c4a, 
00072   0x8afb, 0x89be, 0x8894, 0x877b, 0x8676, 0x8583, 0x84a3, 0x83d6, 
00073   0x831c, 0x8276, 0x81e2, 0x8163, 0x80f6, 0x809e, 0x8059, 0x8027, 
00074   0x800a, 0x8000, 0x800a, 0x8027, 0x8059, 0x809e, 0x80f6, 0x8163, 
00075   0x81e2, 0x8276, 0x831c, 0x83d6, 0x84a3, 0x8583, 0x8676, 0x877b, 
00076   0x8894, 0x89be, 0x8afb, 0x8c4a, 0x8dab, 0x8f1d, 0x90a1, 0x9236, 
00077   0x93dc, 0x9592, 0x9759, 0x9930, 0x9b17, 0x9d0e, 0x9f14, 0xa129, 
00078   0xa34c, 0xa57e, 0xa7bd, 0xaa0a, 0xac65, 0xaecc, 0xb140, 0xb3c0, 
00079   0xb64c, 0xb8e3, 0xbb85, 0xbe32, 0xc0e9, 0xc3a9, 0xc673, 0xc946, 
00080   0xcc21, 0xcf04, 0xd1ef, 0xd4e1, 0xd7d9, 0xdad8, 0xdddc, 0xe0e6, 
00081   0xe3f4, 0xe707, 0xea1e, 0xed38, 0xf055, 0xf374, 0xf695, 0xf9b8, 
00082   0xfcdc, 0x0, 0x324, 0x648, 0x96b, 0xc8c, 0xfab, 0x12c8, 
00083   0x15e2, 0x18f9, 0x1c0c, 0x1f1a, 0x2224, 0x2528, 0x2827, 0x2b1f, 
00084   0x2e11, 0x30fc, 0x33df, 0x36ba, 0x398d, 0x3c57, 0x3f17, 0x41ce, 
00085   0x447b, 0x471d, 0x49b4, 0x4c40, 0x4ec0, 0x5134, 0x539b, 0x55f6, 
00086   0x5843, 0x5a82, 0x5cb4, 0x5ed7, 0x60ec, 0x62f2, 0x64e9, 0x66d0, 
00087   0x68a7, 0x6a6e, 0x6c24, 0x6dca, 0x6f5f, 0x70e3, 0x7255, 0x73b6, 
00088   0x7505, 0x7642, 0x776c, 0x7885, 0x798a, 0x7a7d, 0x7b5d, 0x7c2a, 
00089   0x7ce4, 0x7d8a, 0x7e1e, 0x7e9d, 0x7f0a, 0x7f62, 0x7fa7, 0x7fd9, 
00090   0x7ff6, 0x7fff, 0x7ff6 
00091 }; 
00092  
00093  
00094 /** 
00095  * @brief Fast approximation to the trigonometric cosine function for Q15 data. 
00096  * @param[in] x Scaled input value in radians. 
00097  * @return  cos(x). 
00098  * 
00099  * The Q15 input value is in the range [0 +1) and is mapped to a radian value in the range [0 2*pi). 
00100  */ 
00101  
00102 q15_t arm_cos_q15( 
00103   q15_t x) 
00104 { 
00105   q31_t cosVal;                                  /* Temporary variables output */ 
00106   q15_t *tablePtr;                               /* Pointer to table */ 
00107   q15_t fract, in, in2;                          /* Temporary variables for input, output */ 
00108   q31_t wa, wb, wc, wd;                          /* Cubic interpolation coefficients */ 
00109   q15_t a, b, c, d;                              /* Four nearest output values */ 
00110   q15_t fractCube, fractSquare;                  /* Temporary values for fractional value */ 
00111   q15_t oneBy6 = 0x1555;                         /* Fixed point value of 1/6 */ 
00112   q15_t tableSpacing = TABLE_SPACING_Q15;        /* Table spacing */ 
00113   int32_t index;                                 /* Index variable */ 
00114  
00115   in = x; 
00116  
00117   /* Calculate the nearest index */ 
00118   index = (int32_t) in / tableSpacing; 
00119  
00120   /* Calculate the nearest value of input */ 
00121   in2 = (q15_t) index *tableSpacing; 
00122  
00123   /* Calculation of fractional value */ 
00124   fract = (in - in2) << 8; 
00125  
00126   /* fractSquare = fract * fract */ 
00127   fractSquare = (q15_t) ((fract * fract) >> 15); 
00128  
00129   /* fractCube = fract * fract * fract */ 
00130   fractCube = (q15_t) ((fractSquare * fract) >> 15); 
00131  
00132   /* Initialise table pointer */ 
00133   tablePtr = (q15_t *) & cosTableQ15 [index]; 
00134  
00135   /* Cubic interpolation process */ 
00136   /* Calculation of wa */ 
00137   /* wa = -(oneBy6)*fractCube + (fractSquare >> 1u) - (0x2AAA)*fract; */ 
00138   wa = (q31_t) oneBy6 *fractCube; 
00139   wa += (q31_t) 0x2AAA * fract; 
00140   wa = -(wa >> 15); 
00141   wa += (fractSquare >> 1u); 
00142  
00143   /* Read first nearest value of output from the cos table */ 
00144   a = *tablePtr++; 
00145  
00146   /* cosVal = a * wa */ 
00147   cosVal = a * wa; 
00148  
00149   /* Calculation of wb */ 
00150   wb = (((fractCube >> 1u) - fractSquare) - (fract >> 1u)) + 0x7FFF; 
00151  
00152   /* Read second nearest value of output from the cos table */ 
00153   b = *tablePtr++; 
00154  
00155   /*      cosVal += b*wb */ 
00156   cosVal += b * wb; 
00157  
00158   /* Calculation of wc */ 
00159   wc = -(q31_t) fractCube + fractSquare; 
00160   wc = (wc >> 1u) + fract; 
00161  
00162   /* Read third nearest value of output from the cos table */ 
00163   c = *tablePtr++; 
00164  
00165   /*      cosVal += c*wc */ 
00166   cosVal += c * wc; 
00167  
00168   /* Calculation of wd */ 
00169   /* wd = (oneBy6)*fractCube - (oneBy6)*fract; */ 
00170   fractCube = fractCube - fract; 
00171   wd = ((q15_t) (((q31_t) oneBy6 * fractCube) >> 15)); 
00172  
00173   /* Read fourth nearest value of output from the cos table */ 
00174   d = *tablePtr++; 
00175  
00176   /* cosVal += d*wd; */ 
00177   cosVal += d * wd; 
00178  
00179   /* Return the output value in 1.15(q15) format */ 
00180   return ((q15_t) (cosVal >> 15u)); 
00181  
00182 } 
00183  
00184 /**  
00185  * @} end of cos group  
00186  */