CMSIS DSP library

Dependents:   KL25Z_FFT_Demo Hat_Board_v5_1 KL25Z_FFT_Demo_tony KL25Z_FFT_Demo_tony ... more

Fork of mbed-dsp by mbed official

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers arm_cos_q15.c Source File

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