diff --git a/libaf/Makefile b/libaf/Makefile index 79ee144c76..2f716e22c2 100644 --- a/libaf/Makefile +++ b/libaf/Makefile @@ -2,7 +2,7 @@ include ../config.mak LIBNAME = libaf.a -SRCS=af.c af_mp.c af_dummy.c af_delay.c af_channels.c af_format.c af_resample.c window.c filter.c af_volume.c af_equalizer.c af_tools.c af_comp.c af_gate.c af_pan.c af_surround.c +SRCS=af.c af_mp.c af_dummy.c af_delay.c af_channels.c af_format.c af_resample.c window.c filter.c af_volume.c af_equalizer.c af_tools.c af_comp.c af_gate.c af_pan.c af_surround.c af_sub.c OBJS=$(SRCS:.c=.o) diff --git a/libaf/af.c b/libaf/af.c index f2bded6cb5..ca2dc7a75f 100644 --- a/libaf/af.c +++ b/libaf/af.c @@ -20,6 +20,7 @@ extern af_info_t af_info_gate; extern af_info_t af_info_comp; extern af_info_t af_info_pan; extern af_info_t af_info_surround; +extern af_info_t af_info_sub; static af_info_t* filter_list[]={ \ &af_info_dummy,\ @@ -33,6 +34,7 @@ static af_info_t* filter_list[]={ \ &af_info_comp,\ &af_info_pan,\ &af_info_surround,\ + &af_info_sub,\ NULL \ }; diff --git a/libaf/af_sub.c b/libaf/af_sub.c new file mode 100644 index 0000000000..f64c68c532 --- /dev/null +++ b/libaf/af_sub.c @@ -0,0 +1,181 @@ +/*============================================================================= +// +// This software has been released under the terms of the GNU Public +// license. See http://www.gnu.org/copyleft/gpl.html for details. +// +// Copyright 2002 Anders Johansson ajh@watri.uwa.edu.au +// +//============================================================================= +*/ + +/* This filter adds a sub-woofer channels to the audio stream by + averaging the left and right channel and low-pass filter them. The + low-pass filter is implemented as a 4th order IIR Butterworth + filter, with a variable cutoff frequency between 10 and 300 Hz. The + filter gives 24dB/octave attenuation. There are two runtime + controls one for setting which channel to insert the sub-audio into + called AF_CONTROL_SUB_CH and one for setting the cutoff frequency + called AF_CONTROL_SUB_FC. +*/ + +#include +#include +#include + +#include "af.h" +#include "dsp.h" + +// Q value for low-pass filter +#define Q 1.0 + +// Analog domain biquad section +typedef struct{ + float a[3]; // Numerator coefficients + float b[3]; // Denominator coefficients +} biquad_t; + +// S-parameters for designing 4th order Butterworth filter +static biquad_t sp[2] = {{{1.0,0.0,0.0},{1.0,0.765367,1.0}}, + {{1.0,0.0,0.0},{1.0,1.847759,1.0}}}; + +// Data for specific instances of this filter +typedef struct af_sub_s +{ + float w[2][4]; // Filter taps for low-pass filter + float q[2][2]; // Circular queues + float fc; // Cutoff frequency [Hz] for low-pass filter + float k; // Filter gain; + int ch; // Channel number which to insert the filtered data + +}af_sub_t; + +// Initialization and runtime control +static int control(struct af_instance_s* af, int cmd, void* arg) +{ + af_sub_t* s = af->setup; + + switch(cmd){ + case AF_CONTROL_REINIT:{ + // Sanity check + if(!arg) return AF_ERROR; + + af->data->rate = ((af_data_t*)arg)->rate; + af->data->nch = max(s->ch+1,((af_data_t*)arg)->nch); + af->data->format = AF_FORMAT_F | AF_FORMAT_NE; + af->data->bps = 4; + + // Design low-pass filter + s->k = 1.0; + if((-1 == szxform(sp[0].a, sp[0].b, Q, s->fc, + (float)af->data->rate, &s->k, s->w[0])) || + (-1 == szxform(sp[1].a, sp[1].b, Q, s->fc, + (float)af->data->rate, &s->k, s->w[1]))) + return AF_ERROR; + return af_test_output(af,(af_data_t*)arg); + } + case AF_CONTROL_COMMAND_LINE:{ + int ch=5; + float fc=60.0; + sscanf(arg,"%f:%i", &fc , &ch); + if(AF_OK != control(af,AF_CONTROL_SUB_CH | AF_CONTROL_SET, &ch)) + return AF_ERROR; + return control(af,AF_CONTROL_SUB_FC | AF_CONTROL_SET, &fc); + } + case AF_CONTROL_SUB_CH | AF_CONTROL_SET: // Requires reinit + // Sanity check + if((*(int*)arg >= AF_NCH) || (*(int*)arg < 0)){ + af_msg(AF_MSG_ERROR,"[sub] Subwoofer channel number must be between " + " 0 and %i current value is %i\n", AF_NCH-1, *(int*)arg); + return AF_ERROR; + } + s->ch = *(int*)arg; + return AF_OK; + case AF_CONTROL_SUB_CH | AF_CONTROL_GET: + *(int*)arg = s->ch; + return AF_OK; + case AF_CONTROL_SUB_FC | AF_CONTROL_SET: // Requires reinit + // Sanity check + if((*(float*)arg > 300) || (*(float*)arg < 20)){ + af_msg(AF_MSG_ERROR,"[sub] Cutoff frequency must be between 20Hz and" + " 300Hz current value is %0.2f",*(float*)arg); + return AF_ERROR; + } + // Set cutoff frequency + s->fc = *(float*)arg; + return AF_OK; + case AF_CONTROL_SUB_FC | AF_CONTROL_GET: + *(float*)arg = s->fc; + return AF_OK; + } + return AF_UNKNOWN; +} + +// Deallocate memory +static void uninit(struct af_instance_s* af) +{ + if(af->data) + free(af->data); + if(af->setup) + free(af->setup); +} + +#ifndef IIR +#define IIR(in,w,q,out) { \ + float h0 = (q)[0]; \ + float h1 = (q)[1]; \ + float hn = (in) - h0 * (w)[0] - h1 * (w)[1]; \ + out = hn + h0 * (w)[2] + h1 * (w)[3]; \ + (q)[1] = h0; \ + (q)[0] = hn; \ +} +#endif + +// Filter data through filter +static af_data_t* play(struct af_instance_s* af, af_data_t* data) +{ + af_data_t* c = data; // Current working data + af_sub_t* s = af->setup; // Setup for this instance + float* a = c->audio; // Audio data + int len = c->len/4; // Number of samples in current audio block + int nch = c->nch; // Number of channels + int ch = s->ch; // Channel in which to insert the sub audio + register int i; + + // Run filter + for(i=0;ik, s->w[0], s->q[0], x); + IIR(x , s->w[1], s->q[1], a[i+ch]); + } + + return c; +} + +// Allocate memory and set function pointers +static int open(af_instance_t* af){ + af_sub_t* s; + af->control=control; + af->uninit=uninit; + af->play=play; + af->mul.n=1; + af->mul.d=1; + af->data=calloc(1,sizeof(af_data_t)); + af->setup=s=calloc(1,sizeof(af_sub_t)); + if(af->data == NULL || af->setup == NULL) + return AF_ERROR; + // Set default values + s->ch = 5; // Channel nr 6 + s->fc = 60; // Cutoff frequency 60Hz + return AF_OK; +} + +// Description of this filter +af_info_t af_info_sub = { + "Audio filter for adding a sub-base channel", + "sub", + "Anders", + "", + AF_FLAGS_NOT_REENTRANT, + open +}; diff --git a/libaf/control.h b/libaf/control.h index c5468ad256..cc7a11486b 100644 --- a/libaf/control.h +++ b/libaf/control.h @@ -202,8 +202,17 @@ typedef struct af_control_ext_s{ #define AF_CONTROL_EQUALIZER_GAIN 0x00001C00 | AF_CONTROL_FILTER_SPECIFIC -// Set delay length in seconds +// Delay length in ms, arg is a control_ext with a float* #define AF_CONTROL_DELAY_LEN 0x00001D00 | AF_CONTROL_FILTER_SPECIFIC +// Subwoofer + +// Channel number which to insert the filtered data, arg in int* +#define AF_CONTROL_SUB_CH 0x00001E00 | AF_CONTROL_FILTER_SPECIFIC + +// Cutoff frequency [Hz] for lowpass filter, arg is float* +#define AF_CONTROL_SUB_FC 0x00001F00 | AF_CONTROL_FILTER_SPECIFIC + + #endif /*__af_control_h */ diff --git a/libaf/filter.c b/libaf/filter.c index 8d677f1e6d..526b00b244 100644 --- a/libaf/filter.c +++ b/libaf/filter.c @@ -14,6 +14,10 @@ #include #include "dsp.h" +/****************************************************************************** +* FIR filter implementations +******************************************************************************/ + /* C implementation of FIR filter y=w*x n number of filter taps, where mod(n,4)==0 @@ -73,6 +77,9 @@ inline int updatepq(unsigned int n, unsigned int d, unsigned int xi, _ftype_t** return (++xi)&(n-1); } +/****************************************************************************** +* FIR filter design +******************************************************************************/ /* Design FIR filter using the Window method @@ -255,3 +262,172 @@ int design_pfir(unsigned int n, unsigned int k, _ftype_t* w, _ftype_t** pw, _fty } return -1; } + +/****************************************************************************** +* IIR filter design +******************************************************************************/ + +/* Helper functions for the bilinear transform */ + +/* Pre-warp the coefficients of a numerator or denominator. + Note that a0 is assumed to be 1, so there is no wrapping + of it. +*/ +void prewarp(_ftype_t* a, _ftype_t fc, _ftype_t fs) +{ + _ftype_t wp; + wp = 2.0 * fs * tan(M_PI * fc / fs); + a[2] = a[2]/(wp * wp); + a[1] = a[1]/wp; +} + +/* Transform the numerator and denominator coefficients of s-domain + biquad section into corresponding z-domain coefficients. + + The transfer function for z-domain is: + + 1 + alpha1 * z^(-1) + alpha2 * z^(-2) + H(z) = ------------------------------------- + 1 + beta1 * z^(-1) + beta2 * z^(-2) + + Store the 4 IIR coefficients in array pointed by coef in following + order: + beta1, beta2 (denominator) + alpha1, alpha2 (numerator) + + Arguments: + a - s-domain numerator coefficients + b - s-domain denominator coefficients + k - filter gain factor. Initially set to 1 and modified by each + biquad section in such a way, as to make it the + coefficient by which to multiply the overall filter gain + in order to achieve a desired overall filter gain, + specified in initial value of k. + fs - sampling rate (Hz) + coef - array of z-domain coefficients to be filled in. + + Return: On return, set coef z-domain coefficients and k to the gain + required to maintain overall gain = 1.0; +*/ +void bilinear(_ftype_t* a, _ftype_t* b, _ftype_t* k, _ftype_t fs, _ftype_t *coef) +{ + _ftype_t ad, bd; + + /* alpha (Numerator in s-domain) */ + ad = 4. * a[2] * fs * fs + 2. * a[1] * fs + a[0]; + /* beta (Denominator in s-domain) */ + bd = 4. * b[2] * fs * fs + 2. * b[1] * fs + b[0]; + + /* Update gain constant for this section */ + *k *= ad/bd; + + /* Denominator */ + *coef++ = (2. * b[0] - 8. * b[2] * fs * fs)/bd; /* beta1 */ + *coef++ = (4. * b[2] * fs * fs - 2. * b[1] * fs + b[0])/bd; /* beta2 */ + + /* Numerator */ + *coef++ = (2. * a[0] - 8. * a[2] * fs * fs)/ad; /* alpha1 */ + *coef = (4. * a[2] * fs * fs - 2. * a[1] * fs + a[0])/ad; /* alpha2 */ +} + + + +/* IIR filter design using bilinear transform and prewarp. Transforms + 2nd order s domain analog filter into a digital IIR biquad link. To + create a filter fill in a, b, Q and fs and make space for coef and k. + + + Example Butterworth design: + + Below are Butterworth polynomials, arranged as a series of 2nd + order sections: + + Note: n is filter order. + + n Polynomials + ------------------------------------------------------------------- + 2 s^2 + 1.4142s + 1 + 4 (s^2 + 0.765367s + 1) * (s^2 + 1.847759s + 1) + 6 (s^2 + 0.5176387s + 1) * (s^2 + 1.414214 + 1) * (s^2 + 1.931852s + 1) + + For n=4 we have following equation for the filter transfer function: + 1 1 + T(s) = --------------------------- * ---------------------------- + s^2 + (1/Q) * 0.765367s + 1 s^2 + (1/Q) * 1.847759s + 1 + + The filter consists of two 2nd order sections since highest s power + is 2. Now we can take the coefficients, or the numbers by which s + is multiplied and plug them into a standard formula to be used by + bilinear transform. + + Our standard form for each 2nd order section is: + + a2 * s^2 + a1 * s + a0 + H(s) = ---------------------- + b2 * s^2 + b1 * s + b0 + + Note that Butterworth numerator is 1 for all filter sections, which + means s^2 = 0 and s^1 = 0 + + Lets convert standard Butterworth polynomials into this form: + + 0 + 0 + 1 0 + 0 + 1 + --------------------------- * -------------------------- + 1 + ((1/Q) * 0.765367) + 1 1 + ((1/Q) * 1.847759) + 1 + + Section 1: + a2 = 0; a1 = 0; a0 = 1; + b2 = 1; b1 = 0.765367; b0 = 1; + + Section 2: + a2 = 0; a1 = 0; a0 = 1; + b2 = 1; b1 = 1.847759; b0 = 1; + + Q is filter quality factor or resonance, in the range of 1 to + 1000. The overall filter Q is a product of all 2nd order stages. + For example, the 6th order filter (3 stages, or biquads) with + individual Q of 2 will have filter Q = 2 * 2 * 2 = 8. + + + Arguments: + a - s-domain numerator coefficients, a[1] is always assumed to be 1.0 + b - s-domain denominator coefficients + Q - Q value for the filter + k - filter gain factor. Initially set to 1 and modified by each + biquad section in such a way, as to make it the + coefficient by which to multiply the overall filter gain + in order to achieve a desired overall filter gain, + specified in initial value of k. + fs - sampling rate (Hz) + coef - array of z-domain coefficients to be filled in. + + Note: Upon return from each call, the k argument will be set to a + value, by which to multiply our actual signal in order for the gain + to be one. On second call to szxform() we provide k that was + changed by the previous section. During actual audio filtering + k can be used for gain compensation. + + return -1 if fail 0 if success. +*/ +int szxform(_ftype_t* a, _ftype_t* b, _ftype_t Q, _ftype_t fc, _ftype_t fs, _ftype_t *k, _ftype_t *coef) +{ + _ftype_t at[3]; + _ftype_t bt[3]; + + if(!a || !b || !k || !coef || (Q>1000.0 || Q< 1.0)) + return -1; + + memcpy(at,a,3*sizeof(_ftype_t)); + memcpy(bt,b,3*sizeof(_ftype_t)); + + bt[1]/=Q; + + /* Calculate a and b and overwrite the original values */ + prewarp(at, fc, fs); + prewarp(bt, fc, fs); + /* Execute bilinear transform */ + bilinear(at, bt, k, fs, coef); + + return 0; +} + diff --git a/libaf/filter.h b/libaf/filter.h index f36bc0bf2f..307760d856 100644 --- a/libaf/filter.h +++ b/libaf/filter.h @@ -45,14 +45,18 @@ // Exported functions extern _ftype_t fir(unsigned int n, _ftype_t* w, _ftype_t* x); + extern _ftype_t* pfir(unsigned int n, unsigned int k, unsigned int xi, _ftype_t** w, _ftype_t** x, _ftype_t* y, unsigned int s); extern int updateq(unsigned int n, unsigned int xi, _ftype_t* xq, _ftype_t* in); extern int updatepq(unsigned int n, unsigned int k, unsigned int xi, _ftype_t** xq, _ftype_t* in, unsigned int s); extern int design_fir(unsigned int n, _ftype_t* w, _ftype_t* fc, unsigned int flags, _ftype_t opt); + extern int design_pfir(unsigned int n, unsigned int k, _ftype_t* w, _ftype_t** pw, _ftype_t g, unsigned int flags); +extern int szxform(_ftype_t* a, _ftype_t* b, _ftype_t Q, _ftype_t fc, _ftype_t fs, _ftype_t *k, _ftype_t *coef); + /* Add new data to circular queue designed to be used with a FIR filter. xq is the circular queue, in pointing at the new sample, xi current index for xq and n the length of the filter. xq must be n*2