static void iir_filter_init(struct iir_data *iir,
float sample_rate, float freq, float qual, float gain, enum IIRFilter filter_type)
{
double omega = 2.0 * M_PI * freq / sample_rate;
double cs = cos(omega);
double sn = sin(omega);
double a1pha = sn / (2.0 * qual);
double A = exp(log(10.0) * gain / 40.0);
double beta = sqrt(A + A);
float b0 = 0.0, b1 = 0.0, b2 = 0.0, a0 = 0.0, a1 = 0.0, a2 = 0.0;
// Set up filter coefficients according to type
switch (filter_type)
{
case LPF:
b0 = (1.0 - cs) / 2.0;
b1 = 1.0 - cs ;
b2 = (1.0 - cs) / 2.0;
a0 = 1.0 + a1pha;
a1 = -2.0 * cs;
a2 = 1.0 - a1pha;
break;
case HPF:
b0 = (1.0 + cs) / 2.0;
b1 = -(1.0 + cs);
b2 = (1.0 + cs) / 2.0;
a0 = 1.0 + a1pha;
a1 = -2.0 * cs;
a2 = 1.0 - a1pha;
break;
case APF:
b0 = 1.0 - a1pha;
b1 = -2.0 * cs;
b2 = 1.0 + a1pha;
a0 = 1.0 + a1pha;
a1 = -2.0 * cs;
a2 = 1.0 - a1pha;
break;
case BPZPGF:
b0 = a1pha;
b1 = 0.0;
b2 = -a1pha;
a0 = 1.0 + a1pha;
a1 = -2.0 * cs;
a2 = 1.0 - a1pha;
break;
case BPCSGF:
b0 = sn / 2.0;
b1 = 0.0;
b2 = -sn / 2.0;
a0 = 1.0 + a1pha;
a1 = -2.0 * cs;
a2 = 1.0 - a1pha;
break;
case NOTCH:
b0 = 1.0;
b1 = -2.0 * cs;
b2 = 1.0;
a0 = 1.0 + a1pha;
a1 = -2.0 * cs;
a2 = 1.0 - a1pha;
break;
case RIAA_phono: /* http://www.dsprelated.com/showmessage/73300/3.php */
{
float b[3], a[3];
if ((int)sample_rate == 44100)
{
static const double zeros[] = {-0.2014898, 0.9233820};
static const double poles[] = {0.7083149, 0.9924091};
make_poly_from_roots(zeros, 2, b);
make_poly_from_roots(poles, 2, a);
}
else if ((int)sample_rate == 48000)
{
static const double zeros[] = {-0.1766069, 0.9321590};
static const double poles[] = {0.7396325, 0.9931330};
make_poly_from_roots(zeros, 2, b);
make_poly_from_roots(poles, 2, a);
}
else if ((int)sample_rate == 88200)
{
static const double zeros[] = {-0.1168735, 0.9648312};
static const double poles[] = {0.8590646, 0.9964002};
make_poly_from_roots(zeros, 2, b);
make_poly_from_roots(poles, 2, a);
}
else if ((int)sample_rate == 96000)
{
static const double zeros[] = {-0.1141486, 0.9676817};
static const double poles[] = {0.8699137, 0.9966946};
make_poly_from_roots(zeros, 2, b);
make_poly_from_roots(poles, 2, a);
}
b0 = b[0];
b1 = b[1];
b2 = b[2];
a0 = a[0];
a1 = a[1];
a2 = a[2];
/* Normalise to 0dB at 1kHz (Thanks to Glenn Davis) */
double y = 2.0 * M_PI * 1000.0 / sample_rate;
double b_re = b0 + b1 * cos(-y) + b2 * cos(-2.0 * y);
double a_re = a0 + a1 * cos(-y) + a2 * cos(-2.0 * y);
double b_im = b1 * sin(-y) + b2 * sin(-2.0 * y);
double a_im = a1 * sin(-y) + a2 * sin(-2.0 * y);
double g = 1.0 / sqrt((sqr(b_re) + sqr(b_im)) / (sqr(a_re) + sqr(a_im)));
b0 *= g; b1 *= g; b2 *= g;
break;
}
case PEQ:
b0 = 1.0 + a1pha * A;
b1 = -2.0 * cs;
b2 = 1.0 - a1pha * A;
a0 = 1.0 + a1pha / A;
a1 = -2.0 * cs;
a2 = 1.0 - a1pha / A;
break;
case BBOOST:
beta = sqrt((A * A + 1) / 1.0 - (pow((A - 1), 2)));
b0 = A * ((A + 1) - (A - 1) * cs + beta * sn);
b1 = 2 * A * ((A - 1) - (A + 1) * cs);
b2 = A * ((A + 1) - (A - 1) * cs - beta * sn);
a0 = ((A + 1) + (A - 1) * cs + beta * sn);
a1 = -2 * ((A - 1) + (A + 1) * cs);
a2 = (A + 1) + (A - 1) * cs - beta * sn;
break;
case LSH:
b0 = A * ((A + 1) - (A - 1) * cs + beta * sn);
b1 = 2 * A * ((A - 1) - (A + 1) * cs);
b2 = A * ((A + 1) - (A - 1) * cs - beta * sn);
a0 = (A + 1) + (A - 1) * cs + beta * sn;
a1 = -2 * ((A - 1) + (A + 1) * cs);
a2 = (A + 1) + (A - 1) * cs - beta * sn;
break;
case RIAA_CD:
omega = 2.0 * M_PI * 5283.0 / sample_rate;
cs = cos(omega);
sn = sin(omega);
a1pha = sn / (2.0 * 0.4845);
A = exp(log(10.0) * -9.477 / 40.0);
beta = sqrt(A + A);
case HSH:
b0 = A * ((A + 1.0) + (A - 1.0) * cs + beta * sn);
b1 = -2.0 * A * ((A - 1.0) + (A + 1.0) * cs);
b2 = A * ((A + 1.0) + (A - 1.0) * cs - beta * sn);
a0 = (A + 1.0) - (A - 1.0) * cs + beta * sn;
a1 = 2.0 * ((A - 1.0) - (A + 1.0) * cs);
a2 = (A + 1.0) - (A - 1.0) * cs - beta * sn;
break;
default:
break;
}
iir->b0 = b0;
iir->b1 = b1;
iir->b2 = b2;
iir->a0 = a0;
iir->a1 = a1;
iir->a2 = a2;
}
static int start(sox_effect_t * effp)
{
priv_t * p = (priv_t *)effp->priv;
double w0, A, alpha, mult;
if (p->filter_type == filter_deemph) { /* See deemph.plt for documentation */
if (effp->in_signal.rate == 44100) {
p->fc = 5283;
p->width = 0.4845;
p->gain = -9.477;
}
else if (effp->in_signal.rate == 48000) {
p->fc = 5356;
p->width = 0.479;
p->gain = -9.62;
}
else {
lsx_fail("sample rate must be 44100 (audio-CD) or 48000 (DAT)");
return SOX_EOF;
}
}
w0 = 2 * M_PI * p->fc / effp->in_signal.rate;
A = exp(p->gain / 40 * log(10.));
alpha = 0, mult = dB_to_linear(max(p->gain, 0));
if (w0 > M_PI) {
lsx_fail("frequency must be less than half the sample-rate (Nyquist rate)");
return SOX_EOF;
}
/* Set defaults: */
p->b0 = p->b1 = p->b2 = p->a1 = p->a2 = 0;
p->a0 = 1;
if (p->width) switch (p->width_type) {
case width_slope:
alpha = sin(w0)/2 * sqrt((A + 1/A)*(1/p->width - 1) + 2);
break;
case width_Q:
alpha = sin(w0)/(2*p->width);
break;
case width_bw_oct:
alpha = sin(w0)*sinh(log(2.)/2 * p->width * w0/sin(w0));
break;
case width_bw_Hz:
alpha = sin(w0)/(2*p->fc/p->width);
break;
case width_bw_kHz: assert(0); /* Shouldn't get here */
case width_bw_old:
alpha = tan(M_PI * p->width / effp->in_signal.rate);
break;
}
switch (p->filter_type) {
case filter_LPF: /* H(s) = 1 / (s^2 + s/Q + 1) */
p->b0 = (1 - cos(w0))/2;
p->b1 = 1 - cos(w0);
p->b2 = (1 - cos(w0))/2;
p->a0 = 1 + alpha;
p->a1 = -2*cos(w0);
p->a2 = 1 - alpha;
break;
case filter_HPF: /* H(s) = s^2 / (s^2 + s/Q + 1) */
p->b0 = (1 + cos(w0))/2;
p->b1 = -(1 + cos(w0));
p->b2 = (1 + cos(w0))/2;
p->a0 = 1 + alpha;
p->a1 = -2*cos(w0);
p->a2 = 1 - alpha;
break;
case filter_BPF_CSG: /* H(s) = s / (s^2 + s/Q + 1) (constant skirt gain, peak gain = Q) */
p->b0 = sin(w0)/2;
p->b1 = 0;
p->b2 = -sin(w0)/2;
p->a0 = 1 + alpha;
p->a1 = -2*cos(w0);
p->a2 = 1 - alpha;
break;
case filter_BPF: /* H(s) = (s/Q) / (s^2 + s/Q + 1) (constant 0 dB peak gain) */
p->b0 = alpha;
p->b1 = 0;
p->b2 = -alpha;
p->a0 = 1 + alpha;
p->a1 = -2*cos(w0);
p->a2 = 1 - alpha;
break;
case filter_notch: /* H(s) = (s^2 + 1) / (s^2 + s/Q + 1) */
p->b0 = 1;
p->b1 = -2*cos(w0);
p->b2 = 1;
p->a0 = 1 + alpha;
p->a1 = -2*cos(w0);
p->a2 = 1 - alpha;
break;
case filter_APF: /* H(s) = (s^2 - s/Q + 1) / (s^2 + s/Q + 1) */
p->b0 = 1 - alpha;
p->b1 = -2*cos(w0);
p->b2 = 1 + alpha;
p->a0 = 1 + alpha;
p->a1 = -2*cos(w0);
p->a2 = 1 - alpha;
break;
case filter_peakingEQ: /* H(s) = (s^2 + s*(A/Q) + 1) / (s^2 + s/(A*Q) + 1) */
if (A == 1)
return SOX_EFF_NULL;
p->b0 = 1 + alpha*A;
p->b1 = -2*cos(w0);
p->b2 = 1 - alpha*A;
p->a0 = 1 + alpha/A;
p->a1 = -2*cos(w0);
p->a2 = 1 - alpha/A;
break;
case filter_lowShelf: /* H(s) = A * (s^2 + (sqrt(A)/Q)*s + A)/(A*s^2 + (sqrt(A)/Q)*s + 1) */
if (A == 1)
return SOX_EFF_NULL;
p->b0 = A*( (A+1) - (A-1)*cos(w0) + 2*sqrt(A)*alpha );
p->b1 = 2*A*( (A-1) - (A+1)*cos(w0) );
p->b2 = A*( (A+1) - (A-1)*cos(w0) - 2*sqrt(A)*alpha );
p->a0 = (A+1) + (A-1)*cos(w0) + 2*sqrt(A)*alpha;
p->a1 = -2*( (A-1) + (A+1)*cos(w0) );
p->a2 = (A+1) + (A-1)*cos(w0) - 2*sqrt(A)*alpha;
break;
case filter_deemph: /* Falls through to high-shelf... */
case filter_highShelf: /* H(s) = A * (A*s^2 + (sqrt(A)/Q)*s + 1)/(s^2 + (sqrt(A)/Q)*s + A) */
if (!A)
return SOX_EFF_NULL;
p->b0 = A*( (A+1) + (A-1)*cos(w0) + 2*sqrt(A)*alpha );
p->b1 = -2*A*( (A-1) + (A+1)*cos(w0) );
p->b2 = A*( (A+1) + (A-1)*cos(w0) - 2*sqrt(A)*alpha );
p->a0 = (A+1) - (A-1)*cos(w0) + 2*sqrt(A)*alpha;
p->a1 = 2*( (A-1) - (A+1)*cos(w0) );
p->a2 = (A+1) - (A-1)*cos(w0) - 2*sqrt(A)*alpha;
break;
case filter_LPF_1: /* single-pole */
p->a1 = -exp(-w0);
p->b0 = 1 + p->a1;
break;
case filter_HPF_1: /* single-pole */
p->a1 = -exp(-w0);
p->b0 = (1 - p->a1)/2;
p->b1 = -p->b0;
break;
case filter_BPF_SPK: case filter_BPF_SPK_N: {
double bw_Hz;
if (!p->width)
p->width = p->fc / 2;
bw_Hz = p->width_type == width_Q? p->fc / p->width :
p->width_type == width_bw_Hz? p->width :
p->fc * (pow(2., p->width) - 1) * pow(2., -0.5 * p->width); /* bw_oct */
#include "band.h" /* Has different licence */
break;
}
case filter_AP1: /* Experimental 1-pole all-pass from Tom Erbe @ UCSD */
p->b0 = exp(-w0);
p->b1 = -1;
p->a1 = -exp(-w0);
break;
case filter_AP2: /* Experimental 2-pole all-pass from Tom Erbe @ UCSD */
p->b0 = 1 - sin(w0);
p->b1 = -2 * cos(w0);
p->b2 = 1 + sin(w0);
p->a0 = 1 + sin(w0);
p->a1 = -2 * cos(w0);
p->a2 = 1 - sin(w0);
break;
case filter_riaa: /* http://www.dsprelated.com/showmessage/73300/3.php */
if (effp->in_signal.rate == 44100) {
static const double zeros[] = {-0.2014898, 0.9233820};
static const double poles[] = {0.7083149, 0.9924091};
make_poly_from_roots(zeros, (size_t)2, &p->b0);
make_poly_from_roots(poles, (size_t)2, &p->a0);
}
else if (effp->in_signal.rate == 48000) {
static const double zeros[] = {-0.1766069, 0.9321590};
static const double poles[] = {0.7396325, 0.9931330};
make_poly_from_roots(zeros, (size_t)2, &p->b0);
make_poly_from_roots(poles, (size_t)2, &p->a0);
}
else if (effp->in_signal.rate == 88200) {
static const double zeros[] = {-0.1168735, 0.9648312};
static const double poles[] = {0.8590646, 0.9964002};
make_poly_from_roots(zeros, (size_t)2, &p->b0);
make_poly_from_roots(poles, (size_t)2, &p->a0);
}
else if (effp->in_signal.rate == 96000) {
static const double zeros[] = {-0.1141486, 0.9676817};
static const double poles[] = {0.8699137, 0.9966946};
make_poly_from_roots(zeros, (size_t)2, &p->b0);
make_poly_from_roots(poles, (size_t)2, &p->a0);
}
else {
lsx_fail("Sample rate must be 44.1k, 48k, 88.2k, or 96k");
return SOX_EOF;
}
{ /* Normalise to 0dB at 1kHz (Thanks to Glenn Davis) */
double y = 2 * M_PI * 1000 / effp->in_signal.rate;
double b_re = p->b0 + p->b1 * cos(-y) + p->b2 * cos(-2 * y);
double a_re = p->a0 + p->a1 * cos(-y) + p->a2 * cos(-2 * y);
double b_im = p->b1 * sin(-y) + p->b2 * sin(-2 * y);
double a_im = p->a1 * sin(-y) + p->a2 * sin(-2 * y);
double g = 1 / sqrt((sqr(b_re) + sqr(b_im)) / (sqr(a_re) + sqr(a_im)));
p->b0 *= g; p->b1 *= g; p->b2 *= g;
}
mult = (p->b0 + p->b1 + p->b2) / (p->a0 + p->a1 + p->a2);
lsx_debug("gain=%f", linear_to_dB(mult));
break;
}
if (effp->in_signal.mult)
*effp->in_signal.mult /= mult;
return lsx_biquad_start(effp);
}
