From d9f58c19993c4b37dfae7e2b0d4e911671066f0c Mon Sep 17 00:00:00 2001 From: Chip Audette Date: Wed, 22 Feb 2017 07:43:11 -0500 Subject: [PATCH] Add ConfigFIRFilterBank_F32 to library --- AudioConfigFIRFilterBank_F32.h | 274 +++++++++++++++++++++++ OpenAudio_ArduinoLibrary.h | 1 + keywords.txt | 3 + utility/rfft.c | 384 +++++++++++++++++++++++++++++++++ 4 files changed, 662 insertions(+) create mode 100644 AudioConfigFIRFilterBank_F32.h create mode 100644 utility/rfft.c diff --git a/AudioConfigFIRFilterBank_F32.h b/AudioConfigFIRFilterBank_F32.h new file mode 100644 index 0000000..49a6e79 --- /dev/null +++ b/AudioConfigFIRFilterBank_F32.h @@ -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 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; inchannel; //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); +//} + diff --git a/OpenAudio_ArduinoLibrary.h b/OpenAudio_ArduinoLibrary.h index f9a5522..571a985 100644 --- a/OpenAudio_ArduinoLibrary.h +++ b/OpenAudio_ArduinoLibrary.h @@ -4,6 +4,7 @@ #include #include "AudioCalcEnvelope_F32.h" #include "AudioCalcGainWDRC_F32.h" +#include "AudioConfigFIRFilterBank_F32.h" #include #include "AudioEffectCompWDRC_F32.h" #include "AudioEffectEmpty_F32.h" diff --git a/keywords.txt b/keywords.txt index 9777b2d..421bf4d 100644 --- a/keywords.txt +++ b/keywords.txt @@ -20,6 +20,9 @@ setInputGain_dB KEYWORD2 AudioControlSGTL5000_Extended KEYWORD1 micBiasEnable KEYWORD2 +AudioConfigFIRFilterBank_F32 KEYWORD1 +createFilterCoeff KEYWORD2 + AudioConvert_I16toF32 KEYWORD1 AudioConvert_F32toI16 KEYWORD1 diff --git a/utility/rfft.c b/utility/rfft.c new file mode 100644 index 0000000..df59701 --- /dev/null +++ b/utility/rfft.c @@ -0,0 +1,384 @@ + +#include +//#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; + } +} +