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