boblark
3 years ago
parent
a23e900831
commit
0dc8bb6864
@ -0,0 +1,276 @@ |
||||
/**
|
||||
****************************************************************************** |
||||
* @file AudioFilterConvolution_F32.cpp |
||||
* @author Giuseppe Callipo - IK8YFW - ik8yfw@libero.it |
||||
* @version V1.0.0 |
||||
* @date 02-05-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. |
||||
*******************************************************************/ |
||||
|
||||
#include "AudioFilterConvolution_F32.h" |
||||
|
||||
boolean AudioFilterConvolution_F32::begin(int status) |
||||
{ |
||||
enabled = status; |
||||
return(true); |
||||
} |
||||
|
||||
void AudioFilterConvolution_F32::passThrough(int stat) |
||||
{ |
||||
passThru=stat; |
||||
} |
||||
|
||||
void AudioFilterConvolution_F32::impulse(float32_t *FIR_coef) { |
||||
// arm_q15_to_float(coefs, FIR_coef, 513); // convert int_buffer to float 32bit
|
||||
int k = 0; |
||||
int 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 (int i = 0; i < 1024; i++) { |
||||
// Serial.println(FIR_filter_mask[i] * 32768);
|
||||
} |
||||
// for 1st time thru, zero out the last sample buffer to 0
|
||||
memset(last_sample_buffer_L, 0, sizeof(last_sample_buffer_L)); |
||||
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: |
||||
bp = block->data; |
||||
for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) { |
||||
buffer[i] = *bp++; |
||||
} |
||||
bp = block->data; |
||||
for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) { |
||||
*bp++ = tbuffer[i]; // tbuffer contains results of last FFT/multiply/iFFT processing (convolution filtering)
|
||||
} |
||||
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 = block->data; |
||||
for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) { |
||||
*bp++ = tbuffer[i+128]; // tbuffer contains results of last FFT/multiply/iFFT processing (convolution filtering)
|
||||
} |
||||
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 = block->data; |
||||
for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) { |
||||
*bp++ = tbuffer[i+256]; // tbuffer contains results of last FFT/multiply/iFFT processing (convolution filtering)
|
||||
} |
||||
AudioStream_F32::transmit(block); |
||||
AudioStream_F32::release(block); |
||||
state = 3; |
||||
break; |
||||
case 3: |
||||
bp = block->data; |
||||
for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) { |
||||
buffer[384 + i] = *bp++; |
||||
} |
||||
bp = block->data; |
||||
for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) { |
||||
*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
|
||||
// 1st convert Q15 samples to float
|
||||
//arm_q15_to_float(buffer, float_buffer_L, 512);
|
||||
arm_copy_f32 (buffer, float_buffer_L, 512); |
||||
// float_buffer samples are now standardized from > -1.0 to < 1.0
|
||||
if (passThru ==0) { |
||||
memset(FFT_buffer + 1024, 0, sizeof(FFT_buffer) / 2); // zero pad last half of array- necessary to prevent aliasing in FFT
|
||||
//fill FFT_buffer with current audio samples
|
||||
k = 0; |
||||
for (i = 0; i < 512; i++) |
||||
{ |
||||
FFT_buffer[k++] = float_buffer_L[i]; // real
|
||||
FFT_buffer[k++] = float_buffer_L[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
|
||||
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++) { |
||||
float_buffer_L[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++; |
||||
} |
||||
} //end if passTHru
|
||||
// convert floats to Q15 and save in temporary array tbuffer
|
||||
//arm_float_to_q15(&float_buffer_L[0], &tbuffer[0], BUFFER_SIZE*4);
|
||||
arm_copy_f32 (&float_buffer_L[0], &tbuffer[0], BUFFER_SIZE*4); |
||||
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 = fc / Fsamprate; |
||||
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 == 0) // low pass filter
|
||||
// { fcf = fc;
|
||||
{ fcf = fc * 2.0; |
||||
nc = numCoeffs; |
||||
} |
||||
else if(type == 1) // high-pass filter
|
||||
{ fcf = -fc; |
||||
nc = 2*(numCoeffs/2); |
||||
} |
||||
else if ((type == 2) || (type==3)) // band-pass filter
|
||||
{ |
||||
fcf = dfc; |
||||
nc = 2*(numCoeffs/2); // maybe not needed
|
||||
} |
||||
else if (type==4) // Hilbert transform
|
||||
{ |
||||
nc = 2*(numCoeffs/2); |
||||
// clear coefficients
|
||||
for(ii=0; ii< 2*(nc-1); ii++) coeffs[ii]=0; |
||||
// set real delay
|
||||
coeffs[nc]=1; |
||||
|
||||
// set imaginary Hilbert coefficients
|
||||
for(ii=1; ii< (nc+1); ii+=2) |
||||
{ |
||||
if(2*ii==nc) continue; |
||||
float x =(float)(2*ii - nc)/(float)nc; |
||||
float w = Izero(Beta*sqrtf(1.0f - x*x))/izb; // Kaiser window
|
||||
coeffs[2*ii+1] = 1.0f/(PIH*(float)(ii-nc/2)) * w ; |
||||
} |
||||
return; |
||||
} |
||||
|
||||
for(ii= - nc, jj=0; ii< nc; ii+=2,jj++) |
||||
{ |
||||
float x =(float)ii/(float)nc; |
||||
float w = Izero(Beta*sqrtf(1.0f - x*x))/izb; // Kaiser window
|
||||
coeffs[jj] = fcf * m_sinc(ii,fcf) * w; |
||||
} |
||||
|
||||
if(type==1) |
||||
{ |
||||
coeffs[nc/2] += 1; |
||||
} |
||||
else if (type==2) |
||||
{ |
||||
for(jj=0; jj< nc+1; jj++) coeffs[jj] *= 2.0f*cosf(PIH*(2*jj-nc)*fc); |
||||
} |
||||
else if (type==3) |
||||
{ |
||||
for(jj=0; jj< nc+1; jj++) coeffs[jj] *= -2.0f*cosf(PIH*(2*jj-nc)*fc); |
||||
coeffs[nc/2] += 1; |
||||
} |
||||
} // END calc_FIR_coef
|
||||
|
||||
|
||||
|
||||
void AudioFilterConvolution_F32::initFilter ( float32_t fc, float32_t Astop, int type, float dfc){ |
||||
//Init Fir
|
||||
calc_FIR_coeffs (FIR_Coef, MAX_NUMCOEF, fc, Astop, type, dfc, fs); |
||||
begin(0); // set to zero to disable audio processing until impulse has been loaded
|
||||
impulse(FIR_Coef); // generates Filter Mask and enables the audio stream
|
||||
} |
||||
@ -0,0 +1,131 @@ |
||||
/**
|
||||
****************************************************************************** |
||||
* @file AudioFilterConvolution_F32.h |
||||
* @author Giuseppe Callipo - IK8YFW - ik8yfw@libero.it |
||||
* @version V1.0.0 |
||||
* @date 02-05-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 re-inititialization; |
||||
5) Optimize it to use as output audio filter on SDR receiver. |
||||
|
||||
Brought to the Teensy F32 OpenAudio_ArduinoLibrary 2 Feb 2022, Bob Larkin |
||||
******************************************************************************/ |
||||
/*Notes from Giuseppe Callipo, IK8YFW:
|
||||
|
||||
*** How to use it. *** |
||||
|
||||
Create an audio project based on chipaudette/OpenAudio_ArduinoLibrary |
||||
like the Keiths SDR Project, and just add the h and cpp file to your |
||||
processing chain as unique output filter: |
||||
|
||||
************************************************************ |
||||
|
||||
1) Include the header |
||||
#include "AudioFilterConvolution_F32.h" (or OpenAudio_ArduinoLibrary.h) |
||||
|
||||
2) Initialize as F32 block |
||||
(the block must be 128 but the sample rate can be changed but must initialized) |
||||
const float sample_rate_Hz = 96000.0f; // or 44100.0f or other
|
||||
const int audio_block_samples = 128; |
||||
AudioSettings_F32 audio_settings(sample_rate_Hz, audio_block_samples); |
||||
|
||||
AudioFilterConvolution_F32 FilterConv(audio_settings); |
||||
|
||||
3) Connect before output |
||||
|
||||
AudioConnection_F32 patchCord1(FilterConv,0,Output_i2s,0); |
||||
AudioConnection_F32 patchCord2(FilterConv,0,Output_i2s,1); |
||||
|
||||
4) Set the filter value when you need - some examples: |
||||
// CW - Centered at 800Hz, ( 40 db x oct ), 2=BPF, width = 1200Hz
|
||||
FilterConv.initFilter((float32_t)800, 40, 2, 1200.0); |
||||
|
||||
// SSB - Centered at 1500Hz, ( 60 db x oct ), 2=BPF, width = 3000Hz
|
||||
FilterConv.initFilter((float32_t)1500, 60, 2, 3000.0); |
||||
************************************************************** */ |
||||
/* Additional Notes from Bob
|
||||
* Measured 128 word update in update() is 248 microseconds (T4.x) |
||||
* Comparison with a conventional FIR, from this library, the |
||||
* AudioFilterFIRGeneral_F32 showed that a 512 tap FIR gave |
||||
* essentially the same response and was slightly faster at |
||||
* 225 microseconds per update. Also, note that this form of |
||||
* computation uses about 78 kB of memory where the direct FIR |
||||
* uses about 15 kB. The responses differ in only minor ways. |
||||
* ************************************************************ */ |
||||
|
||||
#ifndef AudioFilterConvolution_F32_h_ |
||||
#define AudioFilterConvolution_F32_h_ |
||||
|
||||
#include <AudioStream_F32.h> |
||||
#include <arm_math.h> |
||||
#include <arm_const_structs.h> |
||||
|
||||
#define MAX_NUMCOEF 513 |
||||
#define TPI 6.28318530717959f |
||||
#define PIH 1.57079632679490f |
||||
#define FOURPI 2.0 * TPI |
||||
#define SIXPI 3.0 * TPI |
||||
|
||||
class AudioFilterConvolution_F32 : |
||||
public AudioStream_F32 |
||||
{ |
||||
public: |
||||
AudioFilterConvolution_F32(void) : AudioStream_F32(1, inputQueueArray_f32) { |
||||
fs = AUDIO_SAMPLE_RATE; |
||||
//block_size = AUDIO_BLOCK_SAMPLES;
|
||||
}; |
||||
AudioFilterConvolution_F32(const AudioSettings_F32 &settings) : AudioStream_F32(1, inputQueueArray_f32) { |
||||
// Performs the first initialize
|
||||
fs = settings.sample_rate_Hz; |
||||
}; |
||||
|
||||
boolean begin(int status); |
||||
virtual void update(void); |
||||
void passThrough(int stat); |
||||
void initFilter (float32_t fc, float32_t Astop, int type, float dfc); |
||||
|
||||
private: |
||||
#define BUFFER_SIZE 128 |
||||
float32_t fs; |
||||
|
||||
audio_block_f32_t *inputQueueArray_f32[1]; |
||||
|
||||
float32_t *sp_L; |
||||
volatile uint8_t state; |
||||
int i; |
||||
int k; |
||||
int l; |
||||
int passThru; |
||||
int enabled; |
||||
float32_t FIR_Coef[MAX_NUMCOEF]; |
||||
const uint32_t FFT_length = 1024; |
||||
float32_t FIR_coef[2048] __attribute__((aligned(4))); |
||||
float32_t FIR_filter_mask[2048] __attribute__((aligned(4))); |
||||
float32_t buffer[2048] __attribute__((aligned(4))); |
||||
float32_t tbuffer[2048]__attribute__((aligned(4))); |
||||
float32_t FFT_buffer[2048] __attribute__((aligned(4))); |
||||
float32_t iFFT_buffer[2048] __attribute__((aligned(4))); |
||||
float32_t float_buffer_L[512]__attribute__((aligned(4))); |
||||
float32_t last_sample_buffer_L[512]; |
||||
void impulse(float32_t *coefs); |
||||
float32_t Izero (float32_t x); |
||||
float m_sinc(int m, float fc); |
||||
void calc_FIR_coeffs (float * coeffs, int numCoeffs, float32_t fc, float32_t Astop, int type, float dfc, float Fsamprate); |
||||
|
||||
}; |
||||
|
||||
#endif |
||||
@ -0,0 +1,78 @@ |
||||
/*
|
||||
* TestConvolutionalFilter.ino Bob Larkin 2 Feb 2022 |
||||
* Measure frequency response of LPF. |
||||
* |
||||
* This uses the Goertzel algoritm of AudioAnalyzeToneDetect_F32 as |
||||
* a "direct conversion receiver." This provides the excellent |
||||
* selectivity needed for measuring below -100 dBfs. |
||||
* |
||||
* Public Domain - Teensy |
||||
*/ |
||||
|
||||
#include "Audio.h" |
||||
#include "OpenAudio_ArduinoLibrary.h" |
||||
#include "AudioStream_F32.h" |
||||
|
||||
AudioSynthSineCosine_F32 sine_cos1; //xy=87,151
|
||||
AudioFilterConvolution_F32 convolution1; //xy=163,245
|
||||
AudioFilterFIRGeneral_F32 filterFIRgeneral1; //xy=285,177
|
||||
AudioOutputI2S_F32 audioOutI2S1; //xy=357,337
|
||||
AudioAnalyzeToneDetect_F32 toneDet1; //xy=377,259
|
||||
AudioAnalyzeToneDetect_F32 toneDet2; //xy=378,299
|
||||
AudioConnection_F32 patchCord1(sine_cos1, 0, filterFIRgeneral1, 0); |
||||
AudioConnection_F32 patchCord2(sine_cos1, 1, convolution1, 0); |
||||
AudioConnection_F32 patchCord3(convolution1, toneDet2); |
||||
AudioConnection_F32 patchCord4(convolution1, 0, audioOutI2S1, 0); |
||||
AudioConnection_F32 patchCord5(filterFIRgeneral1, 0, toneDet1, 0); |
||||
AudioControlSGTL5000 sgtl5000_1; //xy=164,343
|
||||
|
||||
float32_t saveDat[512]; |
||||
float32_t coeffFIR[512]; |
||||
float32_t stateFIR[1160]; |
||||
float32_t attenFIR[256]; |
||||
float32_t rdb[1000]; |
||||
|
||||
void setup(void) { |
||||
AudioMemory(5); |
||||
AudioMemory_F32(50); |
||||
Serial.begin(300); delay(1000); |
||||
Serial.println("*** Test Convolutional Filtering routine for F32 ***"); |
||||
sine_cos1.frequency(1000.0f); |
||||
sine_cos1.amplitude(1.0f); |
||||
toneDet1.frequency(100.0f, 5); |
||||
toneDet2.frequency(100.0f, 5); |
||||
convolution1.initFilter(4000.0f, 60.0f, 0, 0.0f); |
||||
convolution1.begin(1); |
||||
|
||||
// Design for direct FIR. This sets frequency response.
|
||||
// Bin for 4kHz at 44.117kHz sample rate and 400 coeff's is (4000/44117) * (512/2) = 23.2
|
||||
for(int ii=0; ii<47; ii++) attenFIR[ii] = 0.0f; |
||||
for(int ii=47; ii<256; ii++) attenFIR[ii] = -150.0f; |
||||
// FIRGeneralNew(float *adb, uint16_t nFIR, float *cf32f, float kdb, float *pStateArray);
|
||||
filterFIRgeneral1.FIRGeneralNew(attenFIR, 512, coeffFIR, 60.0, stateFIR); |
||||
|
||||
// DSP measure frequency response in dB uniformly spaced from 100 to 20000 Hz
|
||||
Serial.println("Freq Hz Conv dB FIR dB"); |
||||
for(float32_t f=100.0; f<=20000.0f; f+=50.0f) |
||||
{ |
||||
sine_cos1.frequency(f); |
||||
toneDet1.frequency(f, (f>2000.0f ? 100 : 5)); |
||||
toneDet2.frequency(f, (f>2000.0f ? 100 : 5)); |
||||
delay(50); |
||||
while(!toneDet1.available())delay(2) ; |
||||
toneDet1.read(); |
||||
while(!toneDet2.available())delay(2) ; |
||||
toneDet2.read(); |
||||
while(!toneDet2.available())delay(2) ; |
||||
Serial.print(f); |
||||
Serial.print(", "); |
||||
while(!toneDet2.available())delay(2) ; |
||||
Serial.print(20.0f*log10f(toneDet2.read() ) ); |
||||
Serial.print(", "); |
||||
while(!toneDet1.available())delay(2) ; |
||||
Serial.println(20.0f*log10f(toneDet1.read() ) ); |
||||
} |
||||
} |
||||
|
||||
void loop() { |
||||
} |
||||
Loading…
Reference in new issue