/** ****************************************************************************** * @file AudioFilterConvolution_F32.cpp * @author Giuseppe Callipo - IK8YFW - ik8yfw@libero.it * @version V2.0.0 * @date 06-02-2021 * @brief F32 Filter Convolution * ****************************************************************************** ****************************************************************************** This software is based on the AudioFilterConvolution routine Written by Brian Millier on Mar 2017 https://circuitcellar.com/research-design-hub/fancy-filtering-with-the-teensy-3-6/ and modified by Giuseppe Callipo - ik8yfw. Modifications: 1) Class refactoring, change some methods visibility; 2) Filter coefficients calculation included into class; 3) Change the class for running in both with F32 OpenAudio_ArduinoLibrary for Teensy; 4) Added initFilter method for single anf fast initialization and on the fly reinititializzation; 5) Optimize it to use as output audio filter on SDR receiver. 6) Optimize the time execution *******************************************************************/ // Revised for OpenAudio_Arduino Teensy F32 library, 8 Feb 2022 #include "AudioFilterConvolution_F32.h" void AudioFilterConvolution_F32::passThrough(int stat) { passThru=stat; } // Function to pre-calculate the multiplying frequency function, the "mask." void AudioFilterConvolution_F32::impulse(float32_t *FIR_coef) { uint32_t k = 0; uint32_t i = 0; enabled = 0; // shut off audio stream while impulse is loading for (i = 0; i < (FFT_length / 2) + 1; i++) { FIR_filter_mask[k++] = FIR_coef[i]; FIR_filter_mask[k++] = 0; } for (i = FFT_length + 1; i < FFT_length * 2; i++) { FIR_filter_mask[i] = 0.0; } arm_cfft_f32( &arm_cfft_sR_f32_len1024, FIR_filter_mask, 0, 1); // for 1st time thru, zero out the last sample buffer to 0 arm_fill_f32(0, last_sample_buffer_L, BUFFER_SIZE *4); state = 0; enabled = 1; //enable audio stream again } void AudioFilterConvolution_F32::update(void) { audio_block_f32_t *block; float32_t *bp; if (enabled != 1 ) return; block = receiveWritable_f32(0); // MUST be Writable, as convolution results are written into block if (block) { switch (state) { case 0: if (passThru ==0) { arm_cmplx_mult_cmplx_f32(FFT_buffer, FIR_filter_mask, iFFT_buffer, FFT_length); // complex multiplication in Freq domain = convolution in time domain arm_cfft_f32(&arm_cfft_sR_f32_len1024, iFFT_buffer, 1, 1); // perform complex inverse FFT k = 0; l = 1024; for (int i = 0; i < 512; i++) { buffer[i] = last_sample_buffer_L[i] + iFFT_buffer[k++]; // this performs the "ADD" in overlap/Add last_sample_buffer_L[i] = iFFT_buffer[l++]; // this saves 512 samples (overlap) for next time around k++; l++; } } arm_copy_f32 (&buffer[0], &tbuffer[0], BUFFER_SIZE*4); bp = block->data; for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) { buffer[i] = *bp; *bp++ = tbuffer[i]; } AudioStream_F32::transmit(block); AudioStream_F32::release(block); state = 1; break; case 1: bp = block->data; for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) { buffer[128+i] = *bp; *bp++ = tbuffer[i+128]; } AudioStream_F32::transmit(block); AudioStream_F32::release(block); state = 2; break; case 2: bp = block->data; for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) { buffer[256 + i] = *bp; *bp++ = tbuffer[i+256]; // tbuffer contains results of last FFT/multiply/iFFT processing (convolution filtering) } AudioStream_F32::transmit(block); AudioStream_F32::release(block); // zero pad last half of array- necessary to prevent aliasing in FFT arm_fill_f32(0, FFT_buffer + 1024, FFT_length); state = 3; break; case 3: bp = block->data; for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) { buffer[384 + i] = *bp; *bp++ = tbuffer[i + 384]; // tbuffer contains results of last FFT/multiply/iFFT processing (convolution filtering) } AudioStream_F32::transmit(block); AudioStream_F32::release(block); state = 0; // 4 blocks are in- now do the FFT1024,complex multiply and iFFT1024 on 512samples of data // using the overlap/add method if (passThru ==0) { //fill FFT_buffer with current audio samples k = 0; for (i = 0; i < 512; i++) { FFT_buffer[k++] = buffer[i]; // real FFT_buffer[k++] = buffer[i]; // imag } // calculations are performed in-place in FFT routines arm_cfft_f32(&arm_cfft_sR_f32_len1024, FFT_buffer, 0, 1);// perform complex FFT } //end if passTHru break; } } } float32_t AudioFilterConvolution_F32::Izero (float32_t x) { float32_t x2 = x / 2.0; float32_t summe = 1.0; float32_t ds = 1.0; float32_t di = 1.0; float32_t errorlimit = 1e-9; float32_t tmp; do { tmp = x2 / di; tmp *= tmp; ds *= tmp; summe += ds; di += 1.0; } while (ds >= errorlimit * summe); return (summe); } // END Izero float AudioFilterConvolution_F32::m_sinc(int m, float fc) { // fc is f_cut/(Fsamp/2) // m is between -M and M step 2 // float x = m*PIH; if(m == 0) return 1.0f; else return sinf(x*fc)/(fc*x); } void AudioFilterConvolution_F32::calc_FIR_coeffs (float *coeffs, int numCoeffs, float32_t fc, float32_t Astop, int type, float dfc, float Fsamprate){ // pointer to coefficients variable, no. of coefficients to calculate, // frequency where it happens, stopband attenuation in dB, // filter type, half-filter bandwidth (only for bandpass and notch). // Modified by WMXZ and DD4WH after // Wheatley, M. (2011): CuteSDR Technical Manual. www.metronix.com, // pages 118 - 120, FIR with Kaiser-Bessel Window. // Assess required number of coefficients by // numCoeffs = (Astop - 8.0) / (2.285 * TPI * normFtrans); // selecting high-pass, numCoeffs is forced to an even number for // better frequency response int ii,jj; float32_t Beta; float32_t izb; float fcf = fc; int nc = numCoeffs; fc = 2.0f * fc / Fsamprate; // Corrected dfc = dfc / Fsamprate; // calculate Kaiser-Bessel window shape factor beta from stop-band attenuation if (Astop < 20.96) Beta = 0.0; else if (Astop >= 50.0) Beta = 0.1102 * (Astop - 8.71); else Beta = 0.5842 * powf((Astop - 20.96), 0.4) + 0.07886 * (Astop - 20.96); izb = Izero (Beta); if(type == LOWPASS) { fcf = fc; nc = numCoeffs; } else if(type == HIGHPASS) { fcf = -fc; nc = 2*(numCoeffs/2); } else if ((type == BANDPASS) || (type==BANDREJECT)) { fcf = dfc; nc = 2*(numCoeffs/2); // maybe not needed } /* else if (type==HILBERT) { nc = 2*(numCoeffs/2); // clear coefficients for(ii=0; ii< 2*(nc-1); ii++) coeffs[ii]=0; // set real delay coeffs[nc]=1; // <<<<