Jordan Hanson / Mbed 2 deprecated AutonomousDAQ

Arianna autonomous DAQ firmware

Dependencies:   mbed SDFileSystemFilinfo AriSnProtocol NetServicesMin AriSnComm MODSERIAL PowerControlClkPatch DS1820OW

SnSigProcUtils.h@110:d1da040a0cf2, 2015-11-24 (annotated)

Committer:
uci1
Date:
Tue Nov 24 21:52:27 2015 +0000
Revision:
110:d1da040a0cf2
Parent:
84:80b15993944e 
Stn32 (slow) with conf name. SD stall fix. No interface chip. Safety nets.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
uci1 84:80b15993944e 1 #ifndef SN_SnSigProcUtils
uci1 84:80b15993944e 2 #define SN_SnSigProcUtils
uci1 84:80b15993944e 3
uci1 84:80b15993944e 4 #include <cmath>
uci1 84:80b15993944e 5 #include <stdint.h>
uci1 84:80b15993944e 6
uci1 84:80b15993944e 7 #include "SnConstants.h"
uci1 84:80b15993944e 8 #include "SnSigProcDataTable.h"
uci1 84:80b15993944e 9
uci1 84:80b15993944e 10 struct SnSigProcUtils {
uci1 84:80b15993944e 11
uci1 84:80b15993944e 12 static const double kPi;
uci1 84:80b15993944e 13 static const double kTwoPi;
uci1 84:80b15993944e 14
uci1 84:80b15993944e 15
uci1 84:80b15993944e 16 template<typename Num_t, typename Res_t>
uci1 84:80b15993944e 17 static
uci1 84:80b15993944e 18 void GetArraySumAndMaxSingleLoop(const Num_t* const data, const uint32_t n,
uci1 84:80b15993944e 19 Res_t& sum, Res_t& max) {
uci1 84:80b15993944e 20 const Num_t* d = data;
uci1 84:80b15993944e 21 max = static_cast<Res_t>(*d);
uci1 84:80b15993944e 22 sum = static_cast<Res_t>(*d);
uci1 84:80b15993944e 23 ++d; // skip past element 0
uci1 84:80b15993944e 24 for (uint32_t i=1; i<n; ++i, ++d) {
uci1 84:80b15993944e 25 sum += static_cast<Res_t>(*d);
uci1 84:80b15993944e 26 if (*d > max) {
uci1 84:80b15993944e 27 max = static_cast<Res_t>(*d);
uci1 84:80b15993944e 28 }
uci1 84:80b15993944e 29 }
uci1 84:80b15993944e 30 }
uci1 84:80b15993944e 31
uci1 84:80b15993944e 32 template<typename Dst_t, typename Src_t>
uci1 84:80b15993944e 33 static
uci1 84:80b15993944e 34 void MemCpyOrLoopCpy(Dst_t* const dst, const Src_t* const src,
uci1 84:80b15993944e 35 const uint32_t n) {
uci1 84:80b15993944e 36 #ifdef DEBUG_
uci1 84:80b15993944e 37 printf("MemCpyOrLoopCpy : loop\r\n");
uci1 84:80b15993944e 38 #endif
uci1 84:80b15993944e 39 Dst_t* d = dst;
uci1 84:80b15993944e 40 const Src_t* s = src;
uci1 84:80b15993944e 41 for (uint32_t i=0; i<n; ++i, ++d, ++s) {
uci1 84:80b15993944e 42 *d = static_cast<Dst_t>(*s);
uci1 84:80b15993944e 43 }
uci1 84:80b15993944e 44 }
uci1 84:80b15993944e 45
uci1 84:80b15993944e 46
uci1 84:80b15993944e 47 template<typename Same_t>
uci1 84:80b15993944e 48 static
uci1 84:80b15993944e 49 void MemCpyOrLoopCpy(Same_t* const dst, const Same_t* const src,
uci1 84:80b15993944e 50 const uint32_t n) {
uci1 84:80b15993944e 51 // can only memcpy if the types are the same
uci1 84:80b15993944e 52 #ifdef DEBUG_
uci1 84:80b15993944e 53 printf("MemCpyOrLoopCpy : memcpy\r\n");
uci1 84:80b15993944e 54 #endif
uci1 84:80b15993944e 55 memcpy(dst, src, sizeof(Same_t)*n);
uci1 84:80b15993944e 56 }
uci1 84:80b15993944e 57
uci1 84:80b15993944e 58 // the number of samples is a template parameter rather than
uci1 84:80b15993944e 59 // an input parameter so that we can be sure to use the proper
uci1 84:80b15993944e 60 // lookup tables, and that we should be somewhat safe against
uci1 84:80b15993944e 61 // using the wrong tables if the number of samples changes in the
uci1 84:80b15993944e 62 // future, as the code should no longer compile.
uci1 84:80b15993944e 63 template<uint32_t nn, typename Num_t>
uci1 84:80b15993944e 64 static
uci1 84:80b15993944e 65 bool DiscreteCpxFFT(Num_t* const data,
uci1 84:80b15993944e 66 const bool inverse) {
uci1 84:80b15993944e 67 // replace the data in the 'data' array by its discrete, complex-valued FFT
uci1 84:80b15993944e 68 //
uci1 84:80b15993944e 69 // if inverse is true, will do the inverse FFT
uci1 84:80b15993944e 70 // (unlike the function provided in NumRecC, the inverse FFT will
uci1 84:80b15993944e 71 // be provided already be normalized)
uci1 84:80b15993944e 72 // so calling DiscreteCpxFFT inverse on the result of
uci1 84:80b15993944e 73 // DiscreteCpxFFT forward will yield the original data (within
uci1 84:80b15993944e 74 // numerical accuracy)
uci1 84:80b15993944e 75 //
uci1 84:80b15993944e 76 // See RealFFT for notes on normalization. However, unlike RealFFT,
uci1 84:80b15993944e 77 // the factors of 2 and special bin treatments are not necessary
uci1 84:80b15993944e 78 // for the complex FFT, since both positive and negative
uci1 84:80b15993944e 79 // frequencies are stored in their own bins.
uci1 84:80b15993944e 80 //
uci1 84:80b15993944e 81 // NOTE: "data" must be of length 2*nn!! since the calculation is done
uci1 84:80b15993944e 82 // in place. the time domain data should be in the first half of "data".
uci1 84:80b15993944e 83 // nn must be a power of two
uci1 84:80b15993944e 84 // the fft is organized like: [ real_0, img_0, real_1, img_1, ... ]
uci1 84:80b15993944e 85 //
uci1 84:80b15993944e 86 // NUM REC C - 12.2, pg 507-508
uci1 84:80b15993944e 87 //
uci1 84:80b15993944e 88 // return true iff FFT succeeded
uci1 84:80b15993944e 89
uci1 84:80b15993944e 90 if ( ((nn-1)&nn)!=0 ) {
uci1 84:80b15993944e 91 #ifdef DEBUG
uci1 84:80b15993944e 92 printf("<DiscreteCpxFFT>: nn (=%u) must be a power of 2.",nn);
uci1 84:80b15993944e 93 return false;
uci1 84:80b15993944e 94 #endif
uci1 84:80b15993944e 95 } else {
uci1 84:80b15993944e 96
uci1 84:80b15993944e 97 static const uint32_t n = nn<<1; // nn * 2
uci1 84:80b15993944e 98 Num_t tempr,tempi;
uci1 84:80b15993944e 99 uint32_t m;
uci1 84:80b15993944e 100 uint32_t i,j=1;
uci1 84:80b15993944e 101 Num_t* di=data, * di1=data+1, * dj, * dj1;
uci1 84:80b15993944e 102 #ifdef USE_DFFT_LUTS
uci1 84:80b15993944e 103 const typename SnSigProcDataTable<n>::DfftDivideIndicies_t* kdi =
uci1 84:80b15993944e 104 SnSigProcDataTable<n>::kDfftDivideIndicies;
uci1 84:80b15993944e 105 #endif
uci1 84:80b15993944e 106 for (i=1; i<n; i+=2, di+=2, di1+=2) {
uci1 84:80b15993944e 107 if (j > i) {
uci1 84:80b15993944e 108 dj = data + j - 1;
uci1 84:80b15993944e 109 dj1 = dj + 1;
uci1 84:80b15993944e 110 tempr = *dj; *dj = *di; *di = tempr;
uci1 84:80b15993944e 111 tempr = *dj1; *dj1 = *di1; *di1 = tempr;
uci1 84:80b15993944e 112 }
uci1 84:80b15993944e 113 #ifdef USE_DFFT_LUTS
uci1 84:80b15993944e 114 j = *kdi++;
uci1 84:80b15993944e 115 m = *kdi++;
uci1 84:80b15993944e 116 #else
uci1 84:80b15993944e 117 m = nn;
uci1 84:80b15993944e 118 while (m>1 && j>m) {
uci1 84:80b15993944e 119 j -= m;
uci1 84:80b15993944e 120 m >>= 1;
uci1 84:80b15993944e 121 }
uci1 84:80b15993944e 122 j += m;
uci1 84:80b15993944e 123 #endif
uci1 84:80b15993944e 124 }
uci1 84:80b15993944e 125
uci1 84:80b15993944e 126 #ifdef USE_DFFT_LUTS
uci1 84:80b15993944e 127 double wr, wi;
uci1 84:80b15993944e 128 const typename SnSigProcDataTable<n>::DfftTwidleFactor_t* ktf =
uci1 84:80b15993944e 129 SnSigProcDataTable<n>::kDfftTwidleFactors;
uci1 84:80b15993944e 130 #else
uci1 84:80b15993944e 131 double wtemp, wr, wpr, wpi, wi, theta;
uci1 84:80b15993944e 132 const int8_t isign = inverse ? -1 : 1;
uci1 84:80b15993944e 133 #endif
uci1 84:80b15993944e 134 uint32_t mmax=2, istep;
uci1 84:80b15993944e 135 while (n > mmax) {
uci1 84:80b15993944e 136 istep = mmax<<1;
uci1 84:80b15993944e 137 #ifndef USE_DFFT_LUTS // if not defined!
uci1 84:80b15993944e 138 theta = isign * (kTwoPi / mmax);
uci1 84:80b15993944e 139 wtemp = std::sin(0.5*theta);
uci1 84:80b15993944e 140 wpr = -2.0 * wtemp * wtemp;
uci1 84:80b15993944e 141 wpi = std::sin(theta);
uci1 84:80b15993944e 142 wr = 1.0;
uci1 84:80b15993944e 143 wi = 0.0;
uci1 84:80b15993944e 144 #endif
uci1 84:80b15993944e 145 for (m=1; m<mmax; m+=2) {
uci1 84:80b15993944e 146 #ifdef USE_DFFT_LUTS
uci1 84:80b15993944e 147 wr = *ktf++;
uci1 84:80b15993944e 148 wi = *ktf++;
uci1 84:80b15993944e 149 if (inverse) {
uci1 84:80b15993944e 150 wi = -wi;
uci1 84:80b15993944e 151 }
uci1 84:80b15993944e 152 #endif
uci1 84:80b15993944e 153 for (i=m; i<=n; i+=istep) {
uci1 84:80b15993944e 154 j = i + mmax;
uci1 84:80b15993944e 155 dj = data + j - 1;
uci1 84:80b15993944e 156 dj1 = dj+1;
uci1 84:80b15993944e 157 di = data + i - 1;
uci1 84:80b15993944e 158 di1 = di+1;
uci1 84:80b15993944e 159 tempr = wr * (*dj) - wi*(*dj1);
uci1 84:80b15993944e 160 tempi = wr * (*dj1) + wi*(*dj);
uci1 84:80b15993944e 161 *dj = *di - tempr;
uci1 84:80b15993944e 162 *dj1 = *di1 - tempi;
uci1 84:80b15993944e 163 *di += tempr;
uci1 84:80b15993944e 164 *di1 += tempi;
uci1 84:80b15993944e 165 }
uci1 84:80b15993944e 166 #ifndef USE_DFFT_LUTS // if not defined!
uci1 84:80b15993944e 167 wtemp = wr;
uci1 84:80b15993944e 168 wr = wtemp*wpr - wi*wpi + wr;
uci1 84:80b15993944e 169 wi = wi*wpr + wtemp*wpi + wi;
uci1 84:80b15993944e 170 #endif
uci1 84:80b15993944e 171 }
uci1 84:80b15993944e 172 mmax = istep;
uci1 84:80b15993944e 173 }
uci1 84:80b15993944e 174 if (inverse) {
uci1 84:80b15993944e 175 // scale amplitude back
uci1 84:80b15993944e 176 di = data;
uci1 84:80b15993944e 177 static const Num_t tn = 2.0/static_cast<Num_t>(n);
uci1 84:80b15993944e 178 for (i=0; i<n; ++i, ++di) {
uci1 84:80b15993944e 179 *di *= tn;
uci1 84:80b15993944e 180 }
uci1 84:80b15993944e 181 }
uci1 84:80b15993944e 182 }
uci1 84:80b15993944e 183
uci1 84:80b15993944e 184 return true;
uci1 84:80b15993944e 185 }
uci1 84:80b15993944e 186
uci1 84:80b15993944e 187 // the number of samples is a template parameter rather than
uci1 84:80b15993944e 188 // an input parameter so that we can be sure to use the proper
uci1 84:80b15993944e 189 // lookup tables, and that we should be somewhat safe against
uci1 84:80b15993944e 190 // using the wrong tables if the number of samples changes in the
uci1 84:80b15993944e 191 // future, as the code should no longer compile.
uci1 84:80b15993944e 192 template<uint32_t n, typename Num_t>
uci1 84:80b15993944e 193 static
uci1 84:80b15993944e 194 bool RealFFT(Num_t* const data,
uci1 84:80b15993944e 195 const bool inverse) {
uci1 84:80b15993944e 196 // replace the real-valued data with the positive frequency half of its
uci1 84:80b15993944e 197 // complex fourier transform
uci1 84:80b15993944e 198 //
uci1 84:80b15993944e 199 // the real valued first & last components of the complex transform
uci1 84:80b15993944e 200 // are stored in data[0] and data[1], respectively
uci1 84:80b15993944e 201 //
uci1 84:80b15993944e 202 // n must be a power of 2
uci1 84:80b15993944e 203 //
uci1 84:80b15993944e 204 // data goes from [0..n-1]
uci1 84:80b15993944e 205 //
uci1 84:80b15993944e 206 // Note on the normalization convention used:
uci1 84:80b15993944e 207 // The 2/n normalization is applied only on the inverse FFT.
uci1 84:80b15993944e 208 // * To normalize the *amplitude* of frequencies in the forward
uci1 84:80b15993944e 209 // FFT, multiply by 2/n in bins [2..n-1] (which store two both
uci1 84:80b15993944e 210 // the pos and neg frequencies) and 1/n in bins [0..1]. For
uci1 84:80b15993944e 211 // example, if you have in the time domain (200*sine_50MHz +
uci1 84:80b15993944e 212 // 300*sine_230MHz) and you want to see a spike at 50MHz with
uci1 84:80b15993944e 213 // amplitude 200 and a spike at 230MHz with amplitude 300, you
uci1 84:80b15993944e 214 // should multiply bins 0 and 1 by 1/n and the other bins by 2/n.
uci1 84:80b15993944e 215 // * To normalize the *total power* of the forward FFT, multiply
uci1 84:80b15993944e 216 // by 2/sqrt(n) for bins [2..n-1] (which store two both
uci1 84:80b15993944e 217 // the pos and neg frequencies) and 1/sqrt(n) in bins [0..1].
uci1 84:80b15993944e 218 // This is the normalization chosen by NewGraphFromRealFFT when its
uci1 84:80b15993944e 219 // "normalize" parameter is true.
uci1 84:80b15993944e 220 //
uci1 84:80b15993944e 221 // if inverse is true, will perform the inverse FFT
uci1 84:80b15993944e 222 // (unlike the function provided in NumRecC, the inverse FFT will
uci1 84:80b15993944e 223 // be provided already be mulitplied by 2/n) -- so taking the inverse
uci1 84:80b15993944e 224 // FFT of the result of the forward FFT will yield directly the original
uci1 84:80b15993944e 225 // data (within numerical accuracy).
uci1 84:80b15993944e 226 //
uci1 84:80b15993944e 227 // NUM REC C - 12.3, pg 513-514
uci1 84:80b15993944e 228 //
uci1 84:80b15993944e 229 // return true iff the FFT succeeded
uci1 84:80b15993944e 230
uci1 84:80b15993944e 231 if ( ((n-1)&n)!=0 ) {
uci1 84:80b15993944e 232 #ifdef DEBUG
uci1 84:80b15993944e 233 printf("<RealFFT>: n (=%u) must be a power of 2.",n);
uci1 84:80b15993944e 234 #endif
uci1 84:80b15993944e 235 return false;
uci1 84:80b15993944e 236 } else {
uci1 84:80b15993944e 237
uci1 84:80b15993944e 238 double theta = kPi / static_cast<double>(n>>1);
uci1 84:80b15993944e 239 float c1=0.5, c2;
uci1 84:80b15993944e 240 if (inverse) {
uci1 84:80b15993944e 241 c2 = 0.5;
uci1 84:80b15993944e 242 theta = -theta; // set up for inverse tranform
uci1 84:80b15993944e 243 } else {
uci1 84:80b15993944e 244 c2 = -0.5;
uci1 84:80b15993944e 245 DiscreteCpxFFT<(n/2)>(data,false); // forward FFT
uci1 84:80b15993944e 246 }
uci1 84:80b15993944e 247
uci1 84:80b15993944e 248 #ifdef USE_DFFT_LUTS
uci1 84:80b15993944e 249 typename SnSigProcDataTable<n>::RealDfftTwidleFactor_t
uci1 84:80b15993944e 250 wr, wi;
uci1 84:80b15993944e 251 const typename SnSigProcDataTable<n>::RealDfftTwidleFactor_t
uci1 84:80b15993944e 252 * krtf = SnSigProcDataTable<n>::kRealDfftTwidleFactor;
uci1 84:80b15993944e 253 #else
uci1 84:80b15993944e 254 double wtemp = std::sin(0.5*theta);
uci1 84:80b15993944e 255 const double wpr = -2.0*wtemp*wtemp;
uci1 84:80b15993944e 256 const double wpi = std::sin(theta);
uci1 84:80b15993944e 257 double wr = 1.0+wpr;
uci1 84:80b15993944e 258 double wi = wpi;
uci1 84:80b15993944e 259 #endif
uci1 84:80b15993944e 260 float h1r, h1i, h2r, h2i;
uci1 84:80b15993944e 261 const uint32_t n4 = n>>2; // n/4
uci1 84:80b15993944e 262 uint32_t i;
uci1 84:80b15993944e 263 Num_t* d1 = data+2, * d2 = d1+1,
uci1 84:80b15993944e 264 * d3 = data+n-2, * d4 = d3+1;
uci1 84:80b15993944e 265 for (i=1; i<n4; ++i, d1+=2, d2+=2, d3-=2, d4-=2) {
uci1 84:80b15993944e 266 #ifdef USE_DFFT_LUTS
uci1 84:80b15993944e 267 wr = *krtf++;
uci1 84:80b15993944e 268 wi = *krtf++;
uci1 84:80b15993944e 269 if (inverse) {
uci1 84:80b15993944e 270 wi = -wi;
uci1 84:80b15993944e 271 }
uci1 84:80b15993944e 272 #endif
uci1 84:80b15993944e 273 h1r = c1 * ((*d1)+(*d3));
uci1 84:80b15993944e 274 h1i = c1 * ((*d2)-(*d4));
uci1 84:80b15993944e 275 h2r = -c2 * ((*d2)+(*d4));
uci1 84:80b15993944e 276 h2i = c2 * ((*d1)-(*d3));
uci1 84:80b15993944e 277 *d1 = h1r + wr*h2r - wi*h2i;
uci1 84:80b15993944e 278 *d2 = h1i + wr*h2i + wi*h2r;
uci1 84:80b15993944e 279 *d3 = h1r - wr*h2r + wi*h2i;
uci1 84:80b15993944e 280 *d4 = -h1i + wr*h2i + wi*h2r;
uci1 84:80b15993944e 281 #ifndef USE_DFFT_LUTS // if not defined!
uci1 84:80b15993944e 282 wtemp = wr;
uci1 84:80b15993944e 283 wr = wtemp*wpr - wi*wpi + wr;
uci1 84:80b15993944e 284 wi = wi*wpr + wtemp*wpi + wi;
uci1 84:80b15993944e 285 #endif
uci1 84:80b15993944e 286 }
uci1 84:80b15993944e 287
uci1 84:80b15993944e 288 // now fill in the beginning and end of the array
uci1 84:80b15993944e 289 if (inverse) {
uci1 84:80b15993944e 290 h1r = *data;
uci1 84:80b15993944e 291 d1 = data+1;
uci1 84:80b15993944e 292 *data = c1*(h1r + (*d1));
uci1 84:80b15993944e 293 *d1 = c1*(h1r - (*d1));
uci1 84:80b15993944e 294 DiscreteCpxFFT<(n/2)>(data,inverse); // inverse tranform
uci1 84:80b15993944e 295 } else {
uci1 84:80b15993944e 296 h1r = *data;
uci1 84:80b15993944e 297 d1 = data+1;
uci1 84:80b15993944e 298 *data = h1r + (*d1);
uci1 84:80b15993944e 299 *d1 = h1r - (*d1);
uci1 84:80b15993944e 300 }
uci1 84:80b15993944e 301 }
uci1 84:80b15993944e 302 return true;
uci1 84:80b15993944e 303 }
uci1 84:80b15993944e 304
uci1 84:80b15993944e 305 template<typename Num_t>
uci1 84:80b15993944e 306 static
uci1 84:80b15993944e 307 uint32_t ReplaceRfftWithSpectrum(Num_t* const rfft,
uci1 84:80b15993944e 308 const uint32_t n,
uci1 84:80b15993944e 309 const bool doSqrt) {
uci1 84:80b15993944e 310 // rfft should contain the result of RealFFT and n is the number
uci1 84:80b15993944e 311 // of samples.
uci1 84:80b15993944e 312 // this routine will replace the first half of the array
uci1 84:80b15993944e 313 // with the "spectrum" (frequency magnitude) at each freq bin.
uci1 84:80b15993944e 314 // valid freq bins are from [0, (n/2)+1]
uci1 84:80b15993944e 315 //
uci1 84:80b15993944e 316 // return the number of freq bins filled, or else 0 on error
uci1 84:80b15993944e 317
uci1 84:80b15993944e 318 if (rfft!=0) {
uci1 84:80b15993944e 319 //const uint32_t hn = n>>1; // n/2
uci1 84:80b15993944e 320 //uint32_t j=0;
uci1 84:80b15993944e 321 // set the first freq bin
uci1 84:80b15993944e 322 rfft[0] = (doSqrt) ? std::abs(rfft[0])
uci1 84:80b15993944e 323 : (rfft[0]*rfft[0]);
uci1 84:80b15993944e 324 // store the last freq bin
uci1 84:80b15993944e 325 const Num_t hibin =
uci1 84:80b15993944e 326 (doSqrt) ? std::abs(rfft[1])
uci1 84:80b15993944e 327 : (rfft[1]*rfft[1]);
uci1 84:80b15993944e 328 // loop over other freqs
uci1 84:80b15993944e 329 const Num_t* fe=rfft+2; // evens
uci1 84:80b15993944e 330 const Num_t* fo=fe+1; // odds
uci1 84:80b15993944e 331 Num_t* fbin=rfft+1; // current freq bin
uci1 84:80b15993944e 332 for (uint32_t i=2; i<n; i+=2, fe+=2, fo+=2, ++fbin) {
uci1 84:80b15993944e 333 *fbin = ( ((*fe)*(*fe)) + ((*fo)*(*fo)) );
uci1 84:80b15993944e 334 if (doSqrt) {
uci1 84:80b15993944e 335 *fbin = std::sqrt(static_cast<float>(*fbin));
uci1 84:80b15993944e 336 }
uci1 84:80b15993944e 337 }
uci1 84:80b15993944e 338 // set the last freq bin
uci1 84:80b15993944e 339 *fbin = hibin;
uci1 84:80b15993944e 340
uci1 84:80b15993944e 341 return (fbin-rfft+1);
uci1 84:80b15993944e 342 }
uci1 84:80b15993944e 343 return 0;
uci1 84:80b15993944e 344 }
uci1 84:80b15993944e 345
uci1 84:80b15993944e 346 };
uci1 84:80b15993944e 347
uci1 84:80b15993944e 348 #endif // SN_SnSigProcUtils