jens
0179c510fe
git-svn-id: http://moon:8086/svn/software/trunk/libsrc/fir@1 b431acfa-c32f-4a4a-93f1-934dc6c82436
354 lines
7.9 KiB
C
Executable File
354 lines
7.9 KiB
C
Executable File
/*************************************************************************/
|
|
/* fir.c */
|
|
/*************************************************************************/
|
|
#include <stdio.h>
|
|
#include <malloc.h>
|
|
#include <math.h>
|
|
#include <float.h>
|
|
|
|
#include "fir2.h"
|
|
|
|
|
|
/*************************************************************************/
|
|
/* Global Variables */
|
|
/*************************************************************************/
|
|
void CalcSincFilter(fir_float_t *pY, fir_float_t s, fir_float_t f, int len)
|
|
{
|
|
int i;
|
|
fir_float_t x;
|
|
fir_float_t off = ((fir_float_t)len-1)/2;
|
|
|
|
for (i=0; i < len; i++)
|
|
{
|
|
x = (fir_float_t)(f*(i-off));
|
|
pY[i] = s*Sinc(x);
|
|
}
|
|
}
|
|
|
|
fir_float_t Sinc(fir_float_t x)
|
|
{
|
|
if (x == 0)
|
|
return 1.0;
|
|
|
|
return (fir_float_t)(sin(fir_pi*x)/(fir_pi*x));
|
|
}
|
|
|
|
void FIRCalcLowpass(fir_float_t omega, fir_float_t *pCoeff, int N)
|
|
{
|
|
CalcSincFilter(pCoeff, omega, omega, N);
|
|
CalcKaiser(pCoeff, pCoeff, 8.0, N);
|
|
}
|
|
|
|
void FIRCalcHighpass(fir_float_t omega, fir_float_t *pCoeff, int N)
|
|
{
|
|
int i;
|
|
|
|
CalcSincFilter(pCoeff, omega, omega, N);
|
|
|
|
for (i=0; i < N; i++)
|
|
{
|
|
pCoeff[i] = -pCoeff[i];
|
|
}
|
|
pCoeff[(N-1)/2] = 1 + pCoeff[(N-1)/2];
|
|
|
|
CalcKaiser(pCoeff, pCoeff, 8.0, N);
|
|
}
|
|
|
|
void FIRCalcBandpass(fir_float_t omega, fir_float_t bw, fir_float_t *pCoeff, int N)
|
|
{
|
|
int i;
|
|
|
|
CalcSincFilter(pCoeff, bw, bw, N);
|
|
|
|
for (i=0; i < N; i++)
|
|
{
|
|
pCoeff[i] *= (fir_float_t)cos(2*fir_pi*omega*i);
|
|
}
|
|
CalcKaiser(pCoeff, pCoeff, 8.0, N);
|
|
}
|
|
|
|
fir_float_t CalcFirRC(fir_float_t *pB, fir_float_t fa, fir_float_t Tsym, fir_float_t Alpha, int N)
|
|
{
|
|
int n, delay;
|
|
fir_float_t term, k, k0, phi;
|
|
|
|
if (N%2)
|
|
delay = (N-1)/2;
|
|
else
|
|
delay = N/2;
|
|
|
|
k = (fir_float_t)2.0/Tsym;
|
|
k0 = (fir_float_t)0.5*Tsym*fa;
|
|
for (n=0; n < N; n++)
|
|
{
|
|
phi = (n-delay)/fa;
|
|
if (fabs(fabs(4*Alpha*phi/Tsym) - 1.0) > sqrt(DBL_EPSILON))
|
|
{
|
|
term = (fir_float_t)4.*Alpha*phi/Tsym;
|
|
pB[n] = Sinc(2*phi/Tsym)/fa * (fir_float_t)cos(2*fir_pi*Alpha*phi/Tsym) /((fir_float_t)1.0 - term*term);
|
|
}
|
|
else
|
|
{
|
|
pB[n] = Alpha * (fir_float_t)sin(fir_pi/(2*Alpha)) /(2*fa);
|
|
}
|
|
pB[n] *= k;
|
|
}
|
|
return k0;
|
|
}
|
|
|
|
fir_float_t CalcFirSRRC(fir_float_t *pB, fir_float_t fa, fir_float_t Tsym, fir_float_t Alpha, int N)
|
|
{
|
|
int n, delay;
|
|
fir_float_t term, k, k0, phi;
|
|
|
|
if (N%2)
|
|
delay = (N-1)/2;
|
|
else
|
|
delay = N/2;
|
|
|
|
k = (fir_float_t)sqrt(2.0/Tsym);
|
|
k0 = (fir_float_t)0.5*Tsym*fa;
|
|
for (n=0; n < N; n++)
|
|
{
|
|
phi = (n-delay)/fa;
|
|
if (phi == 0.0)
|
|
{
|
|
pB[n] = (fir_float_t)(-k * (fir_pi*(Alpha-1.0) - 4*Alpha) /(fir_pi*fa));
|
|
}
|
|
else
|
|
{
|
|
if (fabs(fabs(8*Alpha*phi/Tsym) - 1.0) < sqrt(DBL_EPSILON))
|
|
{
|
|
pB[n] = (fir_float_t)(k / (2*fir_pi*fa) \
|
|
* (fir_pi*(Alpha+1.0) * sin(fir_pi*(Alpha+1.0)/(4*Alpha)) \
|
|
- 4*Alpha * sin(fir_pi*(Alpha-1.0)/(4*Alpha)) \
|
|
+ fir_pi*(Alpha-1.0) * cos(fir_pi*(Alpha-1.0)/(4*Alpha))));
|
|
|
|
}
|
|
else
|
|
{
|
|
term = 8*Alpha*phi/Tsym;
|
|
pB[n] = (fir_float_t)(-4*Alpha/fa * ( cos((1.0+Alpha)*2*fir_pi*phi/Tsym) \
|
|
+ sin((1.0-Alpha)*2*fir_pi*phi/Tsym) / (8*Alpha*phi/Tsym)) \
|
|
/ (fir_pi * sqrt(1.0/(2/Tsym)) * (term*term - 1.0)));
|
|
|
|
}
|
|
}
|
|
pB[n] *= k;
|
|
}
|
|
|
|
return k0;
|
|
}
|
|
|
|
// Hamming
|
|
// 2*pi*k
|
|
// w(k) = 0.54 - 0.46*cos(------), where 0 <= k < N
|
|
// N-1
|
|
//
|
|
// len: window length
|
|
// pX: Input buffer (in)
|
|
// pY: Window weighted input buffer y[n] = x[n] * w[n] (out)
|
|
void CalcHamming(fir_float_t *pX, fir_float_t *pY, int len)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i < len; i++)
|
|
pY[i] = (fir_float_t)(pX[i]*(0.54-0.46*cos(2*fir_pi*i/(len-1))));
|
|
|
|
}
|
|
|
|
// Hanning
|
|
// 2*pi*k
|
|
// w = 0.5 - 0.5*cos(------), where 0 < k <= N
|
|
// N+1
|
|
// len: window length
|
|
// pX: Input buffer (in)
|
|
// pY: Window weighted input buffer y[n] = x[n] * w[n] (out)
|
|
void CalcVonHann(fir_float_t *pX, fir_float_t *pY, int len)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i < len; i++)
|
|
pY[i] = (fir_float_t)(0.5*pX[i]*(1.0-cos(2*fir_pi*i/(len-1))));
|
|
|
|
}
|
|
|
|
// Blackman
|
|
// 2*pi*k 4*pi*k
|
|
// w(k) = 0.42 - 0.5*cos(------) + 0.08*cos(------), where 0 <= k < N
|
|
// N-1 N-1
|
|
//
|
|
// len: window length
|
|
// pX: Input buffer (in)
|
|
// pY: Window weighted input buffer y[n] = x[n] * w[n] (out)
|
|
void CalcBlackman(fir_float_t *pX, fir_float_t *pY, int len)
|
|
{
|
|
int i;
|
|
|
|
for (i=0; i < len; i++)
|
|
pY[i] = (fir_float_t)(pX[i] * (0.42 - 0.5*cos(2*fir_pi*i/(len-1)) + 0.08*cos(4*fir_pi*i/(len-1))));
|
|
|
|
}
|
|
|
|
// Computes the 0th order modified Bessel function of the first kind.
|
|
// (Needed to compute Kaiser window)
|
|
//
|
|
// y = sum( (x/(2*n))^2 )
|
|
// n
|
|
//
|
|
#define BIZ_EPSILON 1E-11 // Max error acceptable
|
|
|
|
double besselizero(double x)
|
|
{
|
|
double temp;
|
|
double sum = 1.0;
|
|
double u = 1.0;
|
|
double halfx = (double)(x/2.0);
|
|
int n = 1;
|
|
|
|
do
|
|
{
|
|
temp = halfx/(double)n;
|
|
u *=temp * temp;
|
|
sum += u;
|
|
n++;
|
|
} while (u >= BIZ_EPSILON * sum);
|
|
|
|
return(sum);
|
|
}
|
|
|
|
|
|
// Kaiser
|
|
//
|
|
// n window length
|
|
// w buffer for the window parameters
|
|
// b beta parameter of Kaiser window, Beta >= 1
|
|
//
|
|
// Beta trades the rejection of the low pass filter against the
|
|
// transition width from passband to stop band. Larger Beta means a
|
|
// slower transition and greater stop band rejection. See Rabiner and
|
|
// Gold (Theory and Application of DSP) under Kaiser windows for more
|
|
// about Beta. The following table from Rabiner and Gold gives some
|
|
// feel for the effect of Beta:
|
|
//
|
|
// All ripples in dB, width of transition band = D*N where N = window
|
|
// length
|
|
//
|
|
// BETA D PB RIP SB RIP
|
|
// 2.120 1.50 +-0.27 -30
|
|
// 3.384 2.23 0.0864 -40
|
|
// 4.538 2.93 0.0274 -50
|
|
// 5.658 3.62 0.00868 -60
|
|
// 6.764 4.32 0.00275 -70
|
|
// 7.865 5.0 0.000868 -80
|
|
// 8.960 5.7 0.000275 -90
|
|
// 10.056 6.4 0.000087 -100
|
|
void CalcKaiser(fir_float_t *pX, fir_float_t *pY, fir_float_t b, int len)
|
|
{
|
|
double tmp, tmp2, *pW;
|
|
double k1 = 1.0/besselizero(b);
|
|
int k2 = 1 - (len & 1);
|
|
int end = (len + 1) >> 1;
|
|
int i;
|
|
|
|
pW = (double*)malloc(len*sizeof(double));
|
|
|
|
// Calculate window coefficients
|
|
for (i=0 ; i<end ; i++)
|
|
{
|
|
tmp = (double)(2*i + k2) / ((double)len - 1.0);
|
|
tmp2 = k1 * besselizero(b*sqrt(1.0 - tmp*tmp));
|
|
pW[end-(1&(!k2))+i] = tmp2;
|
|
pW[end-1-i] = tmp2;
|
|
}
|
|
|
|
if (pX)
|
|
{
|
|
for (i=0; i < len; i++)
|
|
pY[i] = (fir_float_t)(pW[i] * pX[i]);
|
|
}
|
|
else
|
|
{
|
|
for (i=0; i < len; i++)
|
|
pY[i] = (fir_float_t)pW[i];
|
|
}
|
|
free(pW);
|
|
}
|
|
|
|
void CalcKaiserSR(fir_float_t *pX, fir_float_t *pY, fir_float_t b, int len)
|
|
{
|
|
double tmp, tmp2, *pW;
|
|
double k1 = 1.0/besselizero(b);
|
|
int k2 = 1 - (len & 1);
|
|
int end = (len + 1) >> 1;
|
|
int i;
|
|
|
|
pW = (double*)malloc(len*sizeof(double));
|
|
|
|
// Calculate window coefficients
|
|
for (i=0 ; i<end ; i++)
|
|
{
|
|
tmp = (double)(2*i + k2) / ((double)len - 1.0);
|
|
tmp2 = k1 * besselizero(b*sqrt(1.0 - tmp*tmp));
|
|
pW[end-(1&(!k2))+i] = tmp2;
|
|
pW[end-1-i] = tmp2;
|
|
}
|
|
|
|
if (pX)
|
|
{
|
|
for (i=0; i < len; i++)
|
|
pY[i] = (fir_float_t)((fir_float_t)pow(pW[i], 0.5) * pX[i]);
|
|
}
|
|
else
|
|
{
|
|
for (i=0; i < len; i++)
|
|
pY[i] = (fir_float_t)(fir_float_t)pow(pW[i], 0.5);
|
|
}
|
|
free(pW);
|
|
}
|
|
|
|
void FIR(fir_float_t *pCoeff, fir_float_t *pState, int order, fir_float_t *x, fir_float_t *y, int len)
|
|
{
|
|
int i, n;
|
|
fir_float_t *pB, *pS;
|
|
|
|
for (i=0; i<len; i++)
|
|
{
|
|
pB = &pCoeff[order-1];
|
|
pS = &pState[order-1];
|
|
|
|
y[i] = 0;
|
|
for (n=0; n < order; n++)
|
|
{
|
|
if (pS != &pState[0])
|
|
*pS = *(pS-1);
|
|
else
|
|
*pS = x[i];
|
|
|
|
y[i] += *(pB--) * *(pS--);
|
|
}
|
|
}
|
|
}
|
|
|
|
void FIRneu(fir_float_t *pCoeff, fir_float_t *pState, int order, fir_float_t *x, fir_float_t *y, int len)
|
|
{
|
|
int i, n;
|
|
fir_float_t *pB, *pS, t;
|
|
|
|
for (i=0; i<len; i++)
|
|
{
|
|
pB = &pCoeff[order-1];
|
|
pS = &pState[order-1];
|
|
|
|
t = 0;
|
|
*pS = x[i];
|
|
for (n=0; n < order; n++)
|
|
{
|
|
t += *(pB--) * *(pS--);
|
|
*pS = *(pS-1);
|
|
}
|
|
y[i] = t;
|
|
}
|
|
}
|
|
|