git.gag.com Git - fw/altos/blob - ao-tools/lib/cc-dsp.c

   1 /*
   2  * Copyright © 2009 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 #include "cc.h"
  20 #include "cephes.h"
  21 #include <math.h>
  22 #include <stdlib.h>
  23
  24 static inline double sqr (double x) { return x * x; }
  25
  26 /*
  27  * Kaiser Window digital filter
  28  */
  29
  30 #if 0
  31 /* not used in this program */
  32 static double highpass (double n, double m, double wc)
  33 {
  34         double  alpha = m/2;
  35         double  dist;
  36
  37         dist = n - alpha;
  38         if (dist == 0)
  39                 return (M_PI/2 - wc) / M_PI;
  40         return -sin(dist * (M_PI/2-wc)) / (M_PI * dist);
  41 }
  42 #endif
  43
  44 static double lowpass (double n, double m, double wc)
  45 {
  46         double  alpha = m/2;
  47         double  dist;
  48         dist = n - alpha;
  49         if (dist == 0)
  50                 return wc / M_PI;
  51         return sin (wc * dist) / (M_PI * dist);
  52 }
  53
  54 static double kaiser (double n, double m, double beta)
  55 {
  56         double alpha = m / 2;
  57         return i0 (beta * sqrt (1 - sqr((n - alpha) / alpha))) / i0(beta);
  58 }
  59
  60 static double beta (double A)
  61 {
  62         if (A > 50)
  63                 return 0.1102 * (A - 8.7);
  64         else if (A >= 21)
  65                 return 0.5842 * pow((A - 21), 0.4) + 0.07886 * (A - 21);
  66         else
  67                 return 0.0;
  68 }
  69
  70 static int M (double A, double delta_omega)
  71 {
  72         if (A > 21)
  73                 return ceil ((A - 7.95) / (2.285 * delta_omega));
  74         else
  75                 return ceil(5.79 / delta_omega);
  76 }
  77
  78 struct filter_param {
  79         double omega_pass;
  80         double delta_omega;
  81         double A;
  82         double beta;
  83         int M;
  84 } filter_param_t;
  85
  86 static struct filter_param
  87 filter (double omega_pass, double omega_stop, double error)
  88 {
  89         struct filter_param  p;
  90         p.omega_pass = omega_pass;
  91         p.delta_omega = omega_stop - omega_pass;
  92         p.A = -20 * log10 (error);
  93         p.beta = beta (p.A);
  94         p.M = M (p.A, p.delta_omega);
  95         if ((p.M & 1) == 1)
  96                 p.M++;
  97         return p;
  98 }
  99
 100 static double *
 101 make_low_pass_filter(double omega_pass, double omega_stop, double error, int *length_p)
 102 {
 103         struct filter_param     p = filter(omega_pass, omega_stop, error);
 104         int             length;
 105         int             n;
 106         double          *lpf;
 107
 108         length = p.M + 1;
 109         lpf = calloc (length, sizeof(double));
 110         for (n = 0; n < length; n++)
 111                 lpf[n] = lowpass(n, p.M, omega_pass) * kaiser(n, p.M, p.beta);
 112         *length_p = length;
 113         return lpf;
 114 }
 115
 116 static double *
 117 convolve(double *d, int d_len, double *e, int e_len)
 118 {
 119         int w = (e_len - 1) / 2;
 120         int n;
 121         double *con = calloc (d_len, sizeof (double));
 122
 123         for (n = 0; n < d_len; n++) {
 124                 double  v = 0;
 125                 int o;
 126                 for (o = -w; o <= w; o++) {
 127                         int     p = n + o;
 128                         double sample = p < 0 ? d[0] : p >= d_len ? d[d_len-1] : d[p];
 129                         v += sample * e[o + w];
 130                 }
 131                 con[n] = v;
 132         }
 133         return con;
 134 }
 135
 136 double *
 137 cc_low_pass(double *data, int data_len, double omega_pass, double omega_stop, double error)
 138 {
 139         int fir_len;
 140         double *fir = make_low_pass_filter(omega_pass, omega_stop, error, &fir_len);
 141         double *result;
 142
 143         result = convolve(data, data_len, fir, fir_len);
 144         free(fir);
 145         return result;
 146 }