DSP

C#编写的FFT实现类

2019-07-13 20:16发布

站内文章 / DSP
12402 0 1222
using System; using System.Collections.Generic; using System.Linq; using System.Numerics; //需要在工程中引用 Numerics函数集 using System.Text; using System.Threading.Tasks; using System.Runtime.InteropServices; namespace Device.RA.Controls.Content { public class FFT2 : XCommon.Disposable { const string DLLName = "FFTProcDll.dll"; [DllImport(DLLName, CallingConvention = CallingConvention.Cdecl)] static extern IntPtr Fft_Init(int order); [DllImport(DLLName, CallingConvention = CallingConvention.Cdecl)] static extern void FftCtl_Destory(IntPtr FftHandle); //FFT运算处理 [DllImport(DLLName, CallingConvention = CallingConvention.Cdecl)] static extern unsafe void FftCtl_ddcFFT(IntPtr FftHandle, byte[] DdcDatInput, ref float[] SpecDatOut, int Point, ref int MaxValue, int WinOpt = 0); IntPtr _ptr; float[] _result; public void Initialize(int length) { _ptr = Fft_Init((int)Math.Log(length, 2)); _result = new float[length]; } public unsafe float[] Execute(byte[] data) { int max = Int32.MinValue; //fixed (float* pFloat = _result) { FftCtl_ddcFFT(_ptr, data, ref _result, _result.Length, ref max); } return _result; } protected override void Dispose(bool disposing) { if (disposing) { if (_ptr != IntPtr.Zero) { FftCtl_Destory(_ptr); _ptr = IntPtr.Zero; } } } } public class FFT { /// /// FFT Class /// public FFT() { } #region Private Properties private double mFFTScale = 1.0; private UInt32 mLogN = 0; // log2 of FFT size private UInt32 mN = 0; // Time series length private UInt32 mLengthTotal; // mN + mZp private UInt32 mLengthHalf; // (mN + mZp) / 2 private FFTElement[] mX; // Vector of linked list elements // Element for linked list to store input/output data. private class FFTElement { public double re = 0.0; // Real component public double im = 0.0; // Imaginary component public FFTElement next; // Next element in linked list public UInt32 revTgt; // Target position post bit-reversal } #endregion #region FFT Core Functions /// /// Initialize the FFT. Must call first and this anytime the FFT setup changes. /// /// /// public void Initialize(UInt32 inputDataLength, UInt32 zeroPaddingLength = 0) { mN = inputDataLength; // Find the power of two for the total FFT size up to 2^32 bool foundIt = false; for (mLogN = 1; mLogN <= 32; mLogN++) { double n = Math.Pow(2.0, mLogN); if ((inputDataLength + zeroPaddingLength) == n) { foundIt = true; break; } } if (foundIt == false) throw new ArgumentOutOfRangeException("inputDataLength + zeroPaddingLength was not an even power of 2! FFT cannot continue."); // Set global parameters. mLengthTotal = inputDataLength + zeroPaddingLength; mLengthHalf = (mLengthTotal / 2) + 1; // Set the overall scale factor for all the terms mFFTScale = Math.Sqrt(2) / (double)(mLengthTotal); // Natural FFT Scale Factor // Window Scale Factor mFFTScale *= ((double)mLengthTotal) / (double)inputDataLength; // Zero Padding Scale Factor // Allocate elements for linked list of complex numbers. mX = new FFTElement[mLengthTotal]; for (UInt32 k = 0; k < (mLengthTotal); k++) mX[k] = new FFTElement(); // Set up "next" pointers. for (UInt32 k = 0; k < (mLengthTotal) - 1; k++) mX[k].next = mX[k + 1]; // Specify target for bit reversal re-ordering. for (UInt32 k = 0; k < (mLengthTotal); k++) mX[k].revTgt = BitReverse(k, mLogN); } /// /// Executes a FFT of the input time series. /// /// /// Complex[] Spectrum public Complex[] Execute(double[] timeSeries) { UInt32 numFlies = mLengthTotal >> 1; // Number of butterflies per sub-FFT UInt32 span = mLengthTotal >> 1; // Width of the butterfly UInt32 spacing = mLengthTotal; // Distance between start of sub-FFTs UInt32 wIndexStep = 1; // Increment for twiddle table index // Copy data into linked complex number objects FFTElement x = mX[0]; UInt32 k = 0; for (UInt32 i = 0; i < mN; i++) { x.re = timeSeries[k]; x.im = 0.0; x = x.next; k++; } // If zero padded, clean the 2nd half of the linked list from previous results if (mN != mLengthTotal) { for (UInt32 i = mN; i < mLengthTotal; i++) { x.re = 0.0; x.im = 0.0; x = x.next; } } // For each stage of the FFT for (UInt32 stage = 0; stage < mLogN; stage++) { // Compute a multiplier factor for the "twiddle factors". // The twiddle factors are complex unit vectors spaced at // regular angular intervals. The angle by which the twiddle // factor advances depends on the FFT stage. In many FFT // implementations the twiddle factors are cached, but because // array lookup is relatively slow in C#, it's just // as fast to compute them on the fly. double wAngleInc = wIndexStep * -2.0 * Math.PI / (mLengthTotal); double wMulRe = Math.Cos(wAngleInc); double wMulIm = Math.Sin(wAngleInc); for (UInt32 start = 0; start < (mLengthTotal); start += spacing) { FFTElement xTop = mX[start]; FFTElement xBot = mX[start + span]; double wRe = 1.0; double wIm = 0.0; // For each butterfly in this stage for (UInt32 flyCount = 0; flyCount < numFlies; ++flyCount) { // Get the top & bottom values double xTopRe = xTop.re; double xTopIm = xTop.im; double xBotRe = xBot.re; double xBotIm = xBot.im; // Top branch of butterfly has addition xTop.re = xTopRe + xBotRe; xTop.im = xTopIm + xBotIm; // Bottom branch of butterfly has subtraction, // followed by multiplication by twiddle factor xBotRe = xTopRe - xBotRe; xBotIm = xTopIm - xBotIm; xBot.re = xBotRe * wRe - xBotIm * wIm; xBot.im = xBotRe * wIm + xBotIm * wRe; // Advance butterfly to next top & bottom positions xTop = xTop.next; xBot = xBot.next; // Update the twiddle factor, via complex multiply // by unit vector with the appropriate angle // (wRe + j wIm) = (wRe + j wIm) x (wMulRe + j wMulIm) double tRe = wRe; wRe = wRe * wMulRe - wIm * wMulIm; wIm = tRe * wMulIm + wIm * wMulRe; } } numFlies >>= 1; // Divide by 2 by right shift span >>= 1; spacing >>= 1; wIndexStep <<= 1; // Multiply by 2 by left shift } // The algorithm leaves the result in a scrambled order. // Unscramble while copying values from the complex // linked list elements to a complex output vector & properly apply scale factors. x = mX[0]; Complex[] unswizzle = new Complex[mLengthTotal]; while (x != null) { UInt32 target = x.revTgt; unswizzle[target] = new Complex(x.re * mFFTScale, x.im * mFFTScale); x = x.next; } // Return 1/2 the FFT result from DC to Fs/2 (The real part of the spectrum) //UInt32 halfLength = ((mN + mZp) / 2) + 1; Complex[] result = new Complex[mLengthHalf]; Array.Copy(unswizzle, result, mLengthHalf); // DC and Fs/2 Points are scaled differently, since they have only a real part result[0] = new Complex(result[0].Real / Math.Sqrt(2), 0.0); result[mLengthHalf - 1] = new Complex(result[mLengthHalf - 1].Real / Math.Sqrt(2), 0.0); return result; } #region Private FFT Routines /** * Do bit reversal of specified number of places of an int * For example, 1101 bit-reversed is 1011 * * @param x Number to be bit-reverse. * @param numBits Number of bits in the number. */ private UInt32 BitReverse(UInt32 x, UInt32 numBits) { UInt32 y = 0; for (UInt32 i = 0; i < numBits; i++) { y <<= 1; y |= x & 0x0001; x >>= 1; } return y; } #endregion #endregion #region Utility Functions /// /// Return the Frequency Array for the currently defined FFT. /// Takes into account the total number of points and zero padding points that were defined. /// /// /// public double[] FrequencySpan(double samplingFrequencyHz) { UInt32 points = (UInt32)mLengthHalf; double[] result = new double[points]; double stopValue = samplingFrequencyHz / 2.0; double increment = stopValue / ((double)points - 1.0); for (Int32 i = 0; i < points; i++) result[i] += increment * i; return result; } #endregion } }

热门文章