Simon Ford
CMSIS DSP Library from CMSIS 2.0. See http://www.onarm.com/cmsis/ for full details
Dependents: K22F_DSP_Matrix_least_square BNO055-ELEC3810 1BNO055 ECE4180Project--Slave2 ... more
Embed:
(wiki syntax)
Show/hide line numbers
arm_cfft_radix4_f32.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_cfft_radix4_f32.c 00009 * 00010 * Description: Radix-4 Decimation in Frequency CFFT & CIFFT Floating point processing function 00011 * 00012 * 00013 * Target Processor: Cortex-M4/Cortex-M3 00014 * 00015 * Version 1.0.3 2010/11/29 00016 * Re-organized the CMSIS folders and updated documentation. 00017 * 00018 * Version 1.0.2 2010/11/11 00019 * Documentation updated. 00020 * 00021 * Version 1.0.1 2010/10/05 00022 * Production release and review comments incorporated. 00023 * 00024 * Version 1.0.0 2010/09/20 00025 * Production release and review comments incorporated. 00026 * 00027 * Version 0.0.5 2010/04/26 00028 * incorporated review comments and updated with latest CMSIS layer 00029 * 00030 * Version 0.0.3 2010/03/10 00031 * Initial version 00032 * -------------------------------------------------------------------- */ 00033 00034 #include "arm_math.h" 00035 00036 /** 00037 * @ingroup groupTransforms 00038 */ 00039 00040 /** 00041 * @defgroup CFFT_CIFFT Complex FFT Functions 00042 * 00043 * \par 00044 * Complex Fast Fourier Transform(CFFT) and Complex Inverse Fast Fourier Transform(CIFFT) is an efficient algorithm to compute Discrete Fourier Transform(DFT) and Inverse Discrete Fourier Transform(IDFT). 00045 * Computational complexity of CFFT reduces drastically when compared to DFT. 00046 * \par 00047 * This set of functions implements CFFT/CIFFT 00048 * for Q15, Q31, and floating-point data types. The functions operates on in-place buffer which uses same buffer for input and output. 00049 * Complex input is stored in input buffer in an interleaved fashion. 00050 * 00051 * \par 00052 * The functions operate on blocks of input and output data and each call to the function processes 00053 * <code>2*fftLen</code> samples through the transform. <code>pSrc</code> points to In-place arrays containing <code>2*fftLen</code> values. 00054 * \par 00055 * The <code>pSrc</code> points to the array of in-place buffer of size <code>2*fftLen</code> and inputs and outputs are stored in an interleaved fashion as shown below. 00056 * <pre> {real[0], imag[0], real[1], imag[1],..} </pre> 00057 * 00058 * \par Lengths supported by the transform: 00059 * \par 00060 * Internally, the function utilize a radix-4 decimation in frequency(DIF) algorithm 00061 * and the size of the FFT supported are of the lengths [16, 64, 256, 1024]. 00062 * 00063 * 00064 * \par Algorithm: 00065 * 00066 * <b>Complex Fast Fourier Transform:</b> 00067 * \par 00068 * Input real and imaginary data: 00069 * <pre> 00070 * x(n) = xa + j * ya 00071 * x(n+N/4 ) = xb + j * yb 00072 * x(n+N/2 ) = xc + j * yc 00073 * x(n+3N 4) = xd + j * yd 00074 * </pre> 00075 * where N is length of FFT 00076 * \par 00077 * Output real and imaginary data: 00078 * <pre> 00079 * X(4r) = xa'+ j * ya' 00080 * X(4r+1) = xb'+ j * yb' 00081 * X(4r+2) = xc'+ j * yc' 00082 * X(4r+3) = xd'+ j * yd' 00083 * </pre> 00084 * \par 00085 * Twiddle factors for radix-4 FFT: 00086 * <pre> 00087 * Wn = co1 + j * (- si1) 00088 * W2n = co2 + j * (- si2) 00089 * W3n = co3 + j * (- si3) 00090 * </pre> 00091 * 00092 * \par 00093 * \image html CFFT.gif "Radix-4 Decimation-in Frequency Complex Fast Fourier Transform" 00094 * 00095 * \par 00096 * Output from Radix-4 CFFT Results in Digit reversal order. Interchange middle two branches of every butterfly results in Bit reversed output. 00097 * \par 00098 * <b> Butterfly CFFT equations:</b> 00099 * <pre> 00100 * xa' = xa + xb + xc + xd 00101 * ya' = ya + yb + yc + yd 00102 * xc' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1) 00103 * yc' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1) 00104 * xb' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2) 00105 * yb' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) 00106 * xd' = (xa-yb-xc+yd)* co3 + (ya+xb-yc-xd)* (si3) 00107 * yd' = (ya+xb-yc-xd)* co3 - (xa-yb-xc+yd)* (si3) 00108 * </pre> 00109 * 00110 * 00111 * <b>Complex Inverse Fast Fourier Transform:</b> 00112 * \par 00113 * CIFFT uses same twiddle factor table as CFFT with modifications in the design equation as shown below. 00114 * 00115 * \par 00116 * <b> Modified Butterfly CIFFT equations:</b> 00117 * <pre> 00118 * xa' = xa + xb + xc + xd 00119 * ya' = ya + yb + yc + yd 00120 * xc' = (xa-yb-xc+yd)* co1 - (ya+xb-yc-xd)* (si1) 00121 * yc' = (ya+xb-yc-xd)* co1 + (xa-yb-xc+yd)* (si1) 00122 * xb' = (xa-xb+xc-xd)* co2 - (ya-yb+yc-yd)* (si2) 00123 * yb' = (ya-yb+yc-yd)* co2 + (xa-xb+xc-xd)* (si2) 00124 * xd' = (xa+yb-xc-yd)* co3 - (ya-xb-yc+xd)* (si3) 00125 * yd' = (ya-xb-yc+xd)* co3 + (xa+yb-xc-yd)* (si3) 00126 * </pre> 00127 * 00128 * \par Instance Structure 00129 * A separate instance structure must be defined for each Instance but the twiddle factors and bit reversal tables can be reused. 00130 * There are separate instance structure declarations for each of the 3 supported data types. 00131 * 00132 * \par Initialization Functions 00133 * There is also an associated initialization function for each data type. 00134 * The initialization function performs the following operations: 00135 * - Sets the values of the internal structure fields. 00136 * - Initializes twiddle factor table and bit reversal table pointers 00137 * \par 00138 * Use of the initialization function is optional. 00139 * However, if the initialization function is used, then the instance structure cannot be placed into a const data section. 00140 * To place an instance structure into a const data section, the instance structure must be manually initialized. 00141 * Manually initialize the instance structure as follows: 00142 * <pre> 00143 *arm_cfft_radix4_instance_f32 S = {fftLen, ifftFlag, bitReverseFlag, pTwiddle, pBitRevTable, twidCoefModifier, bitRevFactor, onebyfftLen}; 00144 *arm_cfft_radix4_instance_q31 S = {fftLen, ifftFlag, bitReverseFlag, pTwiddle, pBitRevTable, twidCoefModifier, bitRevFactor}; 00145 *arm_cfft_radix4_instance_q15 S = {fftLen, ifftFlag, bitReverseFlag, pTwiddle, pBitRevTable, twidCoefModifier, bitRevFactor}; 00146 * </pre> 00147 * \par 00148 * where <code>fftLen</code> length of CFFT/CIFFT; <code>ifftFlag</code> Flag for selection of CFFT or CIFFT(Set ifftFlag to calculate CIFFT otherwise calculates CFFT); 00149 * <code>bitReverseFlag</code> Flag for selection of output order(Set bitReverseFlag to output in normal order otherwise output in bit reversed order); 00150 * <code>pTwiddle</code>points to array of twiddle coefficients; <code>pBitRevTable</code> points to the array of bit reversal table. 00151 * <code>twidCoefModifier</code> modifier for twiddle factor table which supports all FFT lengths with same table; 00152 * <code>pBitRevTable</code> modifier for bit reversal table which supports all FFT lengths with same table. 00153 * <code>onebyfftLen</code> value of 1/fftLen to calculate CIFFT; 00154 * 00155 * \par Fixed-Point Behavior 00156 * Care must be taken when using the fixed-point versions of the CFFT/CIFFT function. 00157 * Refer to the function specific documentation below for usage guidelines. 00158 */ 00159 00160 00161 /** 00162 * @addtogroup CFFT_CIFFT 00163 * @{ 00164 */ 00165 00166 /** 00167 * @details 00168 * @brief Processing function for the floating-point CFFT/CIFFT. 00169 * @param[in] *S points to an instance of the floating-point CFFT/CIFFT structure. 00170 * @param[in, out] *pSrc points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place. 00171 * @return none. 00172 */ 00173 00174 void arm_cfft_radix4_f32( 00175 const arm_cfft_radix4_instance_f32 * S, 00176 float32_t * pSrc) 00177 { 00178 00179 if(S->ifftFlag == 1u) 00180 { 00181 /* Complex IFFT radix-4 */ 00182 arm_radix4_butterfly_inverse_f32(pSrc, S->fftLen, S->pTwiddle, 00183 S->twidCoefModifier, S->onebyfftLen); 00184 } 00185 else 00186 { 00187 /* Complex FFT radix-4 */ 00188 arm_radix4_butterfly_f32(pSrc, S->fftLen, S->pTwiddle, 00189 S->twidCoefModifier); 00190 } 00191 00192 if(S->bitReverseFlag == 1u) 00193 { 00194 /* Bit Reversal */ 00195 arm_bitreversal_f32(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable); 00196 } 00197 00198 } 00199 00200 00201 /** 00202 * @} end of CFFT_CIFFT group 00203 */ 00204 00205 00206 00207 /* ---------------------------------------------------------------------- 00208 ** Internal helper function used by the FFTs 00209 ** ------------------------------------------------------------------- */ 00210 00211 /* 00212 * @brief Core function for the floating-point CFFT butterfly process. 00213 * @param[in, out] *pSrc points to the in-place buffer of floating-point data type. 00214 * @param[in] fftLen length of the FFT. 00215 * @param[in] *pCoef points to the twiddle coefficient buffer. 00216 * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. 00217 * @return none. 00218 */ 00219 00220 void arm_radix4_butterfly_f32( 00221 float32_t * pSrc, 00222 uint16_t fftLen, 00223 float32_t * pCoef, 00224 uint16_t twidCoefModifier) 00225 { 00226 00227 float32_t co1, co2, co3, si1, si2, si3; 00228 float32_t t1, t2, r1, r2, s1, s2; 00229 uint32_t ia1, ia2, ia3; 00230 uint32_t i0, i1, i2, i3; 00231 uint32_t n1, n2, j, k; 00232 00233 /* Initializations for the first stage */ 00234 n2 = fftLen; 00235 n1 = n2; 00236 00237 /* n2 = fftLen/4 */ 00238 n2 >>= 2u; 00239 i0 = 0u; 00240 ia1 = 0u; 00241 00242 j = n2; 00243 00244 /* Calculation of first stage */ 00245 do 00246 { 00247 /* index calculation for the input as, */ 00248 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ 00249 i1 = i0 + n2; 00250 i2 = i1 + n2; 00251 i3 = i2 + n2; 00252 00253 /* Butterfly implementation */ 00254 00255 /* xa + xc */ 00256 r1 = pSrc[(2u * i0)] + pSrc[(2u * i2)]; 00257 00258 /* xa - xc */ 00259 r2 = pSrc[2u * i0] - pSrc[2u * i2]; 00260 00261 /* ya + yc */ 00262 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u]; 00263 00264 /* ya - yc */ 00265 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u]; 00266 00267 /* xb + xd */ 00268 t1 = pSrc[2u * i1] + pSrc[2u * i3]; 00269 00270 /* xa' = xa + xb + xc + xd */ 00271 pSrc[2u * i0] = r1 + t1; 00272 00273 /* (xa + xc) - (xb + xd) */ 00274 r1 = r1 - t1; 00275 00276 /* yb + yd */ 00277 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u]; 00278 00279 /* ya' = ya + yb + yc + yd */ 00280 pSrc[(2u * i0) + 1u] = s1 + t2; 00281 00282 /* (ya + yc) - (yb + yd) */ 00283 s1 = s1 - t2; 00284 00285 /* yb - yd */ 00286 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u]; 00287 00288 /* xb - xd */ 00289 t2 = pSrc[2u * i1] - pSrc[2u * i3]; 00290 00291 /* index calculation for the coefficients */ 00292 ia2 = ia1 + ia1; 00293 co2 = pCoef[ia2 * 2u]; 00294 si2 = pCoef[(ia2 * 2u) + 1u]; 00295 00296 /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ 00297 pSrc[2u * i1] = (r1 * co2) + (s1 * si2); 00298 00299 /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ 00300 pSrc[(2u * i1) + 1u] = (s1 * co2) - (r1 * si2); 00301 00302 /* (xa - xc) + (yb - yd) */ 00303 r1 = r2 + t1; 00304 00305 /* (xa - xc) - (yb - yd) */ 00306 r2 = r2 - t1; 00307 00308 /* (ya - yc) - (xb - xd) */ 00309 s1 = s2 - t2; 00310 00311 /* (ya - yc) + (xb - xd) */ 00312 s2 = s2 + t2; 00313 00314 co1 = pCoef[ia1 * 2u]; 00315 si1 = pCoef[(ia1 * 2u) + 1u]; 00316 00317 /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ 00318 pSrc[2u * i2] = (r1 * co1) + (s1 * si1); 00319 00320 /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ 00321 pSrc[(2u * i2) + 1u] = (s1 * co1) - (r1 * si1); 00322 00323 /* index calculation for the coefficients */ 00324 ia3 = ia2 + ia1; 00325 co3 = pCoef[ia3 * 2u]; 00326 si3 = pCoef[(ia3 * 2u) + 1u]; 00327 00328 00329 /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ 00330 pSrc[2u * i3] = (r2 * co3) + (s2 * si3); 00331 00332 /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ 00333 pSrc[(2u * i3) + 1u] = (s2 * co3) - (r2 * si3); 00334 00335 /* Twiddle coefficients index modifier */ 00336 ia1 = ia1 + twidCoefModifier; 00337 00338 /* Updating input index */ 00339 i0 = i0 + 1u; 00340 00341 } 00342 while(--j); 00343 00344 twidCoefModifier <<= 2u; 00345 00346 /* Calculation of second stage to excluding last stage */ 00347 for (k = fftLen / 4; k > 4u; k >>= 2u) 00348 { 00349 /* Initializations for the first stage */ 00350 n1 = n2; 00351 n2 >>= 2u; 00352 ia1 = 0u; 00353 00354 /* Calculation of first stage */ 00355 for (j = 0u; j <= (n2 - 1u); j++) 00356 { 00357 /* index calculation for the coefficients */ 00358 ia2 = ia1 + ia1; 00359 ia3 = ia2 + ia1; 00360 co1 = pCoef[ia1 * 2u]; 00361 si1 = pCoef[(ia1 * 2u) + 1u]; 00362 co2 = pCoef[ia2 * 2u]; 00363 si2 = pCoef[(ia2 * 2u) + 1u]; 00364 co3 = pCoef[ia3 * 2u]; 00365 si3 = pCoef[(ia3 * 2u) + 1u]; 00366 00367 /* Twiddle coefficients index modifier */ 00368 ia1 = ia1 + twidCoefModifier; 00369 00370 for (i0 = j; i0 < fftLen; i0 += n1) 00371 { 00372 /* index calculation for the input as, */ 00373 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ 00374 i1 = i0 + n2; 00375 i2 = i1 + n2; 00376 i3 = i2 + n2; 00377 00378 /* xa + xc */ 00379 r1 = pSrc[(2u * i0)] + pSrc[(2u * i2)]; 00380 00381 /* xa - xc */ 00382 r2 = pSrc[(2u * i0)] - pSrc[(2u * i2)]; 00383 00384 /* ya + yc */ 00385 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u]; 00386 00387 /* ya - yc */ 00388 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u]; 00389 00390 /* xb + xd */ 00391 t1 = pSrc[2u * i1] + pSrc[2u * i3]; 00392 00393 /* xa' = xa + xb + xc + xd */ 00394 pSrc[2u * i0] = r1 + t1; 00395 00396 /* xa + xc -(xb + xd) */ 00397 r1 = r1 - t1; 00398 00399 /* yb + yd */ 00400 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u]; 00401 00402 /* ya' = ya + yb + yc + yd */ 00403 pSrc[(2u * i0) + 1u] = s1 + t2; 00404 00405 /* (ya + yc) - (yb + yd) */ 00406 s1 = s1 - t2; 00407 00408 /* (yb - yd) */ 00409 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u]; 00410 00411 /* (xb - xd) */ 00412 t2 = pSrc[2u * i1] - pSrc[2u * i3]; 00413 00414 /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ 00415 pSrc[2u * i1] = (r1 * co2) + (s1 * si2); 00416 00417 /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ 00418 pSrc[(2u * i1) + 1u] = (s1 * co2) - (r1 * si2); 00419 00420 /* (xa - xc) + (yb - yd) */ 00421 r1 = r2 + t1; 00422 00423 /* (xa - xc) - (yb - yd) */ 00424 r2 = r2 - t1; 00425 00426 /* (ya - yc) - (xb - xd) */ 00427 s1 = s2 - t2; 00428 00429 /* (ya - yc) + (xb - xd) */ 00430 s2 = s2 + t2; 00431 00432 /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ 00433 pSrc[2u * i2] = (r1 * co1) + (s1 * si1); 00434 00435 /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ 00436 pSrc[(2u * i2) + 1u] = (s1 * co1) - (r1 * si1); 00437 00438 /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ 00439 pSrc[2u * i3] = (r2 * co3) + (s2 * si3); 00440 00441 /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ 00442 pSrc[(2u * i3) + 1u] = (s2 * co3) - (r2 * si3); 00443 } 00444 } 00445 twidCoefModifier <<= 2u; 00446 } 00447 00448 /* Initializations of last stage */ 00449 n1 = n2; 00450 n2 >>= 2u; 00451 00452 /* Calculations of last stage */ 00453 for (i0 = 0u; i0 <= (fftLen - n1); i0 += n1) 00454 { 00455 /* index calculation for the input as, */ 00456 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ 00457 i1 = i0 + n2; 00458 i2 = i1 + n2; 00459 i3 = i2 + n2; 00460 00461 /* Butterfly implementation */ 00462 00463 /* xa + xb */ 00464 r1 = pSrc[2u * i0] + pSrc[2u * i2]; 00465 00466 /* xa - xb */ 00467 r2 = pSrc[2u * i0] - pSrc[2u * i2]; 00468 00469 /* ya + yc */ 00470 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u]; 00471 00472 /* ya - yc */ 00473 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u]; 00474 00475 /* xc + xd */ 00476 t1 = pSrc[2u * i1] + pSrc[2u * i3]; 00477 00478 /* xa' = xa + xb + xc + xd */ 00479 pSrc[2u * i0] = r1 + t1; 00480 00481 /* (xa + xb) - (xc + xd) */ 00482 r1 = r1 - t1; 00483 00484 /* yb + yd */ 00485 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u]; 00486 00487 /* ya' = ya + yb + yc + yd */ 00488 pSrc[(2u * i0) + 1u] = s1 + t2; 00489 00490 /* (ya + yc) - (yb + yd) */ 00491 s1 = s1 - t2; 00492 00493 /* (yb-yd) */ 00494 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u]; 00495 00496 /* (xb-xd) */ 00497 t2 = pSrc[2u * i1] - pSrc[2u * i3]; 00498 00499 /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ 00500 pSrc[2u * i1] = r1; 00501 00502 /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ 00503 pSrc[(2u * i1) + 1u] = s1; 00504 00505 /* (xa+yb-xc-yd) */ 00506 r1 = r2 + t1; 00507 00508 /* (xa-yb-xc+yd) */ 00509 r2 = r2 - t1; 00510 00511 /* (ya-xb-yc+xd) */ 00512 s1 = s2 - t2; 00513 00514 /* (ya+xb-yc-xd) */ 00515 s2 = s2 + t2; 00516 00517 /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ 00518 pSrc[2u * i2] = r1; 00519 00520 /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ 00521 pSrc[(2u * i2) + 1u] = s1; 00522 00523 /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ 00524 pSrc[2u * i3] = r2; 00525 00526 /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ 00527 pSrc[(2u * i3) + 1u] = s2; 00528 } 00529 } 00530 00531 /* 00532 * @brief Core function for the floating-point CIFFT butterfly process. 00533 * @param[in, out] *pSrc points to the in-place buffer of floating-point data type. 00534 * @param[in] fftLen length of the FFT. 00535 * @param[in] *pCoef points to twiddle coefficient buffer. 00536 * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. 00537 * @param[in] onebyfftLen value of 1/fftLen. 00538 * @return none. 00539 */ 00540 00541 void arm_radix4_butterfly_inverse_f32( 00542 float32_t * pSrc, 00543 uint16_t fftLen, 00544 float32_t * pCoef, 00545 uint16_t twidCoefModifier, 00546 float32_t onebyfftLen) 00547 { 00548 float32_t co1, co2, co3, si1, si2, si3; 00549 float32_t t1, t2, r1, r2, s1, s2; 00550 uint32_t ia1, ia2, ia3; 00551 uint32_t i0, i1, i2, i3; 00552 uint32_t n1, n2, j, k; 00553 00554 /* Initializations for the first stage */ 00555 n2 = fftLen; 00556 n1 = n2; 00557 00558 /* n2 = fftLen/4 */ 00559 n2 >>= 2u; 00560 i0 = 0u; 00561 ia1 = 0u; 00562 00563 j = n2; 00564 00565 /* Calculation of first stage */ 00566 do 00567 { 00568 /* index calculation for the input as, */ 00569 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ 00570 i1 = i0 + n2; 00571 i2 = i1 + n2; 00572 i3 = i2 + n2; 00573 00574 /* Butterfly implementation */ 00575 /* xa + xc */ 00576 r1 = pSrc[(2u * i0)] + pSrc[(2u * i2)]; 00577 00578 /* xa - xc */ 00579 r2 = pSrc[2u * i0] - pSrc[2u * i2]; 00580 00581 /* ya + yc */ 00582 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u]; 00583 00584 /* ya - yc */ 00585 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u]; 00586 00587 /* xb + xd */ 00588 t1 = pSrc[2u * i1] + pSrc[2u * i3]; 00589 00590 /* xa' = xa + xb + xc + xd */ 00591 pSrc[2u * i0] = r1 + t1; 00592 00593 /* (xa + xc) - (xb + xd) */ 00594 r1 = r1 - t1; 00595 00596 /* yb + yd */ 00597 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u]; 00598 00599 /* ya' = ya + yb + yc + yd */ 00600 pSrc[(2u * i0) + 1u] = s1 + t2; 00601 00602 /* (ya + yc) - (yb + yd) */ 00603 s1 = s1 - t2; 00604 00605 /* yb - yd */ 00606 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u]; 00607 00608 /* xb - xd */ 00609 t2 = pSrc[2u * i1] - pSrc[2u * i3]; 00610 00611 /* index calculation for the coefficients */ 00612 ia2 = ia1 + ia1; 00613 co2 = pCoef[ia2 * 2u]; 00614 si2 = pCoef[(ia2 * 2u) + 1u]; 00615 00616 /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ 00617 pSrc[2u * i1] = (r1 * co2) - (s1 * si2); 00618 00619 /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ 00620 pSrc[(2u * i1) + 1u] = (s1 * co2) + (r1 * si2); 00621 00622 /* (xa - xc) - (yb - yd) */ 00623 r1 = r2 - t1; 00624 00625 /* (xa - xc) + (yb - yd) */ 00626 r2 = r2 + t1; 00627 00628 /* (ya - yc) + (xb - xd) */ 00629 s1 = s2 + t2; 00630 00631 /* (ya - yc) - (xb - xd) */ 00632 s2 = s2 - t2; 00633 00634 co1 = pCoef[ia1 * 2u]; 00635 si1 = pCoef[(ia1 * 2u) + 1u]; 00636 00637 /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ 00638 pSrc[2u * i2] = (r1 * co1) - (s1 * si1); 00639 00640 /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ 00641 pSrc[(2u * i2) + 1u] = (s1 * co1) + (r1 * si1); 00642 00643 /* index calculation for the coefficients */ 00644 ia3 = ia2 + ia1; 00645 co3 = pCoef[ia3 * 2u]; 00646 si3 = pCoef[(ia3 * 2u) + 1u]; 00647 00648 /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ 00649 pSrc[2u * i3] = (r2 * co3) - (s2 * si3); 00650 00651 /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ 00652 pSrc[(2u * i3) + 1u] = (s2 * co3) + (r2 * si3); 00653 00654 /* Twiddle coefficients index modifier */ 00655 ia1 = ia1 + twidCoefModifier; 00656 00657 /* Updating input index */ 00658 i0 = i0 + 1u; 00659 00660 } 00661 while(--j); 00662 00663 twidCoefModifier <<= 2u; 00664 00665 /* Calculation of second stage to excluding last stage */ 00666 for (k = fftLen / 4; k > 4u; k >>= 2u) 00667 { 00668 /* Initializations for the first stage */ 00669 n1 = n2; 00670 n2 >>= 2u; 00671 ia1 = 0u; 00672 00673 /* Calculation of first stage */ 00674 for (j = 0u; j <= (n2 - 1u); j++) 00675 { 00676 /* index calculation for the coefficients */ 00677 ia2 = ia1 + ia1; 00678 ia3 = ia2 + ia1; 00679 co1 = pCoef[ia1 * 2u]; 00680 si1 = pCoef[(ia1 * 2u) + 1u]; 00681 co2 = pCoef[ia2 * 2u]; 00682 si2 = pCoef[(ia2 * 2u) + 1u]; 00683 co3 = pCoef[ia3 * 2u]; 00684 si3 = pCoef[(ia3 * 2u) + 1u]; 00685 00686 /* Twiddle coefficients index modifier */ 00687 ia1 = ia1 + twidCoefModifier; 00688 00689 for (i0 = j; i0 < fftLen; i0 += n1) 00690 { 00691 /* index calculation for the input as, */ 00692 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ 00693 i1 = i0 + n2; 00694 i2 = i1 + n2; 00695 i3 = i2 + n2; 00696 00697 /* xa + xc */ 00698 r1 = pSrc[(2u * i0)] + pSrc[(2u * i2)]; 00699 00700 /* xa - xc */ 00701 r2 = pSrc[(2u * i0)] - pSrc[(2u * i2)]; 00702 00703 /* ya + yc */ 00704 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u]; 00705 00706 /* ya - yc */ 00707 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u]; 00708 00709 /* xb + xd */ 00710 t1 = pSrc[2u * i1] + pSrc[2u * i3]; 00711 00712 /* xa' = xa + xb + xc + xd */ 00713 pSrc[2u * i0] = r1 + t1; 00714 00715 /* xa + xc -(xb + xd) */ 00716 r1 = r1 - t1; 00717 00718 /* yb + yd */ 00719 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u]; 00720 00721 /* ya' = ya + yb + yc + yd */ 00722 pSrc[(2u * i0) + 1u] = s1 + t2; 00723 00724 /* (ya + yc) - (yb + yd) */ 00725 s1 = s1 - t2; 00726 00727 /* (yb - yd) */ 00728 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u]; 00729 00730 /* (xb - xd) */ 00731 t2 = pSrc[2u * i1] - pSrc[2u * i3]; 00732 00733 /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ 00734 pSrc[2u * i1] = (r1 * co2) - (s1 * si2); 00735 00736 /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ 00737 pSrc[(2u * i1) + 1u] = (s1 * co2) + (r1 * si2); 00738 00739 /* (xa - xc) - (yb - yd) */ 00740 r1 = r2 - t1; 00741 00742 /* (xa - xc) + (yb - yd) */ 00743 r2 = r2 + t1; 00744 00745 /* (ya - yc) + (xb - xd) */ 00746 s1 = s2 + t2; 00747 00748 /* (ya - yc) - (xb - xd) */ 00749 s2 = s2 - t2; 00750 00751 /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ 00752 pSrc[2u * i2] = (r1 * co1) - (s1 * si1); 00753 00754 /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ 00755 pSrc[(2u * i2) + 1u] = (s1 * co1) + (r1 * si1); 00756 00757 /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ 00758 pSrc[2u * i3] = (r2 * co3) - (s2 * si3); 00759 00760 /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ 00761 pSrc[(2u * i3) + 1u] = (s2 * co3) + (r2 * si3); 00762 } 00763 } 00764 twidCoefModifier <<= 2u; 00765 } 00766 00767 /* Initializations of last stage */ 00768 n1 = n2; 00769 n2 >>= 2u; 00770 00771 /* Calculations of last stage */ 00772 for (i0 = 0u; i0 <= (fftLen - n1); i0 += n1) 00773 { 00774 /* index calculation for the input as, */ 00775 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ 00776 i1 = i0 + n2; 00777 i2 = i1 + n2; 00778 i3 = i2 + n2; 00779 00780 /* Butterfly implementation */ 00781 /* xa + xc */ 00782 r1 = pSrc[2u * i0] + pSrc[2u * i2]; 00783 00784 /* xa - xc */ 00785 r2 = pSrc[2u * i0] - pSrc[2u * i2]; 00786 00787 /* ya + yc */ 00788 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u]; 00789 00790 /* ya - yc */ 00791 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u]; 00792 00793 /* xc + xd */ 00794 t1 = pSrc[2u * i1] + pSrc[2u * i3]; 00795 00796 /* xa' = xa + xb + xc + xd */ 00797 pSrc[2u * i0] = (r1 + t1) * onebyfftLen; 00798 00799 /* (xa + xb) - (xc + xd) */ 00800 r1 = r1 - t1; 00801 00802 /* yb + yd */ 00803 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u]; 00804 00805 /* ya' = ya + yb + yc + yd */ 00806 pSrc[(2u * i0) + 1u] = (s1 + t2) * onebyfftLen; 00807 00808 /* (ya + yc) - (yb + yd) */ 00809 s1 = s1 - t2; 00810 00811 /* (yb-yd) */ 00812 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u]; 00813 00814 /* (xb-xd) */ 00815 t2 = pSrc[2u * i1] - pSrc[2u * i3]; 00816 00817 /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ 00818 pSrc[2u * i1] = r1 * onebyfftLen; 00819 00820 /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ 00821 pSrc[(2u * i1) + 1u] = s1 * onebyfftLen; 00822 00823 00824 /* (xa - xc) - (yb-yd) */ 00825 r1 = r2 - t1; 00826 00827 /* (xa - xc) + (yb-yd) */ 00828 r2 = r2 + t1; 00829 00830 /* (ya - yc) + (xb-xd) */ 00831 s1 = s2 + t2; 00832 00833 /* (ya - yc) - (xb-xd) */ 00834 s2 = s2 - t2; 00835 00836 /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ 00837 pSrc[2u * i2] = r1 * onebyfftLen; 00838 00839 /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ 00840 pSrc[(2u * i2) + 1u] = s1 * onebyfftLen; 00841 00842 /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ 00843 pSrc[2u * i3] = r2 * onebyfftLen; 00844 00845 /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ 00846 pSrc[(2u * i3) + 1u] = s2 * onebyfftLen; 00847 } 00848 } 00849 00850 /* 00851 * @brief In-place bit reversal function. 00852 * @param[in, out] *pSrc points to the in-place buffer of floating-point data type. 00853 * @param[in] fftSize length of the FFT. 00854 * @param[in] bitRevFactor bit reversal modifier that supports different size FFTs with the same bit reversal table. 00855 * @param[in] *pBitRevTab points to the bit reversal table. 00856 * @return none. 00857 */ 00858 00859 void arm_bitreversal_f32( 00860 float32_t * pSrc, 00861 uint16_t fftSize, 00862 uint16_t bitRevFactor, 00863 uint16_t * pBitRevTab) 00864 { 00865 uint16_t fftLenBy2, fftLenBy2p1; 00866 uint16_t i, j; 00867 float32_t in; 00868 00869 /* Initializations */ 00870 j = 0u; 00871 fftLenBy2 = fftSize >> 1u; 00872 fftLenBy2p1 = (fftSize >> 1u) + 1u; 00873 00874 /* Bit Reversal Implementation */ 00875 for (i = 0u; i <= (fftLenBy2 - 2u); i += 2u) 00876 { 00877 if(i < j) 00878 { 00879 /* pSrc[i] <-> pSrc[j]; */ 00880 in = pSrc[2u * i]; 00881 pSrc[2u * i] = pSrc[2u * j]; 00882 pSrc[2u * j] = in; 00883 00884 /* pSrc[i+1u] <-> pSrc[j+1u] */ 00885 in = pSrc[(2u * i) + 1u]; 00886 pSrc[(2u * i) + 1u] = pSrc[(2u * j) + 1u]; 00887 pSrc[(2u * j) + 1u] = in; 00888 00889 /* pSrc[i+fftLenBy2p1] <-> pSrc[j+fftLenBy2p1] */ 00890 in = pSrc[2u * (i + fftLenBy2p1)]; 00891 pSrc[2u * (i + fftLenBy2p1)] = pSrc[2u * (j + fftLenBy2p1)]; 00892 pSrc[2u * (j + fftLenBy2p1)] = in; 00893 00894 /* pSrc[i+fftLenBy2p1+1u] <-> pSrc[j+fftLenBy2p1+1u] */ 00895 in = pSrc[(2u * (i + fftLenBy2p1)) + 1u]; 00896 pSrc[(2u * (i + fftLenBy2p1)) + 1u] = 00897 pSrc[(2u * (j + fftLenBy2p1)) + 1u]; 00898 pSrc[(2u * (j + fftLenBy2p1)) + 1u] = in; 00899 00900 } 00901 00902 /* pSrc[i+1u] <-> pSrc[j+1u] */ 00903 in = pSrc[2u * (i + 1u)]; 00904 pSrc[2u * (i + 1u)] = pSrc[2u * (j + fftLenBy2)]; 00905 pSrc[2u * (j + fftLenBy2)] = in; 00906 00907 /* pSrc[i+2u] <-> pSrc[j+2u] */ 00908 in = pSrc[(2u * (i + 1u)) + 1u]; 00909 pSrc[(2u * (i + 1u)) + 1u] = pSrc[(2u * (j + fftLenBy2)) + 1u]; 00910 pSrc[(2u * (j + fftLenBy2)) + 1u] = in; 00911 00912 /* Reading the index for the bit reversal */ 00913 j = *pBitRevTab; 00914 00915 /* Updating the bit reversal index depending on the fft length */ 00916 pBitRevTab += bitRevFactor; 00917 } 00918 }
Generated on Tue Jul 12 2022 14:13:52 by
1.7.2