![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Arianna autonomous DAQ firmware
Dependencies: mbed SDFileSystemFilinfo AriSnProtocol NetServicesMin AriSnComm MODSERIAL PowerControlClkPatch DS1820OW
SnSigProcUtils.h
- Committer:
- uci1
- Date:
- 2019-06-05
- Revision:
- 125:ce4045184366
- Parent:
- 84:80b15993944e
File content as of revision 125:ce4045184366:
#ifndef SN_SnSigProcUtils #define SN_SnSigProcUtils #include <cmath> #include <stdint.h> #include "SnConstants.h" #include "SnSigProcDataTable.h" struct SnSigProcUtils { static const double kPi; static const double kTwoPi; template<typename Num_t, typename Res_t> static void GetArraySumAndMaxSingleLoop(const Num_t* const data, const uint32_t n, Res_t& sum, Res_t& max) { const Num_t* d = data; max = static_cast<Res_t>(*d); sum = static_cast<Res_t>(*d); ++d; // skip past element 0 for (uint32_t i=1; i<n; ++i, ++d) { sum += static_cast<Res_t>(*d); if (*d > max) { max = static_cast<Res_t>(*d); } } } template<typename Dst_t, typename Src_t> static void MemCpyOrLoopCpy(Dst_t* const dst, const Src_t* const src, const uint32_t n) { #ifdef DEBUG_ printf("MemCpyOrLoopCpy : loop\r\n"); #endif Dst_t* d = dst; const Src_t* s = src; for (uint32_t i=0; i<n; ++i, ++d, ++s) { *d = static_cast<Dst_t>(*s); } } template<typename Same_t> static void MemCpyOrLoopCpy(Same_t* const dst, const Same_t* const src, const uint32_t n) { // can only memcpy if the types are the same #ifdef DEBUG_ printf("MemCpyOrLoopCpy : memcpy\r\n"); #endif memcpy(dst, src, sizeof(Same_t)*n); } // the number of samples is a template parameter rather than // an input parameter so that we can be sure to use the proper // lookup tables, and that we should be somewhat safe against // using the wrong tables if the number of samples changes in the // future, as the code should no longer compile. template<uint32_t nn, typename Num_t> static bool DiscreteCpxFFT(Num_t* const data, const bool inverse) { // replace the data in the 'data' array by its discrete, complex-valued FFT // // if inverse is true, will do the inverse FFT // (unlike the function provided in NumRecC, the inverse FFT will // be provided already be normalized) // so calling DiscreteCpxFFT inverse on the result of // DiscreteCpxFFT forward will yield the original data (within // numerical accuracy) // // See RealFFT for notes on normalization. However, unlike RealFFT, // the factors of 2 and special bin treatments are not necessary // for the complex FFT, since both positive and negative // frequencies are stored in their own bins. // // NOTE: "data" must be of length 2*nn!! since the calculation is done // in place. the time domain data should be in the first half of "data". // nn must be a power of two // the fft is organized like: [ real_0, img_0, real_1, img_1, ... ] // // NUM REC C - 12.2, pg 507-508 // // return true iff FFT succeeded if ( ((nn-1)&nn)!=0 ) { #ifdef DEBUG printf("<DiscreteCpxFFT>: nn (=%u) must be a power of 2.",nn); return false; #endif } else { static const uint32_t n = nn<<1; // nn * 2 Num_t tempr,tempi; uint32_t m; uint32_t i,j=1; Num_t* di=data, * di1=data+1, * dj, * dj1; #ifdef USE_DFFT_LUTS const typename SnSigProcDataTable<n>::DfftDivideIndicies_t* kdi = SnSigProcDataTable<n>::kDfftDivideIndicies; #endif for (i=1; i<n; i+=2, di+=2, di1+=2) { if (j > i) { dj = data + j - 1; dj1 = dj + 1; tempr = *dj; *dj = *di; *di = tempr; tempr = *dj1; *dj1 = *di1; *di1 = tempr; } #ifdef USE_DFFT_LUTS j = *kdi++; m = *kdi++; #else m = nn; while (m>1 && j>m) { j -= m; m >>= 1; } j += m; #endif } #ifdef USE_DFFT_LUTS double wr, wi; const typename SnSigProcDataTable<n>::DfftTwidleFactor_t* ktf = SnSigProcDataTable<n>::kDfftTwidleFactors; #else double wtemp, wr, wpr, wpi, wi, theta; const int8_t isign = inverse ? -1 : 1; #endif uint32_t mmax=2, istep; while (n > mmax) { istep = mmax<<1; #ifndef USE_DFFT_LUTS // if not defined! theta = isign * (kTwoPi / mmax); wtemp = std::sin(0.5*theta); wpr = -2.0 * wtemp * wtemp; wpi = std::sin(theta); wr = 1.0; wi = 0.0; #endif for (m=1; m<mmax; m+=2) { #ifdef USE_DFFT_LUTS wr = *ktf++; wi = *ktf++; if (inverse) { wi = -wi; } #endif for (i=m; i<=n; i+=istep) { j = i + mmax; dj = data + j - 1; dj1 = dj+1; di = data + i - 1; di1 = di+1; tempr = wr * (*dj) - wi*(*dj1); tempi = wr * (*dj1) + wi*(*dj); *dj = *di - tempr; *dj1 = *di1 - tempi; *di += tempr; *di1 += tempi; } #ifndef USE_DFFT_LUTS // if not defined! wtemp = wr; wr = wtemp*wpr - wi*wpi + wr; wi = wi*wpr + wtemp*wpi + wi; #endif } mmax = istep; } if (inverse) { // scale amplitude back di = data; static const Num_t tn = 2.0/static_cast<Num_t>(n); for (i=0; i<n; ++i, ++di) { *di *= tn; } } } return true; } // the number of samples is a template parameter rather than // an input parameter so that we can be sure to use the proper // lookup tables, and that we should be somewhat safe against // using the wrong tables if the number of samples changes in the // future, as the code should no longer compile. template<uint32_t n, typename Num_t> static bool RealFFT(Num_t* const data, const bool inverse) { // replace the real-valued data with the positive frequency half of its // complex fourier transform // // the real valued first & last components of the complex transform // are stored in data[0] and data[1], respectively // // n must be a power of 2 // // data goes from [0..n-1] // // Note on the normalization convention used: // The 2/n normalization is applied only on the inverse FFT. // * To normalize the *amplitude* of frequencies in the forward // FFT, multiply by 2/n in bins [2..n-1] (which store two both // the pos and neg frequencies) and 1/n in bins [0..1]. For // example, if you have in the time domain (200*sine_50MHz + // 300*sine_230MHz) and you want to see a spike at 50MHz with // amplitude 200 and a spike at 230MHz with amplitude 300, you // should multiply bins 0 and 1 by 1/n and the other bins by 2/n. // * To normalize the *total power* of the forward FFT, multiply // by 2/sqrt(n) for bins [2..n-1] (which store two both // the pos and neg frequencies) and 1/sqrt(n) in bins [0..1]. // This is the normalization chosen by NewGraphFromRealFFT when its // "normalize" parameter is true. // // if inverse is true, will perform the inverse FFT // (unlike the function provided in NumRecC, the inverse FFT will // be provided already be mulitplied by 2/n) -- so taking the inverse // FFT of the result of the forward FFT will yield directly the original // data (within numerical accuracy). // // NUM REC C - 12.3, pg 513-514 // // return true iff the FFT succeeded if ( ((n-1)&n)!=0 ) { #ifdef DEBUG printf("<RealFFT>: n (=%u) must be a power of 2.",n); #endif return false; } else { double theta = kPi / static_cast<double>(n>>1); float c1=0.5, c2; if (inverse) { c2 = 0.5; theta = -theta; // set up for inverse tranform } else { c2 = -0.5; DiscreteCpxFFT<(n/2)>(data,false); // forward FFT } #ifdef USE_DFFT_LUTS typename SnSigProcDataTable<n>::RealDfftTwidleFactor_t wr, wi; const typename SnSigProcDataTable<n>::RealDfftTwidleFactor_t * krtf = SnSigProcDataTable<n>::kRealDfftTwidleFactor; #else double wtemp = std::sin(0.5*theta); const double wpr = -2.0*wtemp*wtemp; const double wpi = std::sin(theta); double wr = 1.0+wpr; double wi = wpi; #endif float h1r, h1i, h2r, h2i; const uint32_t n4 = n>>2; // n/4 uint32_t i; Num_t* d1 = data+2, * d2 = d1+1, * d3 = data+n-2, * d4 = d3+1; for (i=1; i<n4; ++i, d1+=2, d2+=2, d3-=2, d4-=2) { #ifdef USE_DFFT_LUTS wr = *krtf++; wi = *krtf++; if (inverse) { wi = -wi; } #endif h1r = c1 * ((*d1)+(*d3)); h1i = c1 * ((*d2)-(*d4)); h2r = -c2 * ((*d2)+(*d4)); h2i = c2 * ((*d1)-(*d3)); *d1 = h1r + wr*h2r - wi*h2i; *d2 = h1i + wr*h2i + wi*h2r; *d3 = h1r - wr*h2r + wi*h2i; *d4 = -h1i + wr*h2i + wi*h2r; #ifndef USE_DFFT_LUTS // if not defined! wtemp = wr; wr = wtemp*wpr - wi*wpi + wr; wi = wi*wpr + wtemp*wpi + wi; #endif } // now fill in the beginning and end of the array if (inverse) { h1r = *data; d1 = data+1; *data = c1*(h1r + (*d1)); *d1 = c1*(h1r - (*d1)); DiscreteCpxFFT<(n/2)>(data,inverse); // inverse tranform } else { h1r = *data; d1 = data+1; *data = h1r + (*d1); *d1 = h1r - (*d1); } } return true; } template<typename Num_t> static uint32_t ReplaceRfftWithSpectrum(Num_t* const rfft, const uint32_t n, const bool doSqrt) { // rfft should contain the result of RealFFT and n is the number // of samples. // this routine will replace the first half of the array // with the "spectrum" (frequency magnitude) at each freq bin. // valid freq bins are from [0, (n/2)+1] // // return the number of freq bins filled, or else 0 on error if (rfft!=0) { //const uint32_t hn = n>>1; // n/2 //uint32_t j=0; // set the first freq bin rfft[0] = (doSqrt) ? std::abs(rfft[0]) : (rfft[0]*rfft[0]); // store the last freq bin const Num_t hibin = (doSqrt) ? std::abs(rfft[1]) : (rfft[1]*rfft[1]); // loop over other freqs const Num_t* fe=rfft+2; // evens const Num_t* fo=fe+1; // odds Num_t* fbin=rfft+1; // current freq bin for (uint32_t i=2; i<n; i+=2, fe+=2, fo+=2, ++fbin) { *fbin = ( ((*fe)*(*fe)) + ((*fo)*(*fo)) ); if (doSqrt) { *fbin = std::sqrt(static_cast<float>(*fbin)); } } // set the last freq bin *fbin = hibin; return (fbin-rfft+1); } return 0; } }; #endif // SN_SnSigProcUtils