Eq.h

Go to the documentation of this file.

/*
        Eq.h
        
        Copyright 2004-7 Tim Goetze <tim@quitte.de>
        
        http://quitte.de/dsp/
 
        Equalizer circuit using recursive filtering.
        Based on a motorola paper implementing a similar circuit on a DSP56001.
 
*/
/*
        This program is free software; you can redistribute it and/or
        modify it under the terms of the GNU General Public License
        as published by the Free Software Foundation; either version 2
        of the License, or (at your option) any later version.
 
        This program is distributed in the hope that it will be useful,
        but WITHOUT ANY WARRANTY; without even the implied warranty of
        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
        GNU General Public License for more details.
 
        You should have received a copy of the GNU General Public License
        along with this program; if not, write to the Free Software
        Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
        02111-1307, USA or point your web browser to http://www.gnu.org.
*/
 
#ifndef _DSP_EQ_H_
#define _DSP_EQ_H_
 
namespace DSP {
 
/* A single bandpass as used by the Eq, expressed as a biquad. Like all
 * band-pass filters I know of, the filter works with a FIR coefficient of 0 
 * for x[-1], so a generic biquad isn't the optimum implementation. 
 *
 * This routine isn't used anywhere, just here for testing purposes.
 */
template <class T>
void
_BP (double fc, double Q, T * ca, T * cb)
{
        double theta = 2 * fc * M_PI;
 
        double 
                b = (Q - theta * .5) / (2 * Q + theta),
                a = (.5 - b) / 2,
                c = (.5 + b) * cos (theta);
 
        ca[0] = 2 * a;
        ca[1] = 0;
        ca[2] = -2 * a;
 
        cb[0] = 0;
        cb[1] = 2 * c;
        cb[2] = -2 * b;
}
 
template <int Bands, class eq_sample = float>
class Eq
{
        public:
                /* recursion coefficients, 3 per band */
                eq_sample a[Bands], b[Bands], c[Bands];
                /* past outputs, 2 per band */
                eq_sample y[2][Bands];
                /* current gain and recursion factor, each 1 per band = 2 */
                eq_sample gain[Bands], gf[Bands];
                /* input history */
                eq_sample x[2];
                /* history index */
                int h;
 
                eq_sample normal;
 
                Eq()
                        {
                                h = 0;
                                normal = NOISE_FLOOR;
                        }
 
                void reset()
                        {
                                for (int z = 0; z < 2; ++z)
                                {
                                        memset( y[z], 0, Bands*sizeof( eq_sample ) );
                                        x[z] = 0;
                                }
                        }
 
                void init (double fs, double Q)
                        {
                                double f = 31.25;
                                int i = 0;
 
                                for (i = 0; i < Bands && f < fs / 2; ++i, f *= 2)
                                        init_band (i, 2 * f * M_PI / fs, Q);
                                /* just in case, zero the remaining coefficients */
                                for (  ; i < Bands; ++i)
                                        zero_band (i);
 
                                reset();
                        }       
 
                void init_band (int i, double theta, double Q)
                        {
                                b[i] = (Q - theta * .5) / (2 * Q + theta);
                                a[i] = (.5 - b[i]) / 2;
                                c[i] = (.5 + b[i]) * cos (theta);
                                /* fprintf (stderr, "%02d %f %f %f\n", i, a[i], b[i], c[i]); */
                                gain[i] = 1;
                                gf[i] = 1;
                        }
 
                void zero_band (int i)
                        {
                                a[i] = b[i] = c[i] = 0;
                        }
 
                /* per-band recursion:
                 *      y = 2 * (a * (x - x[-2]) + c * y[-1] - b * y[-2]) 
                 */
                eq_sample process (eq_sample s)
                        {
                                int z1 = h, z2 = h ^ 1;
 
                                eq_sample * y1 = y[z1];
                                eq_sample * y2 = y[z2];
 
                                eq_sample x_x2 = s - x[z2];
                                eq_sample r = 0;
 
                                for (int i = 0; i < Bands; ++i)
                                {
                                        y2[i] = normal + 2 * (a[i] * x_x2 + c[i] * y1[i] - b[i] * y2[i]);
                                        r += gain[i] * y2[i];
                                        gain[i] *= gf[i];
                                }
 
                                x[z2] = s;
                                h = z2;
 
                                return r;
                        }
 
                /* zap denormals in history */
                void flush_0()
                        {
                                for (int i = 0; i < Bands; ++i)
                                        if (is_denormal (y[0][i]))
                                                y[0][i] = 0;
                        }
};
 
} /* namespace DSP */
 
#endif /* _DSP_EQ_H_ */