retronetworking
/
freeswitch
13
0
Fork 0
Code Pull Requests Releases Activity
freeswitch/libs/spandsp/src/filter_tools.c

244 lines
6.2 KiB
C
Raw Blame History

/*
* SpanDSP - a series of DSP components for telephony
*
* filter_tools.c - A collection of routines used for filter design.
*
* Written by Steve Underwood <steveu@coppice.org>
*
* Copyright (C) 2008 Steve Underwood
*
* This includes some elements based on the mkfilter package by
* A.J. Fisher, University of York <fisher@minster.york.ac.uk>, November 1996
*
* All rights reserved.
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License version 2.1,
* as published by the Free Software Foundation.
*
* 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this program; if not, write to the Free Software
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#if defined(HAVE_CONFIG_H)
#include "config.h"
#endif
#include <inttypes.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <time.h>
#include <fcntl.h>
#if defined(HAVE_TGMATH_H)
#include <tgmath.h>
#endif
#if defined(HAVE_MATH_H)
#include <math.h>
#endif
#if defined(HAVE_STDBOOL_H)
#include <stdbool.h>
#else
#include "spandsp/stdbool.h"
#endif
#include "floating_fudge.h"
#include "spandsp/telephony.h"
#include "spandsp/complex.h"
#include "filter_tools.h"
#define MAXPZ 8192
#define SEQ_LEN 8192
#define MAX_FFT_LEN SEQ_LEN
static complex_t circle[MAX_FFT_LEN/2];
static __inline__ complex_t expj(double theta)
{
return complex_set(cos(theta), sin(theta));
}
/*- End of function --------------------------------------------------------*/
static __inline__ double fix(double x)
{
/* Nearest integer */
return (x >= 0.0) ? floor(0.5 + x) : -floor(0.5 - x);
}
/*- End of function --------------------------------------------------------*/
static void fftx(complex_t data[], complex_t temp[], int n)
{
int i;
int h;
int p;
int t;
int i2;
complex_t wkt;
if (n > 1)
{
h = n/2;
for (i = 0; i < h; i++)
{
i2 = i*2;
temp[i] = data[i2]; /* Even */
temp[h + i] = data[i2 + 1]; /* Odd */
}
fftx(&temp[0], &data[0], h);
fftx(&temp[h], &data[h], h);
p = 0;
t = MAX_FFT_LEN/n;
for (i = 0; i < h; i++)
{
wkt = complex_mul(&circle[p], &temp[h + i]);
data[i] = complex_add(&temp[i], &wkt);
data[h + i] = complex_sub(&temp[i], &wkt);
p += t;
}
}
}
/*- End of function --------------------------------------------------------*/
void ifft(complex_t data[], int len)
{
int i;
double x;
complex_t temp[MAX_FFT_LEN];
/* A very slow and clunky FFT, that's just fine for filter design. */
for (i = 0; i < MAX_FFT_LEN/2; i++)
{
x = (2.0*3.1415926535*i)/(double) MAX_FFT_LEN;
circle[i] = expj(x);
}
fftx(data, temp, len);
}
/*- End of function --------------------------------------------------------*/
void compute_raised_cosine_filter(double coeffs[],
int len,
int root,
int sinc_compensate,
double alpha,
double beta)
{
double f;
double x;
double f1;
double f2;
double tau;
complex_t vec[SEQ_LEN];
int i;
int j;
int h;
f1 = (1.0 - beta)*alpha;
f2 = (1.0 + beta)*alpha;
tau = 0.5/alpha;
/* (Root) raised cosine */
for (i = 0; i <= SEQ_LEN/2; i++)
{
f = (double) i/(double) SEQ_LEN;
if (f <= f1)
vec[i] = complex_set(1.0, 0.0);
else if (f <= f2)
vec[i] = complex_set(0.5*(1.0 + cos((3.1415926535*tau/beta)*(f - f1))), 0.0);
else
vec[i] = complex_set(0.0, 0.0);
}
if (root)
{
for (i = 0; i <= SEQ_LEN/2; i++)
vec[i].re = sqrt(vec[i].re);
}
if (sinc_compensate)
{
for (i = 1; i <= SEQ_LEN/2; i++)
{
x = 3.1415926535*(double) i/(double) SEQ_LEN;
vec[i].re *= (x/sin(x));
}
}
for (i = 0; i <= SEQ_LEN/2; i++)
vec[i].re *= tau;
for (i = 1; i < SEQ_LEN/2; i++)
vec[SEQ_LEN - i] = vec[i];
ifft(vec, SEQ_LEN);
h = (len - 1)/2;
for (i = 0; i < len; i++)
{
j = (SEQ_LEN - h + i)%SEQ_LEN;
coeffs[i] = vec[j].re/(double) SEQ_LEN;
}
}
/*- End of function --------------------------------------------------------*/
void compute_hilbert_transform(double coeffs[], int len)
{
double x;
int i;
int h;
h = (len - 1)/2;
coeffs[h] = 0.0;
for (i = 1; i <= h; i++)
{
if ((i & 1))
{
x = 1.0/(double) i;
coeffs[h + i] = -x;
coeffs[h - i] = x;
}
else
{
coeffs[h + i] =
coeffs[h - i] = 0.0;
}
}
}
/*- End of function --------------------------------------------------------*/
void apply_hamming_window(double coeffs[], int len)
{
double w;
int i;
int h;
h = (len - 1)/2;
for (i = 1; i <= h; i++)
{
w = 0.53836 - 0.46164*cos(2.0*3.1415926535*(double) (h + i)/(double) (len - 1.0));
coeffs[h + i] *= w;
coeffs[h - i] *= w;
}
}
/*- End of function --------------------------------------------------------*/
void truncate_coeffs(double coeffs[], int len, int bits, int hilbert)
{
double x;
double fac;
double max;
double scale;
int h;
int i;
fac = pow(2.0, (double) (bits - 1.0));
h = (len - 1)/2;
max = (hilbert) ? coeffs[h - 1] : coeffs[h]; /* Max coeff */
scale = (fac - 1.0)/(fac*max);
for (i = 0; i < len; i++)
{
x = coeffs[i]*scale; /* Scale coeffs so max is (fac - 1.0)/fac */
coeffs[i] = fix(x*fac)/fac; /* Truncate */
}
}
/*- End of function --------------------------------------------------------*/
/*- End of file ------------------------------------------------------------*/
View Git Blame Copy Permalink