147 lines
3.3 KiB
C
147 lines
3.3 KiB
C
/*
|
|
* Copyright © 2009 Keith Packard <keithp@keithp.com>
|
|
*
|
|
* 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.
|
|
*/
|
|
|
|
#include "cc.h"
|
|
#include "cephes.h"
|
|
#include <math.h>
|
|
#include <stdlib.h>
|
|
|
|
static inline double sqr (double x) { return x * x; }
|
|
|
|
/*
|
|
* Kaiser Window digital filter
|
|
*/
|
|
|
|
#if 0
|
|
/* not used in this program */
|
|
static double highpass (double n, double m, double wc)
|
|
{
|
|
double alpha = m/2;
|
|
double dist;
|
|
|
|
dist = n - alpha;
|
|
if (dist == 0)
|
|
return (M_PI/2 - wc) / M_PI;
|
|
return -sin(dist * (M_PI/2-wc)) / (M_PI * dist);
|
|
}
|
|
#endif
|
|
|
|
static double lowpass (double n, double m, double wc)
|
|
{
|
|
double alpha = m/2;
|
|
double dist;
|
|
dist = n - alpha;
|
|
if (dist == 0)
|
|
return wc / M_PI;
|
|
return sin (wc * dist) / (M_PI * dist);
|
|
}
|
|
|
|
static double kaiser (double n, double m, double beta)
|
|
{
|
|
double alpha = m / 2;
|
|
return i0 (beta * sqrt (1 - sqr((n - alpha) / alpha))) / i0(beta);
|
|
}
|
|
|
|
static double beta (double A)
|
|
{
|
|
if (A > 50)
|
|
return 0.1102 * (A - 8.7);
|
|
else if (A >= 21)
|
|
return 0.5842 * pow((A - 21), 0.4) + 0.07886 * (A - 21);
|
|
else
|
|
return 0.0;
|
|
}
|
|
|
|
static int M (double A, double delta_omega)
|
|
{
|
|
if (A > 21)
|
|
return ceil ((A - 7.95) / (2.285 * delta_omega));
|
|
else
|
|
return ceil(5.79 / delta_omega);
|
|
}
|
|
|
|
struct filter_param {
|
|
double omega_pass;
|
|
double delta_omega;
|
|
double A;
|
|
double beta;
|
|
int M;
|
|
} filter_param_t;
|
|
|
|
static struct filter_param
|
|
filter (double omega_pass, double omega_stop, double error)
|
|
{
|
|
struct filter_param p;
|
|
p.omega_pass = omega_pass;
|
|
p.delta_omega = omega_stop - omega_pass;
|
|
p.A = -20 * log10 (error);
|
|
p.beta = beta (p.A);
|
|
p.M = M (p.A, p.delta_omega);
|
|
if ((p.M & 1) == 1)
|
|
p.M++;
|
|
return p;
|
|
}
|
|
|
|
static double *
|
|
make_low_pass_filter(double omega_pass, double omega_stop, double error, int *length_p)
|
|
{
|
|
struct filter_param p = filter(omega_pass, omega_stop, error);
|
|
int length;
|
|
int n;
|
|
double *lpf;
|
|
|
|
length = p.M + 1;
|
|
lpf = calloc (length, sizeof(double));
|
|
for (n = 0; n < length; n++)
|
|
lpf[n] = lowpass(n, p.M, omega_pass) * kaiser(n, p.M, p.beta);
|
|
*length_p = length;
|
|
return lpf;
|
|
}
|
|
|
|
static double *
|
|
convolve(double *d, int d_len, double *e, int e_len)
|
|
{
|
|
int w = (e_len - 1) / 2;
|
|
int n;
|
|
double *con = calloc (d_len, sizeof (double));
|
|
|
|
for (n = 0; n < d_len; n++) {
|
|
double v = 0;
|
|
int o;
|
|
for (o = -w; o <= w; o++) {
|
|
int p = n + o;
|
|
double sample = p < 0 ? d[0] : p >= d_len ? d[d_len-1] : d[p];
|
|
v += sample * e[o + w];
|
|
}
|
|
con[n] = v;
|
|
}
|
|
return con;
|
|
}
|
|
|
|
double *
|
|
cc_low_pass(double *data, int data_len, double omega_pass, double omega_stop, double error)
|
|
{
|
|
int fir_len;
|
|
double *fir = make_low_pass_filter(omega_pass, omega_stop, error, &fir_len);
|
|
double *result;
|
|
|
|
result = convolve(data, data_len, fir, fir_len);
|
|
free(fir);
|
|
return result;
|
|
}
|