Chip Audette
8 years ago
parent
77a702943b
commit
d9f58c1999
@ -0,0 +1,274 @@ |
|||||||
|
/*
|
||||||
|
* fir_filterbank.h |
||||||
|
*
|
||||||
|
* Created: Chip Audette, Creare LLC, Feb 2017 |
||||||
|
* Primarly built upon CHAPRO "Generic Hearing Aid" from
|
||||||
|
* Boys Town National Research Hospital (BTNRH): https://github.com/BTNRH/chapro
|
||||||
|
*
|
||||||
|
* License: MIT License. Use at your own risk. |
||||||
|
*
|
||||||
|
*/ |
||||||
|
|
||||||
|
#ifndef AudioConfigFIRFilterBank_F32_h |
||||||
|
#define AudioConfigFIRFilterBank_F32_h |
||||||
|
|
||||||
|
#include "utility/rfft.c" |
||||||
|
|
||||||
|
#define fmove(x,y,n) memmove(x,y,(n)*sizeof(float)) |
||||||
|
#define fcopy(x,y,n) memcpy(x,y,(n)*sizeof(float)) |
||||||
|
#define fzero(x,n) memset(x,0,(n)*sizeof(float)) |
||||||
|
|
||||||
|
class AudioConfigFIRFilterBank_F32 { |
||||||
|
//GUI: inputs:0, outputs:0 //this line used for automatic generation of GUI node
|
||||||
|
//GUI: shortName:config_FIRbank
|
||||||
|
public: |
||||||
|
AudioConfigFIRFilterBank_F32(void) { |
||||||
|
} |
||||||
|
AudioConfigFIRFilterBank_F32(const int n_chan, const int n_fir, const float sample_rate_Hz, float *corner_freq, float *filter_coeff) { |
||||||
|
createFilterCoeff(n_chan, n_fir, sample_rate_Hz, corner_freq, filter_coeff); |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
//createFilterCoeff:
|
||||||
|
// Purpose: create all of the FIR filter coefficients for the FIR filterbank
|
||||||
|
// Syntax: createFilterCoeff(n_chan, n_fir, sample_rate_Hz, corner_freq, filter_coeff)
|
||||||
|
// int n_chan (input): number of channels (number of filters) you desire. Must be 2 or greater
|
||||||
|
// int n_fir (input): length of each FIR filter (should probably be 8 or greater)
|
||||||
|
// float sample_rate_Hz (input): sample rate of your system (used to scale the corner_freq values)
|
||||||
|
// float *corner_freq (input): array of frequencies (Hz) seperating each band in your filter bank.
|
||||||
|
// should contain n_chan-1 values because it should exclude the bottom (0 Hz) and the top
|
||||||
|
// (Nyquist) as those values are already assumed by this routine. An valid example is below:
|
||||||
|
// int n_chan = 8; float cf[] = {317.1666, 502.9734, 797.6319, 1264.9, 2005.9, 3181.1, 5044.7};
|
||||||
|
// float *filter_coeff (output): array of FIR filter coefficients that are computed by this
|
||||||
|
// routine. You must have pre-allocated the array such as: float filter_coeff[N_CHAN][N_FIR];
|
||||||
|
//Optional Usage: if you want 8 default filters spaced logarithmically, use: float *corner_freq = NULL
|
||||||
|
void createFilterCoeff(const int n_chan, const int n_fir, const float sample_rate_Hz, float *corner_freq, float *filter_coeff) { |
||||||
|
float *cf = corner_freq; |
||||||
|
int flag__free_cf = 0; |
||||||
|
if (cf == NULL) { |
||||||
|
//compute corner frequencies that are logarithmically spaced
|
||||||
|
cf = (float *) calloc(n_chan, sizeof(float)); |
||||||
|
flag__free_cf = 1; |
||||||
|
computeLogSpacedCornerFreqs(n_chan, sample_rate_Hz, cf); |
||||||
|
} |
||||||
|
const int window_type = 0; //0 = Hamming
|
||||||
|
fir_filterbank(filter_coeff, cf, n_chan, n_fir, window_type, sample_rate_Hz); |
||||||
|
if (flag__free_cf) free(cf);
|
||||||
|
} |
||||||
|
|
||||||
|
//compute frequencies that space zero to nyquist. Leave zero off, because it is assumed to exist in the later code.
|
||||||
|
//example of an *8* channel set of frequencies: cf = {317.1666, 502.9734, 797.6319, 1264.9, 2005.9, 3181.1, 5044.7}
|
||||||
|
void computeLogSpacedCornerFreqs(const int n_chan, const float sample_rate_Hz, float *cf) { |
||||||
|
float cf_8_band[] = {317.1666, 502.9734, 797.6319, 1264.9, 2005.9, 3181.1, 5044.7}; |
||||||
|
float scale_fac = expf(logf(cf_8_band[6]/cf_8_band[0]) / ((float)(n_chan-2))); |
||||||
|
//Serial.print("MakeFIRFilterBank: computeEvenlySpacedCornerFreqs: scale_fac = "); Serial.println(scale_fac);
|
||||||
|
cf[0] = cf_8_band[0]; |
||||||
|
//Serial.println("MakeFIRFilterBank: computeEvenlySpacedCornerFreqs: cf = ");Serial.print(cf[0]); Serial.print(", ");
|
||||||
|
for (int i=1; i < n_chan-1; i++) { |
||||||
|
cf[i] = cf[i-1]*scale_fac; |
||||||
|
//Serial.print(cf[i]); Serial.print(", ");
|
||||||
|
} |
||||||
|
//Serial.println();
|
||||||
|
} |
||||||
|
private: |
||||||
|
|
||||||
|
int nextPowerOfTwo(int n) { |
||||||
|
const int n_out_vals = 8; |
||||||
|
int out_vals[n_out_vals] = {8, 16, 32, 64, 128, 256, 512, 1024}; |
||||||
|
if (n < out_vals[0]) return out_vals[0]; |
||||||
|
for (int i=1;i<n_out_vals; i++) { |
||||||
|
if ((n > out_vals[i-1]) & (n <= out_vals[i])) { |
||||||
|
return out_vals[i]; |
||||||
|
} |
||||||
|
} |
||||||
|
return n; |
||||||
|
} |
||||||
|
|
||||||
|
void fir_filterbank(float *bb, float *cf, const int nc, const int nw_orig, const int wt, const float sr) |
||||||
|
{ |
||||||
|
double p, w, a = 0.16, sm = 0; |
||||||
|
float *ww, *bk, *xx, *yy; |
||||||
|
int j, k, kk, nt, nf, ns, *be; |
||||||
|
|
||||||
|
int nw = nextPowerOfTwo(nw_orig); |
||||||
|
Serial.print("fir_filterbank: nw_orig = "); Serial.print(nw_orig); |
||||||
|
Serial.print(", nw = "); Serial.println(nw); |
||||||
|
|
||||||
|
nt = nw * 2; |
||||||
|
nf = nw + 1; |
||||||
|
ns = nf * 2; |
||||||
|
be = (int *) calloc(nc + 1, sizeof(int)); |
||||||
|
ww = (float *) calloc(nw, sizeof(float)); |
||||||
|
xx = (float *) calloc(ns, sizeof(float)); |
||||||
|
yy = (float *) calloc(ns, sizeof(float)); |
||||||
|
|
||||||
|
// window
|
||||||
|
for (j = 0; j < nw; j++) ww[j]=0.0f; //clear
|
||||||
|
for (j = 0; j < nw_orig; j++) { |
||||||
|
p = M_PI * (2.0 * j - nw_orig) / nw_orig; |
||||||
|
if (wt == 0) { |
||||||
|
w = 0.54 + 0.46 * cos(p); // Hamming
|
||||||
|
} else { |
||||||
|
w = (1 - a + cos(p) + a * cos(2 * p)) / 2; // Blackman
|
||||||
|
} |
||||||
|
sm += w; |
||||||
|
ww[j] = (float) w; |
||||||
|
} |
||||||
|
|
||||||
|
// frequency bands...add the DC-facing band and add the Nyquist-facing band
|
||||||
|
be[0] = 0; |
||||||
|
for (k = 1; k < nc; k++) { |
||||||
|
kk = round(nf * cf[k - 1] * (2 / sr)); |
||||||
|
be[k] = (kk > nf) ? nf : kk; |
||||||
|
} |
||||||
|
be[nc] = nf; |
||||||
|
|
||||||
|
// channel tranfer functions
|
||||||
|
fzero(xx, ns); |
||||||
|
xx[nw_orig / 2] = 1; //make a single-sample impulse centered on our eventual window
|
||||||
|
cha_fft_rc(xx, nt); |
||||||
|
for (k = 0; k < nc; k++) { |
||||||
|
fzero(yy, ns); //zero the temporary output
|
||||||
|
//int nbins = (be[k + 1] - be[k]) * 2; Serial.print("fir_filterbank: chan ");Serial.print(k); Serial.print(", nbins = ");Serial.println(nbins);
|
||||||
|
fcopy(yy + be[k] * 2, xx + be[k] * 2, (be[k + 1] - be[k]) * 2); //copy just our passband
|
||||||
|
cha_fft_cr(yy, nt); //IFFT back into the time domain
|
||||||
|
|
||||||
|
// apply window to iFFT of bandpass
|
||||||
|
for (j = 0; j < nw; j++) { |
||||||
|
yy[j] *= ww[j]; |
||||||
|
} |
||||||
|
|
||||||
|
bk = bb + k * nw_orig; //pointer to location in output array
|
||||||
|
fcopy(bk, yy, nw_orig); //copy the filter coefficients to the output array
|
||||||
|
|
||||||
|
//print out the coefficients
|
||||||
|
//for (int i=0; i<nw; i++) { Serial.print(yy[i]*1000.0f);Serial.print(" "); }; Serial.println();
|
||||||
|
} |
||||||
|
free(be); |
||||||
|
free(ww); |
||||||
|
free(xx); |
||||||
|
free(yy); |
||||||
|
} |
||||||
|
}; |
||||||
|
#endif |
||||||
|
|
||||||
|
// static CHA_DSL dsl = {5, 50, 119, 0, 8,
|
||||||
|
// {317.1666,502.9734,797.6319,1264.9,2005.9,3181.1,5044.7}, //log spaced frequencies.
|
||||||
|
// {-13.5942,-16.5909,-3.7978,6.6176,11.3050,23.7183,35.8586,37.3885},
|
||||||
|
// {0.7,0.9,1,1.1,1.2,1.4,1.6,1.7},
|
||||||
|
// {32.2,26.5,26.7,26.7,29.8,33.6,34.3,32.7},
|
||||||
|
// {78.7667,88.2,90.7,92.8333,98.2,103.3,101.9,99.8}
|
||||||
|
// };
|
||||||
|
|
||||||
|
// //x is the input waveform
|
||||||
|
// //y is the processed waveform
|
||||||
|
// //n is the length of the waveform
|
||||||
|
// //fs is the sample rate...24000 Hz
|
||||||
|
// //dsl are the settings for each band
|
||||||
|
// t1 = amplify(x, y, n, fs, &dsl);
|
||||||
|
|
||||||
|
//amplify(float *x, float *y, int n, double fs, CHA_DSL *dsl)
|
||||||
|
//{
|
||||||
|
// int nc;
|
||||||
|
// static int nw = 256; // window size
|
||||||
|
// static int cs = 32; // chunk size
|
||||||
|
// static int wt = 0; // window type: 0=Hamming, 1=Blackman
|
||||||
|
// static void *cp[NPTR] = {0};
|
||||||
|
// static CHA_WDRC gha = {1, 50, 24000, 119, 0, 105, 10, 105};
|
||||||
|
//
|
||||||
|
// nc = dsl->nchannel; //8?
|
||||||
|
// cha_firfb_prepare(cp, dsl->cross_freq, nc, fs, nw, wt, cs);
|
||||||
|
// cha_agc_prepare(cp, dsl, &gha);
|
||||||
|
// sp_tic();
|
||||||
|
// WDRC(cp, x, y, n, nc);
|
||||||
|
// return (sp_toc());
|
||||||
|
//}
|
||||||
|
|
||||||
|
//FUNC(int)
|
||||||
|
//cha_firfb_prepare(CHA_PTR cp, double *cf, int nc, double fs,
|
||||||
|
// int nw, int wt, int cs)
|
||||||
|
//{
|
||||||
|
// float *bb;
|
||||||
|
// int ns, nt;
|
||||||
|
//
|
||||||
|
// if (cs <= 0) {
|
||||||
|
// return (1);
|
||||||
|
// }
|
||||||
|
// cha_prepare(cp);
|
||||||
|
// CHA_IVAR[_cs] = cs; //cs = 32
|
||||||
|
// CHA_DVAR[_fs] = fs; //fs = 24000
|
||||||
|
// // allocate window buffers
|
||||||
|
// CHA_IVAR[_nw] = nw; //nw = 256
|
||||||
|
// CHA_IVAR[_nc] = nc; //nc = 32
|
||||||
|
// nt = nw * 2; //nt = 256*2 = 512
|
||||||
|
// ns = nt + 2; //ns = 512+2 = 514
|
||||||
|
// cha_allocate(cp, ns, sizeof(float), _ffxx); //allocate for input
|
||||||
|
// cha_allocate(cp, ns, sizeof(float), _ffyy); //allocate for output
|
||||||
|
// cha_allocate(cp, nc * (nw + cs), sizeof(float), _ffzz); //allocate per channel
|
||||||
|
// // compute FIR-filterbank coefficients
|
||||||
|
// bb = calloc(nc * nw, sizeof(float)); //allocate for filter coeff (256 long, 8 channels)
|
||||||
|
// fir_filterbank(bb, cf, nc, nw, wt, fs); //make the fir filter bank
|
||||||
|
// // Fourier-transform FIR coefficients
|
||||||
|
// if (cs < nw) { // short chunk
|
||||||
|
// fir_transform_sc(cp, bb, nc, nw, cs);
|
||||||
|
// } else { // long chunk
|
||||||
|
// fir_transform_lc(cp, bb, nc, nw, cs);
|
||||||
|
// }
|
||||||
|
// free(bb);
|
||||||
|
//
|
||||||
|
// return (0);
|
||||||
|
//}
|
||||||
|
|
||||||
|
// fir_filterbank( float *bb, double *cf, int nc, int nw, int wt, double sr)
|
||||||
|
// filter coeff, corner freqs, 8, 256, 0, 24000)
|
||||||
|
//{
|
||||||
|
// double p, w, a = 0.16, sm = 0;
|
||||||
|
// float *ww, *bk, *xx, *yy;
|
||||||
|
// int j, k, kk, nt, nf, ns, *be;
|
||||||
|
//
|
||||||
|
// nt = nw * 2; //nt = 256*2 = 512
|
||||||
|
// nf = nw + 1; //nyquist frequency bin is 256+1 = 257
|
||||||
|
// ns = nf * 2; //when complex, number values to carry is nyquist * 2 = 514
|
||||||
|
// be = (int *) calloc(nc + 1, sizeof(int));
|
||||||
|
// ww = (float *) calloc(nw, sizeof(float)); //window is 256 long
|
||||||
|
// xx = (float *) calloc(ns, sizeof(float)); //input data is 514 points long
|
||||||
|
// yy = (float *) calloc(ns, sizeof(float)); //output data is 514 points long
|
||||||
|
// // window
|
||||||
|
// for (j = 0; j < nw; j++) { //nw = 256
|
||||||
|
// p = M_PI * (2.0 * j - nw) / nw; //phase for computing window, radians
|
||||||
|
// if (wt == 0) { //wt is zero
|
||||||
|
// w = 0.54 + 0.46 * cos(p); // Hamming
|
||||||
|
// } else {
|
||||||
|
// w = (1 - a + cos(p) + a * cos(2 * p)) / 2; // Blackman
|
||||||
|
// }
|
||||||
|
// sm += w; //sum the window value. Doesn't appear to be used anywhere
|
||||||
|
// ww[j] = (float) w; //save the windowing coefficient...there are 256 of them
|
||||||
|
// }
|
||||||
|
// // frequency bands
|
||||||
|
// be[0] = 0; //first channel is DC bin
|
||||||
|
// for (k = 1; k < nc; k++) { //loop over the rest of the 8 channels
|
||||||
|
// kk = round(nf * cf[k - 1] * (2 / sr)); //get bin of the channel (upper?) corner frequency...assumes factor of two zero-padding?
|
||||||
|
// be[k] = (kk > nf) ? nf : kk; //make sure we don't go above the nyquist bin (bin 257, assuming a 512 FFT)
|
||||||
|
// }
|
||||||
|
// be[nc] = nf; //the last one is the nyquist freuquency
|
||||||
|
// // channel tranfer functions
|
||||||
|
// fzero(xx, ns); //zero the xx vector
|
||||||
|
// xx[nw / 2] = 1; //create an impulse in the middle of the (non-overlapped part of the) time-domain...sample 129
|
||||||
|
// cha_fft_rc(xx, nt); //convert to frequency domain..512 points long
|
||||||
|
// for (k = 0; k < nc; k++) { //loop over each channel
|
||||||
|
// bk = bb + k * nw; //bin index for this channel
|
||||||
|
// fzero(yy, ns); //zero out the output bins
|
||||||
|
// fcopy(yy + be[k] * 2, xx + be[k] * 2, (be[k + 1] - be[k]) * 2); //copy just the desired frequeny bins in our passband
|
||||||
|
// cha_fft_cr(yy, nt); //convert back to time domain
|
||||||
|
// // apply window to iFFT of bandpass
|
||||||
|
// for (j = 0; j < nw; j++) {
|
||||||
|
// yy[j] *= ww[j];
|
||||||
|
// }
|
||||||
|
// fcopy(bk, yy, nw); //copy output into the output filter...just the 256 points
|
||||||
|
// }
|
||||||
|
// free(be);
|
||||||
|
// free(ww);
|
||||||
|
// free(xx);
|
||||||
|
// free(yy);
|
||||||
|
//}
|
||||||
|
|
||||||
@ -0,0 +1,384 @@ |
|||||||
|
|
||||||
|
#include <math.h> |
||||||
|
//#include "chapro.h"
|
||||||
|
//#include "cha_ff.h"
|
||||||
|
|
||||||
|
/***********************************************************/ |
||||||
|
// FFT functions adapted from G. D. Bergland, "Subroutines FAST and FSST," (1979).
|
||||||
|
// In IEEE Acoustics, Speech, and Signal Processing Society.
|
||||||
|
// "Programs for Digital Signal Processing," IEEE Press, New York,
|
||||||
|
|
||||||
|
static __inline int |
||||||
|
ilog2(int n) |
||||||
|
{ |
||||||
|
int m; |
||||||
|
|
||||||
|
for (m = 1; m < 32; m++) |
||||||
|
if (n == (1 << m)) |
||||||
|
return (m); |
||||||
|
return (-1); |
||||||
|
} |
||||||
|
|
||||||
|
static __inline int |
||||||
|
bitrev(int ii, int m) |
||||||
|
{ |
||||||
|
register int jj; |
||||||
|
|
||||||
|
jj = ii & 1; |
||||||
|
--m; |
||||||
|
while (--m > 0) { |
||||||
|
ii >>= 1; |
||||||
|
jj <<= 1; |
||||||
|
jj |= ii & 1; |
||||||
|
} |
||||||
|
return (jj); |
||||||
|
} |
||||||
|
|
||||||
|
static __inline void |
||||||
|
rad2(int ii, float *x0, float *x1) |
||||||
|
{ |
||||||
|
int k; |
||||||
|
float t; |
||||||
|
|
||||||
|
for (k = 0; k < ii; k++) { |
||||||
|
t = x0[k] + x1[k]; |
||||||
|
x1[k] = x0[k] - x1[k]; |
||||||
|
x0[k] = t; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
static __inline void |
||||||
|
reorder1(int m, float *x) |
||||||
|
{ |
||||||
|
int j, k, kl, n; |
||||||
|
float t; |
||||||
|
|
||||||
|
k = 4; |
||||||
|
kl = 2; |
||||||
|
n = 1 << m; |
||||||
|
for (j = 4; j <= n; j += 2) { |
||||||
|
if (k > j) { |
||||||
|
t = x[j - 1]; |
||||||
|
x[j - 1] = x[k - 1]; |
||||||
|
x[k - 1] = t; |
||||||
|
} |
||||||
|
k -= 2; |
||||||
|
if (k <= kl) { |
||||||
|
k = 2 * j; |
||||||
|
kl = j; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
static __inline void |
||||||
|
reorder2(int m, float *x) |
||||||
|
{ |
||||||
|
int ji, ij, n; |
||||||
|
float t; |
||||||
|
|
||||||
|
n = 1 << m; |
||||||
|
for (ij = 0; ij <= (n - 2); ij += 2) { |
||||||
|
ji = bitrev(ij >> 1, m) << 1; |
||||||
|
if (ij < ji) { |
||||||
|
t = x[ij]; |
||||||
|
x[ij] = x[ji]; |
||||||
|
x[ji] = t; |
||||||
|
t = x[ij + 1]; |
||||||
|
x[ij + 1] = x[ji + 1]; |
||||||
|
x[ji + 1] = t; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/***********************************************************/ |
||||||
|
|
||||||
|
// rcfft
|
||||||
|
|
||||||
|
static void |
||||||
|
rcrad4(int ii, int nn, |
||||||
|
float *x0, float *x1, float *x2, float *x3, |
||||||
|
float *x4, float *x5, float *x6, float *x7) |
||||||
|
{ |
||||||
|
double arg, tpiovn; |
||||||
|
float c1, c2, c3, s1, s2, s3, pr, pi, r1, r5; |
||||||
|
float t0, t1, t2, t3, t4, t5, t6, t7; |
||||||
|
int i0, i4, j, j0, ji, jl, jr, jlast, k, k0, kl, m, n, ni; |
||||||
|
|
||||||
|
n = nn / 4; |
||||||
|
for (m = 1; (1 << m) < n; m++) |
||||||
|
continue; |
||||||
|
tpiovn = 2 * M_PI / nn; |
||||||
|
ji = 3; |
||||||
|
jl = 2; |
||||||
|
jr = 2; |
||||||
|
ni = (n + 1) / 2; |
||||||
|
for (i0 = 0; i0 < ni; i0++) { |
||||||
|
if (i0 == 0) { |
||||||
|
for (k = 0; k < ii; k++) { |
||||||
|
t0 = x0[k] + x2[k]; |
||||||
|
t1 = x1[k] + x3[k]; |
||||||
|
x2[k] = x0[k] - x2[k]; |
||||||
|
x3[k] = x1[k] - x3[k]; |
||||||
|
x0[k] = t0 + t1; |
||||||
|
x1[k] = t0 - t1; |
||||||
|
} |
||||||
|
if (nn > 4) { |
||||||
|
k0 = ii * 4; |
||||||
|
kl = k0 + ii; |
||||||
|
for (k = k0; k < kl; k++) { |
||||||
|
pr = (float) (M_SQRT1_2 * (x1[k] - x3[k])); |
||||||
|
pi = (float) (M_SQRT1_2 * (x1[k] + x3[k])); |
||||||
|
x3[k] = x2[k] + pi; |
||||||
|
x1[k] = pi - x2[k]; |
||||||
|
x2[k] = x0[k] - pr; |
||||||
|
x0[k] += pr; |
||||||
|
} |
||||||
|
} |
||||||
|
} else { |
||||||
|
arg = tpiovn * bitrev(i0, m); |
||||||
|
c1 = cosf(arg); |
||||||
|
s1 = sinf(arg); |
||||||
|
c2 = c1 * c1 - s1 * s1; |
||||||
|
s2 = c1 * s1 + c1 * s1; |
||||||
|
c3 = c1 * c2 - s1 * s2; |
||||||
|
s3 = c2 * s1 + s2 * c1; |
||||||
|
i4 = ii * 4; |
||||||
|
j0 = jr * i4; |
||||||
|
k0 = ji * i4; |
||||||
|
jlast = j0 + ii; |
||||||
|
for (j = j0; j < jlast; j++) { |
||||||
|
k = k0 + j - j0; |
||||||
|
r1 = x1[j] * c1 - x5[k] * s1; |
||||||
|
r5 = x1[j] * s1 + x5[k] * c1; |
||||||
|
t2 = x2[j] * c2 - x6[k] * s2; |
||||||
|
t6 = x2[j] * s2 + x6[k] * c2; |
||||||
|
t3 = x3[j] * c3 - x7[k] * s3; |
||||||
|
t7 = x3[j] * s3 + x7[k] * c3; |
||||||
|
t0 = x0[j] + t2; |
||||||
|
t4 = x4[k] + t6; |
||||||
|
t2 = x0[j] - t2; |
||||||
|
t6 = x4[k] - t6; |
||||||
|
t1 = r1 + t3; |
||||||
|
t5 = r5 + t7; |
||||||
|
t3 = r1 - t3; |
||||||
|
t7 = r5 - t7; |
||||||
|
x0[j] = t0 + t1; |
||||||
|
x7[k] = t4 + t5; |
||||||
|
x6[k] = t0 - t1; |
||||||
|
x1[j] = t5 - t4; |
||||||
|
x2[j] = t2 - t7; |
||||||
|
x5[k] = t6 + t3; |
||||||
|
x4[k] = t2 + t7; |
||||||
|
x3[j] = t3 - t6; |
||||||
|
} |
||||||
|
jr += 2; |
||||||
|
ji -= 2; |
||||||
|
if (ji <= jl) { |
||||||
|
ji = 2 * jr - 1; |
||||||
|
jl = jr; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
//-----------------------------------------------------------
|
||||||
|
|
||||||
|
static int |
||||||
|
rcfft2(float *x, int m) |
||||||
|
{ |
||||||
|
int ii, nn, m2, it, n; |
||||||
|
|
||||||
|
n = 1 << m;; |
||||||
|
m2 = m / 2; |
||||||
|
|
||||||
|
// radix 2
|
||||||
|
|
||||||
|
if (m <= m2 * 2) { |
||||||
|
nn = 1; |
||||||
|
} else { |
||||||
|
nn = 2; |
||||||
|
ii = n / nn; |
||||||
|
rad2(ii, x, x + ii); |
||||||
|
} |
||||||
|
|
||||||
|
// radix 4
|
||||||
|
|
||||||
|
if (m2 != 0) { |
||||||
|
for (it = 0; it < m2; it++) { |
||||||
|
nn = nn * 4; |
||||||
|
ii = n / nn; |
||||||
|
rcrad4(ii, nn, x, x + ii, x + 2 * ii, x + 3 * ii, |
||||||
|
x, x + ii, x + 2 * ii, x + 3 * ii); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// re-order
|
||||||
|
|
||||||
|
reorder1(m, x); |
||||||
|
reorder2(m, x); |
||||||
|
for (it = 3; it < n; it += 2) |
||||||
|
x[it] = -x[it]; |
||||||
|
x[n] = x[1]; |
||||||
|
x[1] = 0.0; |
||||||
|
x[n + 1] = 0.0; |
||||||
|
|
||||||
|
return (0); |
||||||
|
} |
||||||
|
|
||||||
|
/***********************************************************/ |
||||||
|
|
||||||
|
// rcfft
|
||||||
|
|
||||||
|
static void |
||||||
|
crrad4(int jj, int nn, |
||||||
|
float *x0, float *x1, float *x2, float *x3, |
||||||
|
float *x4, float *x5, float *x6, float *x7) |
||||||
|
{ |
||||||
|
double arg, tpiovn; |
||||||
|
float c1, c2, c3, s1, s2, s3; |
||||||
|
float t0, t1, t2, t3, t4, t5, t6, t7; |
||||||
|
int ii, j, j0, ji, jr, jl, jlast, j4, k, k0, kl, m, n, ni; |
||||||
|
|
||||||
|
tpiovn = 2 * M_PI / nn; |
||||||
|
ji = 3; |
||||||
|
jl = 2; |
||||||
|
jr = 2; |
||||||
|
n = nn / 4; |
||||||
|
for (m = 1; (1 << m) < n; m++) |
||||||
|
continue; |
||||||
|
ni = (n + 1) / 2; |
||||||
|
for (ii = 0; ii < ni; ii++) { |
||||||
|
if (ii == 0) { |
||||||
|
for (k = 0; k < jj; k++) { |
||||||
|
t0 = x0[k] + x1[k]; |
||||||
|
t1 = x0[k] - x1[k]; |
||||||
|
t2 = x2[k] * 2; |
||||||
|
t3 = x3[k] * 2; |
||||||
|
x0[k] = t0 + t2; |
||||||
|
x2[k] = t0 - t2; |
||||||
|
x1[k] = t1 + t3; |
||||||
|
x3[k] = t1 - t3; |
||||||
|
} |
||||||
|
if (nn > 4) { |
||||||
|
k0 = jj * 4; |
||||||
|
kl = k0 + jj; |
||||||
|
for (k = k0; k < kl; k++) { |
||||||
|
t2 = x0[k] - x2[k]; |
||||||
|
t3 = x1[k] + x3[k]; |
||||||
|
x0[k] = (x0[k] + x2[k]) * 2; |
||||||
|
x2[k] = (x3[k] - x1[k]) * 2; |
||||||
|
x1[k] = (float) ((t2 + t3) * M_SQRT2); |
||||||
|
x3[k] = (float) ((t3 - t2) * M_SQRT2); |
||||||
|
} |
||||||
|
} |
||||||
|
} else { |
||||||
|
arg = tpiovn * bitrev(ii, m); |
||||||
|
c1 = cosf(arg); |
||||||
|
s1 = -sinf(arg); |
||||||
|
c2 = c1 * c1 - s1 * s1; |
||||||
|
s2 = c1 * s1 + c1 * s1; |
||||||
|
c3 = c1 * c2 - s1 * s2; |
||||||
|
s3 = c2 * s1 + s2 * c1; |
||||||
|
j4 = jj * 4; |
||||||
|
j0 = jr * j4; |
||||||
|
k0 = ji * j4; |
||||||
|
jlast = j0 + jj; |
||||||
|
for (j = j0; j < jlast; j++) { |
||||||
|
k = k0 + j - j0; |
||||||
|
t0 = x0[j] + x6[k]; |
||||||
|
t1 = x7[k] - x1[j]; |
||||||
|
t2 = x0[j] - x6[k]; |
||||||
|
t3 = x7[k] + x1[j]; |
||||||
|
t4 = x2[j] + x4[k]; |
||||||
|
t5 = x5[k] - x3[j]; |
||||||
|
t6 = x5[k] + x3[j]; |
||||||
|
t7 = x4[k] - x2[j]; |
||||||
|
x0[j] = t0 + t4; |
||||||
|
x4[k] = t1 + t5; |
||||||
|
x1[j] = (t2 + t6) * c1 - (t3 + t7) * s1; |
||||||
|
x5[k] = (t2 + t6) * s1 + (t3 + t7) * c1; |
||||||
|
x2[j] = (t0 - t4) * c2 - (t1 - t5) * s2; |
||||||
|
x6[k] = (t0 - t4) * s2 + (t1 - t5) * c2; |
||||||
|
x3[j] = (t2 - t6) * c3 - (t3 - t7) * s3; |
||||||
|
x7[k] = (t2 - t6) * s3 + (t3 - t7) * c3; |
||||||
|
} |
||||||
|
jr += 2; |
||||||
|
ji -= 2; |
||||||
|
if (ji <= jl) { |
||||||
|
ji = 2 * jr - 1; |
||||||
|
jl = jr; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
//-----------------------------------------------------------
|
||||||
|
|
||||||
|
static int |
||||||
|
crfft2(float *x, int m) |
||||||
|
{ |
||||||
|
int n, i, it, nn, jj, m2; |
||||||
|
|
||||||
|
n = 1 << m; |
||||||
|
x[1] = x[n]; |
||||||
|
m2 = m / 2; |
||||||
|
|
||||||
|
// re-order
|
||||||
|
|
||||||
|
for (i = 3; i < n; i += 2) |
||||||
|
x[i] = -x[i]; |
||||||
|
reorder2(m, x); |
||||||
|
reorder1(m, x); |
||||||
|
|
||||||
|
// radix 4
|
||||||
|
|
||||||
|
if (m2 != 0) { |
||||||
|
nn = 4 * n; |
||||||
|
for (it = 0; it < m2; it++) { |
||||||
|
nn = nn / 4; |
||||||
|
jj = n / nn; |
||||||
|
crrad4(jj, nn, x, x + jj, x + 2 * jj, x + 3 * jj, |
||||||
|
x, x + jj, x + 2 * jj, x + 3 * jj); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// radix 2
|
||||||
|
|
||||||
|
if (m > m2 * 2) { |
||||||
|
jj = n / 2; |
||||||
|
rad2(jj, x, x + jj); |
||||||
|
} |
||||||
|
|
||||||
|
return (0); |
||||||
|
} |
||||||
|
|
||||||
|
/***********************************************************/ |
||||||
|
|
||||||
|
// real-to-complex FFT
|
||||||
|
|
||||||
|
//FUNC(void)
|
||||||
|
void cha_fft_rc(float *x, int n) |
||||||
|
{ |
||||||
|
int m; |
||||||
|
|
||||||
|
// assume n is a power of two
|
||||||
|
m = ilog2(n); |
||||||
|
rcfft2(x, m); |
||||||
|
} |
||||||
|
|
||||||
|
// complex-to-real inverse FFT
|
||||||
|
|
||||||
|
//FUNC(void)
|
||||||
|
void cha_fft_cr(float *x, int n) |
||||||
|
{ |
||||||
|
int i, m; |
||||||
|
|
||||||
|
// assume n is a power of two
|
||||||
|
m = ilog2(n); |
||||||
|
crfft2(x, m); |
||||||
|
// scale inverse by 1/n
|
||||||
|
for (i = 0; i < n; i++) { |
||||||
|
x[i] /= n; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
Loading…
Reference in new issue