src/biquad.c

   1 /* libSoX Biquad filter common functions   (c) 2006-7 robs@users.sourceforge.net
   2  *
   3  * This library is free software; you can redistribute it and/or modify it
   4  * under the terms of the GNU Lesser General Public License as published by
   5  * the Free Software Foundation; either version 2.1 of the License, or (at
   6  * your option) any later version.
   7  *
   8  * This library is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU Lesser General Public License
  14  * along with this library; if not, write to the Free Software Foundation,
  15  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
  16  */
  17
  18 #include "biquad.h"
  19 #include <string.h>
  20
  21 typedef biquad_t priv_t;
  22
  23 static char const * const width_str[] = {
  24   "band-width(Hz)",
  25   "band-width(kHz)",
  26   "band-width(Hz, no warp)", /* deprecated */
  27   "band-width(octaves)",
  28   "Q",
  29   "slope",
  30 };
  31 static char const all_width_types[] = "hkboqs";
  32
  33
  34 int lsx_biquad_getopts(sox_effect_t * effp, int argc, char **argv,
  35     int min_args, int max_args, int fc_pos, int width_pos, int gain_pos,
  36     char const * allowed_width_types, filter_t filter_type)
  37 {
  38   priv_t * p = (priv_t *)effp->priv;
  39   char width_type = *allowed_width_types;
  40   char dummy, * dummy_p;     /* To check for extraneous chars. */
  41   --argc, ++argv;
  42
  43   p->filter_type = filter_type;
  44   if (argc < min_args || argc > max_args ||
  45       (argc > fc_pos    && ((p->fc = lsx_parse_frequency(argv[fc_pos], &dummy_p)) <= 0 || *dummy_p)) ||
  46       (argc > width_pos && ((unsigned)(sscanf(argv[width_pos], "%lf%c %c", &p->width, &width_type, &dummy)-1) > 1 || p->width <= 0)) ||
  47       (argc > gain_pos  && sscanf(argv[gain_pos], "%lf %c", &p->gain, &dummy) != 1) ||
  48       !strchr(allowed_width_types, width_type) || (width_type == 's' && p->width > 1))
  49     return lsx_usage(effp);
  50   p->width_type = strchr(all_width_types, width_type) - all_width_types;
  51   if ((size_t)p->width_type >= strlen(all_width_types))
  52     p->width_type = 0;
  53   if (p->width_type == width_bw_kHz) {
  54     p->width *= 1000;
  55     p->width_type = width_bw_Hz;
  56   }
  57   return SOX_SUCCESS;
  58 }
  59
  60
  61 static int start(sox_effect_t * effp)
  62 {
  63   priv_t * p = (priv_t *)effp->priv;
  64   /* Simplify: */
  65   p->b2 /= p->a0;
  66   p->b1 /= p->a0;
  67   p->b0 /= p->a0;
  68   p->a2 /= p->a0;
  69   p->a1 /= p->a0;
  70
  71   p->o2 = p->o1 = p->i2 = p->i1 = 0;
  72   return SOX_SUCCESS;
  73 }
  74
  75
  76 int lsx_biquad_start(sox_effect_t * effp)
  77 {
  78   priv_t * p = (priv_t *)effp->priv;
  79
  80   start(effp);
  81
  82   if (effp->global_info->plot == sox_plot_octave) {
  83     printf(
  84       "%% GNU Octave file (may also work with MATLAB(R) )\n"
  85       "Fs=%g;minF=10;maxF=Fs/2;\n"
  86       "sweepF=logspace(log10(minF),log10(maxF),200);\n"
  87       "[h,w]=freqz([%.15e %.15e %.15e],[1 %.15e %.15e],sweepF,Fs);\n"
  88       "semilogx(w,20*log10(h))\n"
  89       "title('SoX effect: %s gain=%g frequency=%g %s=%g (rate=%g)')\n"
  90       "xlabel('Frequency (Hz)')\n"
  91       "ylabel('Amplitude Response (dB)')\n"
  92       "axis([minF maxF -35 25])\n"
  93       "grid on\n"
  94       "disp('Hit return to continue')\n"
  95       "pause\n"
  96       , effp->in_signal.rate, p->b0, p->b1, p->b2, p->a1, p->a2
  97       , effp->handler.name, p->gain, p->fc, width_str[p->width_type], p->width
  98       , effp->in_signal.rate);
  99     return SOX_EOF;
 100   }
 101   if (effp->global_info->plot == sox_plot_gnuplot) {
 102     printf(
 103       "# gnuplot file\n"
 104       "set title 'SoX effect: %s gain=%g frequency=%g %s=%g (rate=%g)'\n"
 105       "set xlabel 'Frequency (Hz)'\n"
 106       "set ylabel 'Amplitude Response (dB)'\n"
 107       "Fs=%g\n"
 108       "b0=%.15e; b1=%.15e; b2=%.15e; a1=%.15e; a2=%.15e\n"
 109       "o=2*pi/Fs\n"
 110       "H(f)=sqrt((b0*b0+b1*b1+b2*b2+2.*(b0*b1+b1*b2)*cos(f*o)+2.*(b0*b2)*cos(2.*f*o))/(1.+a1*a1+a2*a2+2.*(a1+a1*a2)*cos(f*o)+2.*a2*cos(2.*f*o)))\n"
 111       "set logscale x\n"
 112       "set samples 250\n"
 113       "set grid xtics ytics\n"
 114       "set key off\n"
 115       "plot [f=10:Fs/2] [-35:25] 20*log10(H(f))\n"
 116       "pause -1 'Hit return to continue'\n"
 117       , effp->handler.name, p->gain, p->fc, width_str[p->width_type], p->width
 118       , effp->in_signal.rate, effp->in_signal.rate
 119       , p->b0, p->b1, p->b2, p->a1, p->a2);
 120     return SOX_EOF;
 121   }
 122   if (effp->global_info->plot == sox_plot_data) {
 123     printf("# SoX effect: %s gain=%g frequency=%g %s=%g (rate=%g)\n"
 124       "# IIR filter\n"
 125       "# rate: %g\n"
 126       "# name: b\n"
 127       "# type: matrix\n"
 128       "# rows: 3\n"
 129       "# columns: 1\n"
 130       "%24.16e\n%24.16e\n%24.16e\n"
 131       "# name: a\n"
 132       "# type: matrix\n"
 133       "# rows: 3\n"
 134       "# columns: 1\n"
 135       "%24.16e\n%24.16e\n%24.16e\n"
 136       , effp->handler.name, p->gain, p->fc, width_str[p->width_type], p->width
 137       , effp->in_signal.rate, effp->in_signal.rate
 138       , p->b0, p->b1, p->b2, 1. /* a0 */, p->a1, p->a2);
 139     return SOX_EOF;
 140   }
 141   return SOX_SUCCESS;
 142 }
 143
 144
 145 int lsx_biquad_flow(sox_effect_t * effp, const sox_sample_t *ibuf,
 146     sox_sample_t *obuf, size_t *isamp, size_t *osamp)
 147 {
 148   priv_t * p = (priv_t *)effp->priv;
 149   size_t len = *isamp = *osamp = min(*isamp, *osamp);
 150   while (len--) {
 151     double o0 = *ibuf*p->b0 + p->i1*p->b1 + p->i2*p->b2 - p->o1*p->a1 - p->o2*p->a2;
 152     p->i2 = p->i1, p->i1 = *ibuf++;
 153     p->o2 = p->o1, p->o1 = o0;
 154     *obuf++ = SOX_ROUND_CLIP_COUNT(o0, effp->clips);
 155   }
 156   return SOX_SUCCESS;
 157 }
 158
 159 static int create(sox_effect_t * effp, int argc, char * * argv)
 160 {
 161   priv_t             * p = (priv_t *)effp->priv;
 162   double             * d = &p->b0;
 163   char               c;
 164
 165   --argc, ++argv;
 166   if (argc == 6)
 167     for (; argc && sscanf(*argv, "%lf%c", d, &c) == 1; --argc, ++argv, ++d);
 168   return argc? lsx_usage(effp) : SOX_SUCCESS;
 169 }
 170
 171 sox_effect_handler_t const * lsx_biquad_effect_fn(void)
 172 {
 173   static sox_effect_handler_t handler = {
 174     "biquad", "b0 b1 b2 a0 a1 a2", 0,
 175     create, lsx_biquad_start, lsx_biquad_flow, NULL, NULL, NULL, sizeof(priv_t)
 176   };
 177   return &handler;
 178 }