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
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 */
Generated on Tue Jul 12 2022 12:36:54 by 1.7.2