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_cfft_radix2_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_cfft_radix2_q15.c 00009 * 00010 * Description: Radix-2 Decimation in Frequency CFFT & CIFFT Fixed point processing function 00011 * 00012 * 00013 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 00014 * 00015 * Redistribution and use in source and binary forms, with or without 00016 * modification, are permitted provided that the following conditions 00017 * are met: 00018 * - Redistributions of source code must retain the above copyright 00019 * notice, this list of conditions and the following disclaimer. 00020 * - Redistributions in binary form must reproduce the above copyright 00021 * notice, this list of conditions and the following disclaimer in 00022 * the documentation and/or other materials provided with the 00023 * distribution. 00024 * - Neither the name of ARM LIMITED nor the names of its contributors 00025 * may be used to endorse or promote products derived from this 00026 * software without specific prior written permission. 00027 * 00028 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00029 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00030 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00031 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00032 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00033 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00034 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00035 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00036 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00037 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00038 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00039 * POSSIBILITY OF SUCH DAMAGE. 00040 * -------------------------------------------------------------------- */ 00041 00042 #include "arm_math.h" 00043 00044 void arm_radix2_butterfly_q15( 00045 q15_t * pSrc, 00046 uint32_t fftLen, 00047 q15_t * pCoef, 00048 uint16_t twidCoefModifier); 00049 00050 void arm_radix2_butterfly_inverse_q15( 00051 q15_t * pSrc, 00052 uint32_t fftLen, 00053 q15_t * pCoef, 00054 uint16_t twidCoefModifier); 00055 00056 void arm_bitreversal_q15( 00057 q15_t * pSrc, 00058 uint32_t fftLen, 00059 uint16_t bitRevFactor, 00060 uint16_t * pBitRevTab); 00061 00062 /** 00063 * @ingroup groupTransforms 00064 */ 00065 00066 /** 00067 * @addtogroup ComplexFFT 00068 * @{ 00069 */ 00070 00071 /** 00072 * @details 00073 * @brief Processing function for the fixed-point CFFT/CIFFT. 00074 * @param[in] *S points to an instance of the fixed-point CFFT/CIFFT structure. 00075 * @param[in, out] *pSrc points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place. 00076 * @return none. 00077 */ 00078 00079 void arm_cfft_radix2_q15( 00080 const arm_cfft_radix2_instance_q15 * S, 00081 q15_t * pSrc) 00082 { 00083 00084 if(S->ifftFlag == 1u) 00085 { 00086 arm_radix2_butterfly_inverse_q15(pSrc, S->fftLen, 00087 S->pTwiddle, S->twidCoefModifier); 00088 } 00089 else 00090 { 00091 arm_radix2_butterfly_q15(pSrc, S->fftLen, 00092 S->pTwiddle, S->twidCoefModifier); 00093 } 00094 00095 arm_bitreversal_q15(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable); 00096 } 00097 00098 /** 00099 * @} end of ComplexFFT group 00100 */ 00101 00102 void arm_radix2_butterfly_q15( 00103 q15_t * pSrc, 00104 uint32_t fftLen, 00105 q15_t * pCoef, 00106 uint16_t twidCoefModifier) 00107 { 00108 #ifndef ARM_MATH_CM0_FAMILY 00109 00110 unsigned i, j, k, l; 00111 unsigned n1, n2, ia; 00112 q15_t in; 00113 q31_t T, S, R; 00114 q31_t coeff, out1, out2; 00115 00116 //N = fftLen; 00117 n2 = fftLen; 00118 00119 n1 = n2; 00120 n2 = n2 >> 1; 00121 ia = 0; 00122 00123 // loop for groups 00124 for (i = 0; i < n2; i++) 00125 { 00126 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u)); 00127 00128 ia = ia + twidCoefModifier; 00129 00130 l = i + n2; 00131 00132 T = _SIMD32_OFFSET(pSrc + (2 * i)); 00133 in = ((int16_t) (T & 0xFFFF)) >> 2; 00134 T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF); 00135 00136 S = _SIMD32_OFFSET(pSrc + (2 * l)); 00137 in = ((int16_t) (S & 0xFFFF)) >> 2; 00138 S = ((S >> 2) & 0xFFFF0000) | (in & 0xFFFF); 00139 00140 R = __QSUB16(T, S); 00141 00142 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S); 00143 00144 #ifndef ARM_MATH_BIG_ENDIAN 00145 00146 out1 = __SMUAD(coeff, R) >> 16; 00147 out2 = __SMUSDX(coeff, R); 00148 00149 #else 00150 00151 out1 = __SMUSDX(R, coeff) >> 16u; 00152 out2 = __SMUAD(coeff, R); 00153 00154 #endif // #ifndef ARM_MATH_BIG_ENDIAN 00155 00156 _SIMD32_OFFSET(pSrc + (2u * l)) = 00157 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF); 00158 00159 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u)); 00160 00161 ia = ia + twidCoefModifier; 00162 00163 // loop for butterfly 00164 i++; 00165 l++; 00166 00167 T = _SIMD32_OFFSET(pSrc + (2 * i)); 00168 in = ((int16_t) (T & 0xFFFF)) >> 2; 00169 T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF); 00170 00171 S = _SIMD32_OFFSET(pSrc + (2 * l)); 00172 in = ((int16_t) (S & 0xFFFF)) >> 2; 00173 S = ((S >> 2) & 0xFFFF0000) | (in & 0xFFFF); 00174 00175 R = __QSUB16(T, S); 00176 00177 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S); 00178 00179 #ifndef ARM_MATH_BIG_ENDIAN 00180 00181 out1 = __SMUAD(coeff, R) >> 16; 00182 out2 = __SMUSDX(coeff, R); 00183 00184 #else 00185 00186 out1 = __SMUSDX(R, coeff) >> 16u; 00187 out2 = __SMUAD(coeff, R); 00188 00189 #endif // #ifndef ARM_MATH_BIG_ENDIAN 00190 00191 _SIMD32_OFFSET(pSrc + (2u * l)) = 00192 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF); 00193 00194 } // groups loop end 00195 00196 twidCoefModifier = twidCoefModifier << 1u; 00197 00198 // loop for stage 00199 for (k = fftLen / 2; k > 2; k = k >> 1) 00200 { 00201 n1 = n2; 00202 n2 = n2 >> 1; 00203 ia = 0; 00204 00205 // loop for groups 00206 for (j = 0; j < n2; j++) 00207 { 00208 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u)); 00209 00210 ia = ia + twidCoefModifier; 00211 00212 // loop for butterfly 00213 for (i = j; i < fftLen; i += n1) 00214 { 00215 l = i + n2; 00216 00217 T = _SIMD32_OFFSET(pSrc + (2 * i)); 00218 00219 S = _SIMD32_OFFSET(pSrc + (2 * l)); 00220 00221 R = __QSUB16(T, S); 00222 00223 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S); 00224 00225 #ifndef ARM_MATH_BIG_ENDIAN 00226 00227 out1 = __SMUAD(coeff, R) >> 16; 00228 out2 = __SMUSDX(coeff, R); 00229 00230 #else 00231 00232 out1 = __SMUSDX(R, coeff) >> 16u; 00233 out2 = __SMUAD(coeff, R); 00234 00235 #endif // #ifndef ARM_MATH_BIG_ENDIAN 00236 00237 _SIMD32_OFFSET(pSrc + (2u * l)) = 00238 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF); 00239 00240 i += n1; 00241 00242 l = i + n2; 00243 00244 T = _SIMD32_OFFSET(pSrc + (2 * i)); 00245 00246 S = _SIMD32_OFFSET(pSrc + (2 * l)); 00247 00248 R = __QSUB16(T, S); 00249 00250 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S); 00251 00252 #ifndef ARM_MATH_BIG_ENDIAN 00253 00254 out1 = __SMUAD(coeff, R) >> 16; 00255 out2 = __SMUSDX(coeff, R); 00256 00257 #else 00258 00259 out1 = __SMUSDX(R, coeff) >> 16u; 00260 out2 = __SMUAD(coeff, R); 00261 00262 #endif // #ifndef ARM_MATH_BIG_ENDIAN 00263 00264 _SIMD32_OFFSET(pSrc + (2u * l)) = 00265 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF); 00266 00267 } // butterfly loop end 00268 00269 } // groups loop end 00270 00271 twidCoefModifier = twidCoefModifier << 1u; 00272 } // stages loop end 00273 00274 n1 = n2; 00275 n2 = n2 >> 1; 00276 ia = 0; 00277 00278 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u)); 00279 00280 ia = ia + twidCoefModifier; 00281 00282 // loop for butterfly 00283 for (i = 0; i < fftLen; i += n1) 00284 { 00285 l = i + n2; 00286 00287 T = _SIMD32_OFFSET(pSrc + (2 * i)); 00288 00289 S = _SIMD32_OFFSET(pSrc + (2 * l)); 00290 00291 R = __QSUB16(T, S); 00292 00293 _SIMD32_OFFSET(pSrc + (2 * i)) = __QADD16(T, S); 00294 00295 _SIMD32_OFFSET(pSrc + (2u * l)) = R; 00296 00297 i += n1; 00298 l = i + n2; 00299 00300 T = _SIMD32_OFFSET(pSrc + (2 * i)); 00301 00302 S = _SIMD32_OFFSET(pSrc + (2 * l)); 00303 00304 R = __QSUB16(T, S); 00305 00306 _SIMD32_OFFSET(pSrc + (2 * i)) = __QADD16(T, S); 00307 00308 _SIMD32_OFFSET(pSrc + (2u * l)) = R; 00309 00310 } // groups loop end 00311 00312 00313 #else 00314 00315 unsigned i, j, k, l; 00316 unsigned n1, n2, ia; 00317 q15_t xt, yt, cosVal, sinVal; 00318 00319 00320 //N = fftLen; 00321 n2 = fftLen; 00322 00323 n1 = n2; 00324 n2 = n2 >> 1; 00325 ia = 0; 00326 00327 // loop for groups 00328 for (j = 0; j < n2; j++) 00329 { 00330 cosVal = pCoef[ia * 2]; 00331 sinVal = pCoef[(ia * 2) + 1]; 00332 ia = ia + twidCoefModifier; 00333 00334 // loop for butterfly 00335 for (i = j; i < fftLen; i += n1) 00336 { 00337 l = i + n2; 00338 xt = (pSrc[2 * i] >> 2u) - (pSrc[2 * l] >> 2u); 00339 pSrc[2 * i] = ((pSrc[2 * i] >> 2u) + (pSrc[2 * l] >> 2u)) >> 1u; 00340 00341 yt = (pSrc[2 * i + 1] >> 2u) - (pSrc[2 * l + 1] >> 2u); 00342 pSrc[2 * i + 1] = 00343 ((pSrc[2 * l + 1] >> 2u) + (pSrc[2 * i + 1] >> 2u)) >> 1u; 00344 00345 pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) + 00346 ((int16_t) (((q31_t) yt * sinVal) >> 16))); 00347 00348 pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) - 00349 ((int16_t) (((q31_t) xt * sinVal) >> 16))); 00350 00351 } // butterfly loop end 00352 00353 } // groups loop end 00354 00355 twidCoefModifier = twidCoefModifier << 1u; 00356 00357 // loop for stage 00358 for (k = fftLen / 2; k > 2; k = k >> 1) 00359 { 00360 n1 = n2; 00361 n2 = n2 >> 1; 00362 ia = 0; 00363 00364 // loop for groups 00365 for (j = 0; j < n2; j++) 00366 { 00367 cosVal = pCoef[ia * 2]; 00368 sinVal = pCoef[(ia * 2) + 1]; 00369 ia = ia + twidCoefModifier; 00370 00371 // loop for butterfly 00372 for (i = j; i < fftLen; i += n1) 00373 { 00374 l = i + n2; 00375 xt = pSrc[2 * i] - pSrc[2 * l]; 00376 pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]) >> 1u; 00377 00378 yt = pSrc[2 * i + 1] - pSrc[2 * l + 1]; 00379 pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]) >> 1u; 00380 00381 pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) + 00382 ((int16_t) (((q31_t) yt * sinVal) >> 16))); 00383 00384 pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) - 00385 ((int16_t) (((q31_t) xt * sinVal) >> 16))); 00386 00387 } // butterfly loop end 00388 00389 } // groups loop end 00390 00391 twidCoefModifier = twidCoefModifier << 1u; 00392 } // stages loop end 00393 00394 n1 = n2; 00395 n2 = n2 >> 1; 00396 ia = 0; 00397 00398 // loop for groups 00399 for (j = 0; j < n2; j++) 00400 { 00401 cosVal = pCoef[ia * 2]; 00402 sinVal = pCoef[(ia * 2) + 1]; 00403 00404 ia = ia + twidCoefModifier; 00405 00406 // loop for butterfly 00407 for (i = j; i < fftLen; i += n1) 00408 { 00409 l = i + n2; 00410 xt = pSrc[2 * i] - pSrc[2 * l]; 00411 pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]); 00412 00413 yt = pSrc[2 * i + 1] - pSrc[2 * l + 1]; 00414 pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]); 00415 00416 pSrc[2u * l] = xt; 00417 00418 pSrc[2u * l + 1u] = yt; 00419 00420 } // butterfly loop end 00421 00422 } // groups loop end 00423 00424 twidCoefModifier = twidCoefModifier << 1u; 00425 00426 #endif // #ifndef ARM_MATH_CM0_FAMILY 00427 00428 } 00429 00430 00431 void arm_radix2_butterfly_inverse_q15( 00432 q15_t * pSrc, 00433 uint32_t fftLen, 00434 q15_t * pCoef, 00435 uint16_t twidCoefModifier) 00436 { 00437 #ifndef ARM_MATH_CM0_FAMILY 00438 00439 unsigned i, j, k, l; 00440 unsigned n1, n2, ia; 00441 q15_t in; 00442 q31_t T, S, R; 00443 q31_t coeff, out1, out2; 00444 00445 //N = fftLen; 00446 n2 = fftLen; 00447 00448 n1 = n2; 00449 n2 = n2 >> 1; 00450 ia = 0; 00451 00452 // loop for groups 00453 for (i = 0; i < n2; i++) 00454 { 00455 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u)); 00456 00457 ia = ia + twidCoefModifier; 00458 00459 l = i + n2; 00460 00461 T = _SIMD32_OFFSET(pSrc + (2 * i)); 00462 in = ((int16_t) (T & 0xFFFF)) >> 2; 00463 T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF); 00464 00465 S = _SIMD32_OFFSET(pSrc + (2 * l)); 00466 in = ((int16_t) (S & 0xFFFF)) >> 2; 00467 S = ((S >> 2) & 0xFFFF0000) | (in & 0xFFFF); 00468 00469 R = __QSUB16(T, S); 00470 00471 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S); 00472 00473 #ifndef ARM_MATH_BIG_ENDIAN 00474 00475 out1 = __SMUSD(coeff, R) >> 16; 00476 out2 = __SMUADX(coeff, R); 00477 #else 00478 00479 out1 = __SMUADX(R, coeff) >> 16u; 00480 out2 = __SMUSD(__QSUB(0, coeff), R); 00481 00482 #endif // #ifndef ARM_MATH_BIG_ENDIAN 00483 00484 _SIMD32_OFFSET(pSrc + (2u * l)) = 00485 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF); 00486 00487 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u)); 00488 00489 ia = ia + twidCoefModifier; 00490 00491 // loop for butterfly 00492 i++; 00493 l++; 00494 00495 T = _SIMD32_OFFSET(pSrc + (2 * i)); 00496 in = ((int16_t) (T & 0xFFFF)) >> 2; 00497 T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF); 00498 00499 S = _SIMD32_OFFSET(pSrc + (2 * l)); 00500 in = ((int16_t) (S & 0xFFFF)) >> 2; 00501 S = ((S >> 2) & 0xFFFF0000) | (in & 0xFFFF); 00502 00503 R = __QSUB16(T, S); 00504 00505 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S); 00506 00507 #ifndef ARM_MATH_BIG_ENDIAN 00508 00509 out1 = __SMUSD(coeff, R) >> 16; 00510 out2 = __SMUADX(coeff, R); 00511 #else 00512 00513 out1 = __SMUADX(R, coeff) >> 16u; 00514 out2 = __SMUSD(__QSUB(0, coeff), R); 00515 00516 #endif // #ifndef ARM_MATH_BIG_ENDIAN 00517 00518 _SIMD32_OFFSET(pSrc + (2u * l)) = 00519 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF); 00520 00521 } // groups loop end 00522 00523 twidCoefModifier = twidCoefModifier << 1u; 00524 00525 // loop for stage 00526 for (k = fftLen / 2; k > 2; k = k >> 1) 00527 { 00528 n1 = n2; 00529 n2 = n2 >> 1; 00530 ia = 0; 00531 00532 // loop for groups 00533 for (j = 0; j < n2; j++) 00534 { 00535 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u)); 00536 00537 ia = ia + twidCoefModifier; 00538 00539 // loop for butterfly 00540 for (i = j; i < fftLen; i += n1) 00541 { 00542 l = i + n2; 00543 00544 T = _SIMD32_OFFSET(pSrc + (2 * i)); 00545 00546 S = _SIMD32_OFFSET(pSrc + (2 * l)); 00547 00548 R = __QSUB16(T, S); 00549 00550 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S); 00551 00552 #ifndef ARM_MATH_BIG_ENDIAN 00553 00554 out1 = __SMUSD(coeff, R) >> 16; 00555 out2 = __SMUADX(coeff, R); 00556 00557 #else 00558 00559 out1 = __SMUADX(R, coeff) >> 16u; 00560 out2 = __SMUSD(__QSUB(0, coeff), R); 00561 00562 #endif // #ifndef ARM_MATH_BIG_ENDIAN 00563 00564 _SIMD32_OFFSET(pSrc + (2u * l)) = 00565 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF); 00566 00567 i += n1; 00568 00569 l = i + n2; 00570 00571 T = _SIMD32_OFFSET(pSrc + (2 * i)); 00572 00573 S = _SIMD32_OFFSET(pSrc + (2 * l)); 00574 00575 R = __QSUB16(T, S); 00576 00577 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S); 00578 00579 #ifndef ARM_MATH_BIG_ENDIAN 00580 00581 out1 = __SMUSD(coeff, R) >> 16; 00582 out2 = __SMUADX(coeff, R); 00583 #else 00584 00585 out1 = __SMUADX(R, coeff) >> 16u; 00586 out2 = __SMUSD(__QSUB(0, coeff), R); 00587 00588 #endif // #ifndef ARM_MATH_BIG_ENDIAN 00589 00590 _SIMD32_OFFSET(pSrc + (2u * l)) = 00591 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF); 00592 00593 } // butterfly loop end 00594 00595 } // groups loop end 00596 00597 twidCoefModifier = twidCoefModifier << 1u; 00598 } // stages loop end 00599 00600 n1 = n2; 00601 n2 = n2 >> 1; 00602 ia = 0; 00603 00604 // loop for groups 00605 for (j = 0; j < n2; j++) 00606 { 00607 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u)); 00608 00609 ia = ia + twidCoefModifier; 00610 00611 // loop for butterfly 00612 for (i = j; i < fftLen; i += n1) 00613 { 00614 l = i + n2; 00615 00616 T = _SIMD32_OFFSET(pSrc + (2 * i)); 00617 00618 S = _SIMD32_OFFSET(pSrc + (2 * l)); 00619 00620 R = __QSUB16(T, S); 00621 00622 _SIMD32_OFFSET(pSrc + (2 * i)) = __QADD16(T, S); 00623 00624 _SIMD32_OFFSET(pSrc + (2u * l)) = R; 00625 00626 } // butterfly loop end 00627 00628 } // groups loop end 00629 00630 twidCoefModifier = twidCoefModifier << 1u; 00631 00632 #else 00633 00634 00635 unsigned i, j, k, l; 00636 unsigned n1, n2, ia; 00637 q15_t xt, yt, cosVal, sinVal; 00638 00639 //N = fftLen; 00640 n2 = fftLen; 00641 00642 n1 = n2; 00643 n2 = n2 >> 1; 00644 ia = 0; 00645 00646 // loop for groups 00647 for (j = 0; j < n2; j++) 00648 { 00649 cosVal = pCoef[ia * 2]; 00650 sinVal = pCoef[(ia * 2) + 1]; 00651 ia = ia + twidCoefModifier; 00652 00653 // loop for butterfly 00654 for (i = j; i < fftLen; i += n1) 00655 { 00656 l = i + n2; 00657 xt = (pSrc[2 * i] >> 2u) - (pSrc[2 * l] >> 2u); 00658 pSrc[2 * i] = ((pSrc[2 * i] >> 2u) + (pSrc[2 * l] >> 2u)) >> 1u; 00659 00660 yt = (pSrc[2 * i + 1] >> 2u) - (pSrc[2 * l + 1] >> 2u); 00661 pSrc[2 * i + 1] = 00662 ((pSrc[2 * l + 1] >> 2u) + (pSrc[2 * i + 1] >> 2u)) >> 1u; 00663 00664 pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) - 00665 ((int16_t) (((q31_t) yt * sinVal) >> 16))); 00666 00667 pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) + 00668 ((int16_t) (((q31_t) xt * sinVal) >> 16))); 00669 00670 } // butterfly loop end 00671 00672 } // groups loop end 00673 00674 twidCoefModifier = twidCoefModifier << 1u; 00675 00676 // loop for stage 00677 for (k = fftLen / 2; k > 2; k = k >> 1) 00678 { 00679 n1 = n2; 00680 n2 = n2 >> 1; 00681 ia = 0; 00682 00683 // loop for groups 00684 for (j = 0; j < n2; j++) 00685 { 00686 cosVal = pCoef[ia * 2]; 00687 sinVal = pCoef[(ia * 2) + 1]; 00688 ia = ia + twidCoefModifier; 00689 00690 // loop for butterfly 00691 for (i = j; i < fftLen; i += n1) 00692 { 00693 l = i + n2; 00694 xt = pSrc[2 * i] - pSrc[2 * l]; 00695 pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]) >> 1u; 00696 00697 yt = pSrc[2 * i + 1] - pSrc[2 * l + 1]; 00698 pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]) >> 1u; 00699 00700 pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) - 00701 ((int16_t) (((q31_t) yt * sinVal) >> 16))); 00702 00703 pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) + 00704 ((int16_t) (((q31_t) xt * sinVal) >> 16))); 00705 00706 } // butterfly loop end 00707 00708 } // groups loop end 00709 00710 twidCoefModifier = twidCoefModifier << 1u; 00711 } // stages loop end 00712 00713 n1 = n2; 00714 n2 = n2 >> 1; 00715 ia = 0; 00716 00717 cosVal = pCoef[ia * 2]; 00718 sinVal = pCoef[(ia * 2) + 1]; 00719 00720 ia = ia + twidCoefModifier; 00721 00722 // loop for butterfly 00723 for (i = 0; i < fftLen; i += n1) 00724 { 00725 l = i + n2; 00726 xt = pSrc[2 * i] - pSrc[2 * l]; 00727 pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]); 00728 00729 yt = pSrc[2 * i + 1] - pSrc[2 * l + 1]; 00730 pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]); 00731 00732 pSrc[2u * l] = xt; 00733 00734 pSrc[2u * l + 1u] = yt; 00735 00736 } // groups loop end 00737 00738 00739 #endif // #ifndef ARM_MATH_CM0_FAMILY 00740 00741 }
Generated on Tue Jul 12 2022 12:36:53 by 1.7.2