I have written software which will remove a set of frequencies from an audio stream, in c++, which I use to remove 60 Hz power line contamination (essentially a 60 Hz square wave) from other signals, which might be useful.
the source code is included below.
Notes:
The included code was cut out of larger stuff with a text editor. Maybe a line or two is missing?
I run it on Ubuntu 15.04. This particular program sucks in the audio from either the line-in or mic input. The right channel is copied to the right channel output un altered. The left channel has the first 50 even harmonics
subtracted and copied and copied to the left channel output.That is, the frequencies 600, 180, 300, ..., 3060 Hz are removed. The width of the notch filter at each frequency is about 1 Hz. If you run a signal generator through the left channel, you will notice some distortion within a few Hz of each notch. For fun, I put the output of an FM radio into the line-in and plugged the headphones in, and distortion on the left channel was not that
bad at all, given the amount of processing going on.
jackd and the development library are needed:
sudo apt-get install jackd
sudo apt-get install libjack-dev
For some bizarre reason, you must do two crazy things...
Edit (via sudo gedit or whatever) /etc/security/limits.conf to contain
the line
@audio - rtprio 99
If the username that runs the program is XXXX, you have to do
sudo usermod -a G audio XXXX
- The main program has the constants 4096 used, as the sample size of a block of data sucked in at a time. If the sound card on the machine cannot
handle this, or the machine is otherwise is slow, use 1024. This should always work.
There. I hope this is sufficiently self contained.
File freqs.h
#ifndef FREQS_H_INCLUDED
#define FREQS_H_INCLUDED
#include <stdio.h>
#include <stdlib.h>
#include <memory.h>
#include <math.h>
class freq_info
{public:
double Hz; // The frequency of interest
double w; // 2 * pi * Hz
double dt; // delta time; time between samples
int npts; // number of points
double tot_time; // total time of all points
int npts_1_cycle;
int npts_max_cycles;
double *cos_vals;
double *sin_vals;
double projection_sin;
double projection_cos;
double data_X_sin;
double data_X_cos;
int npts_in_product;
inline void zero() { memset( (void*)this, 0, sizeof(*this)); }
inline freq_info() { zero(); }
inline ~freq_info()
{
if(cos_vals!=0) delete [] cos_vals;
if(sin_vals!=0) delete [] sin_vals;
}
int init( double frequency, double time_between_samples, int number_of_points);
void projection_init();
void data_X_trigs_range( float *data, int data_idx_beg, int data_idx_end);
void projection_fini();
int produce_projection ( float *data, unsigned int npts_in);
};
class freq_group
{public:
// Following 3 should be same for all frequencies of interest
double dt; // delta time; time between samples
int npts; // number of points
double tot_time; // total time of all points
int min_npts_max_cycles; // minimum of npts_max_cycles for all
// FI[i] entries
double info_freq_min;
double info_freq_max;
int n_freqs; // # frequencies
freq_info *FI; // Info for all frequencies of interest.
inline void zero() { memset( (void*)this, 0, sizeof(*this)); }
inline freq_group() { zero(); }
inline ~freq_group()
{
if(FI!=0) delete [] FI;
}
void set_time_and_points(double time_between_samples, int number_of_points);
int setup_freqs( double * freqs_of_interest, int n_freqs_of_interest);
int remove_projections1(float *data_in, float *data_out, unsigned int npts_in);
};
File jack_interface.h
#ifndef JACK_INTERFACE_H_INCLUDED
#define JACK_INTERFACE_H_INCLUDED
#include <stdio.h>
#include <errno.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <jack/jack.h>
//When we are finally running, this holds a bunch of info...
class jack_interface
{public:
enum {max_channels=4};
char jack_interface_name[32];
unsigned int sample_size;
unsigned int samps_per_sec;
jack_client_t *jclient;
int n_channels; // or, number of ports
jack_port_t* in_ports [max_channels];
jack_port_t* out_ports[max_channels];
inline void zero() { memset( (void*)this, 0, sizeof(*this) ); }
inline jack_interface() { zero(); }
int create( char* interface_name,
unsigned int requested_sample_size,
int n_channels,
int (*process) (jack_nframes_t sample_size, void *arg) );
};
#endif JACK_INTERFACE_H_INCLUDED
File freqs.cpp
#include "freqs.h"
inline void ZAP(char *msg=0)
{
if(msg!=0) printf("%s\n", msg);
printf("Fatal error.\n");
exit(1);
}
void freq_info::projection_init()
{
projection_sin = 0.0;
projection_cos = 0.0;
data_X_sin = 0.0;
data_X_cos = 0.0;
npts_in_product = 0;
return;
}
void freq_info::data_X_trigs_range(float *data, int data_idx_beg, int data_idx_end)
{
static char name[] = "freq_info::data_X_trigs_range(...)";
int i;
if( data_idx_beg<0 || data_idx_end<0 || data_idx_end<data_idx_beg || data_idx_end>=npts)
{
printf("%s\nInput ranges out of range.\n", name);
ZAP();
}
for(i=data_idx_beg; i<=data_idx_end; i++)
{
data_X_sin += double( data[i] ) * sin_vals[i];
data_X_cos += double( data[i] ) * cos_vals[i];
}
npts_in_product += data_idx_end - data_idx_beg + 1;
return;
}
void freq_info::projection_fini()
{
projection_sin = (2.0 * data_X_sin * dt) / ( double(npts_in_product-1) * dt );
projection_cos = (2.0 * data_X_cos * dt) / ( double(npts_in_product-1) * dt );
return;
}
void freq_group::set_time_and_points(double time_between_samples, int number_of_points)
{
static char name[] = "freq_group::set_time_and_points(...)";
if(FI!=0) delete [] FI;
zero();
if(time_between_samples<=0.0 || number_of_points<8)
{
printf("%s\nInput parameters too small.\n", name);
ZAP();
}
dt = time_between_samples;
npts = number_of_points;
tot_time = double(npts-1) * dt;
return;
}
int freq_group::setup_freqs(double *freq_list, int n_freqs_in)
{
static char name[] = "freq_group::setup_freqs(...)";
int rval=0;
int i;
double freq_min, freq_max;
n_freqs = n_freqs_in;
if(freq_list==0 || n_freqs<1)
{
printf("%s\nInput args null, zero, or invalid.\n", name);
rval = 10;
goto EXIT_SEQ;
}
if( dt<=0.0 || npts<8 || tot_time<=0.0 )
{
printf("%s\nfreq_group::set_time_and_points NOT called!\n", name);
rval = 20;
goto EXIT_SEQ;
}
freq_min = 1.0e300;
freq_max = 1.0e-300;
for(i=0; i<n_freqs; i++)
{
if( freq_min > freq_list[i] )
freq_min = freq_list[i];
if( freq_max < freq_list[i] )
freq_max = freq_list[i];
}
if(freq_min<=0.0 || freq_max<=0.0)
{
printf("%s\nSpecified frequency zero or negative.\n", name);
rval = 30;
goto EXIT_SEQ;
}
if( 1.0/freq_min > tot_time)
{
printf("%s\nSpecified frequency, %20.10e Hz, too low for time length of sample.\n", name, freq_min);
rval = 40;
goto EXIT_SEQ;
}
info_freq_min = freq_min;
info_freq_max = freq_max;
if( dt > 2.0 * (1.0 / freq_max) )
{
printf("%s\nSpecified frequence, %20.10e Hz, too high for sample rate.\n", name, freq_max);
rval = 50;
goto EXIT_SEQ;
}
if(FI != 0)
{
delete [] FI;
}
FI = new freq_info[ n_freqs ];
min_npts_max_cycles = npts+2;
for(i=0; i<n_freqs; i++)
{
rval = FI[i].init( freq_list[i], dt, npts);
if(rval!=0)
{
printf("%s\nError from freq_info::init(...)\n", name);
goto EXIT_SEQ;
}
if(min_npts_max_cycles > FI[i].npts_max_cycles)
{
min_npts_max_cycles = FI[i].npts_max_cycles;
}
}
EXIT_SEQ:;
return rval;
}
int freq_info::init( double frequency, double time_between_samples, int number_of_points)
{
static char name[] = "freq_info::init(...)";
int rval=0;
static double two_pi = 0.0;
int i, max_cycles;
double arg;
if(frequency<=0 || time_between_samples<0.0 || number_of_points<8)
{
printf("%s\nInput parameters too small.\n", name);
rval = 10;
goto EXIT_SEQ;
}
if(two_pi == 0.0)
{
two_pi = 8.0 * atan( 1.0 );
}
if(cos_vals!=0)
delete [] cos_vals;
if(sin_vals!=0)
delete [] sin_vals;
zero();
Hz = frequency;
w = frequency * two_pi;
dt = time_between_samples;
npts = number_of_points;
tot_time = dt * double(npts - 1);
npts_1_cycle = int( (1.0/Hz) / time_between_samples ) + 1;
max_cycles = int( tot_time * Hz );
// Total # full cycles of Hz in the time period
if(max_cycles<1)
{
printf("%s\nSpecified frequency too low for the time span.\n", name);
rval = 30;
goto EXIT_SEQ;
}
npts_max_cycles = int( (double(max_cycles)/Hz) / time_between_samples ) + 1;
cos_vals = new double[npts+1];
sin_vals = new double[npts+1];
for(i=0; i<npts; i++)
{
arg = w * double(i) * dt;
sin_vals[i] = sin( arg );
cos_vals[i] = cos( arg );
}
EXIT_SEQ:;
return rval;
}
int freq_info::produce_projection ( float *data_in, unsigned int npts_in)
{
static char name[] = "freq_group::produce_projection (...)";
int rval=0;
if( int(npts_in) != npts)
{
printf("%s\nMismatch in # of data points.\n", name);
rval = 10;
goto EXIT_SEQ;
}
projection_init();
data_X_trigs_range(data_in, 0, this->npts_max_cycles);
projection_fini();
EXIT_SEQ:;
return rval;
}
int freq_group::remove_projections1(float *data_in, float *data_out, unsigned int npts_in)
{
static char name[] = "freq_group::remove_projections1(...)";
int rval=0;
int i,j,beg,end;
double accum;
if( int(npts_in) != npts)
{
printf("%s\nMismatch in # of data points.\n", name);
rval = 10;
goto EXIT_SEQ;
}
memset( (void*)data_out, 0, sizeof(*data_out) * npts_in);
for(i=0; i<this->n_freqs; i++)
{
FI[i].projection_init();
}
for(i=0; i<this->n_freqs; i++)
{
FI[i].data_X_trigs_range(data_in, 0, this->min_npts_max_cycles);
}
for(i=0; i<this->n_freqs; i++)
{
if(FI[i].npts_max_cycles==min_npts_max_cycles)
continue;
// have not quite finished a full cycle at this frequency yet...
beg = min_npts_max_cycles + 1;
end = FI[i].npts_max_cycles;
FI[i].data_X_trigs_range(data_in, beg, end);
}
for(i=0; i<this->n_freqs; i++)
{
FI[i].projection_fini();
}
// OK, have the projection of the data onto the various frequencies.
// Remove it!
for(i=0; i<npts; i++)
{
accum = 0.0;
for(j=0; j<this->n_freqs; j++)
{
accum += FI[j].projection_sin * FI[j].sin_vals[i] +
FI[j].projection_cos * FI[j].cos_vals[i];
}
data_out[i] = float( double(data_in[i]) - accum );
}
EXIT_SEQ:;
return rval;
}
File jack_interface.cpp
#include "jack_interface.h"
void jack_interface_shutdown (void *arg)
{
// Do nothing
return;
}
int jack_interface::create( char* interface_name,
unsigned int requested_sample_size,
int n_channels,
int (*process) (jack_nframes_t sample_size, void *arg) )
{
static char name[] = "jack_interface::create(...)";
int rval=0;
const char **ports;
static char default_interface_name[] = "simple";
const char *server_name = NULL;
jack_options_t options = JackNullOption;
jack_status_t status;
char *use_interface_name;
unsigned int use_sample_size;
static char IN1[] = "in1";
static char IN2[] = "in2";
static char IN3[] = "in3";
static char IN4[] = "in4";
static char OUT1[] = "out1";
static char OUT2[] = "out2";
static char OUT3[] = "out3";
static char OUT4[] = "out4";
char* names_in[max_channels];
char* names_out[max_channels];
int i;
// Validate (somewhat) args, etc.
if(interface_name==0 || interface_name[0]==0)
use_interface_name = default_interface_name;
else
use_interface_name = interface_name;
if(requested_sample_size==0)
use_sample_size=1024;
else
use_sample_size = requested_sample_size;
if(n_channels<1 || n_channels>max_channels)
{
printf("%s\nn_channels must be between 1 and %d.\n", name, max_channels);
rval = 10;
goto EXIT_SEQ;
}
if(process==0)
{
printf("%s\nprocess function is null.\n", name);
rval = 20;
goto EXIT_SEQ;
}
//--------------------------------------------------------
// open a client connection to the JACK server
jclient = jack_client_open (use_interface_name, options, &status, server_name);
if (jclient == NULL)
{
printf("%s\njack_client_open() failed, status = 0x%2.0x\n", name, status);
if (status & JackServerFailed)
{
printf ("Unable to connect to JACK server\n");
}
rval = 50;
goto EXIT_SEQ;
}
jack_set_buffer_size(jclient, use_sample_size);
sample_size = jack_get_buffer_size(jclient);
samps_per_sec = jack_get_sample_rate (jclient);
jack_set_process_callback (jclient, process, 0);
jack_on_shutdown (jclient, jack_interface_shutdown, 0);
names_in [0] = IN1;
names_in [1] = IN2;
names_in [2] = IN3;
names_in [3] = IN4;
names_out[0] = OUT1;
names_out[1] = OUT2;
names_out[2] = OUT3;
names_out[3] = OUT4;
for(i=0; i<n_channels; i++)
{
in_ports[i] = jack_port_register (jclient, names_in[i],
JACK_DEFAULT_AUDIO_TYPE,
JackPortIsInput, 0);
if(in_ports[i] == NULL)
{
printf("%s\nCannot create jack input port.\n", name);
rval = 100;
goto EXIT_SEQ;
}
}
for(i=0; i<n_channels; i++)
{
out_ports[i] = jack_port_register (jclient, names_out[i],
JACK_DEFAULT_AUDIO_TYPE,
JackPortIsOutput, 0);
if(out_ports[i] == NULL)
{
printf("%s\nCannot create jack output port.\n", name);
rval = 110;
goto EXIT_SEQ;
}
}
// Tell the JACK server to start. Our
// process() callback will start running now.
if (jack_activate (jclient))
{
printf ("%s\ncannot activate jack client\n", name);
rval = 120;
goto EXIT_SEQ;
}
// Connect the ports. You can't do this before the client is
// activated, because we can't make connections to clients
// that aren't running. Note the confusing (but necessary)
// orientation of the driver backend ports: playback ports are
// "input" to the backend, and capture ports are "output" from
// it.
ports = jack_get_ports (jclient, NULL, NULL, JackPortIsPhysical|JackPortIsOutput);
if (ports == NULL)
{
printf("%s\nno physical capture ports #1\n", name);
rval = 130;
goto EXIT_SEQ;
}
for(i=0; i<n_channels; i++)
{
if( jack_connect(jclient, ports[i], jack_port_name(in_ports[i]) ) )
{
printf("%s\nCannot connect input port # %d\n", name, i);
rval = 140;
goto EXIT_SEQ;
}
}
free (ports);
ports = jack_get_ports (jclient, NULL, NULL, JackPortIsPhysical|JackPortIsInput);
if (ports == NULL)
{
printf( "%s\nno physical playback ports\n", name);
rval = 150;
goto EXIT_SEQ;
}
for(i=0; i<n_channels; i++)
{
if( jack_connect(jclient, jack_port_name(out_ports[i]), ports[i]) )
{
printf("%s\nCannot connect output port # %d\n", name, i);
rval = 160;
goto EXIT_SEQ;
}
}
free (ports);
EXIT_SEQ:;
return rval;
}
File main.cpp
#include <stdio.h>
#include <errno.h>
#include <unistd.h>
#include <stdlib.h>
#include <jack/jack.h>
#include "jack_interface.h"
#include "freqs.h"
enum { nports = 2 };
static jack_port_t *in_ports [ nports ];
static jack_port_t *out_ports[ nports ];
static jack_interface JI;
static jack_port_t *input_portl;
static jack_port_t *input_portr;
static jack_port_t *output_portl;
static jack_port_t *output_portr;
static unsigned int samples_per_sec;
static unsigned int last_size = 0;
static unsigned int sample_size_out;
static freq_group FG;
int process (jack_nframes_t nframes, void *arg)
{
jack_default_audio_sample_t *inl, *inr, *outl, *outr;
input_portl = JI.in_ports[0];
input_portr = JI.in_ports[1];
output_portl= JI.out_ports[0];
output_portr= JI.out_ports[1];
inl = (float*) jack_port_get_buffer (input_portl, nframes);
inr = (float*) jack_port_get_buffer (input_portr, nframes);
outl = (float*) jack_port_get_buffer (output_portl, nframes);
outr = (float*) jack_port_get_buffer (output_portr, nframes);
FG.remove_projections1(last_l_data, outl, nframes);
memcpy (outr, last_r_data, sizeof (jack_default_audio_sample_t)*nframes);
return 0;
}
int main()
{
static char client_name[] = "filter5";
int rval=0;
int i;
enum { nfreqs = 50 }; // Will be filtering out 50 frequencies
double Freqs[nfreqs]; // The list of frequencies to eleminate
double freq_base = 60.0;
double this_freq;
FG.set_time_and_points(1.0/48000.0, 4096);
for(i=0; i<nfreqs; i++)
{
this_freq = double(2*i + 1) * freq_base;
Freqs[i] = this_freq;
}
FG.setup_freqs(Freqs, nfreqs);
rval = JI.create(client_name, 4096, 2, process);
sample_size_out = JI.sample_size;
samples_per_sec = JI.samps_per_sec;
sleep(-1);
return rval;
}