jens/fir
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial import

Browse Source 
git-svn-id: http://moon:8086/svn/software/trunk/libsrc/fir@1 b431acfa-c32f-4a4a-93f1-934dc6c82436
This commit is contained in:
jens
2014-07-19 07:44:42 +00:00
commit 0179c510fe
12 changed files with 1436 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
fir.c
Executable
+94
View File
@@ -0,0 +1,94 @@
/*************************************************************************/
/* fir.c
/*************************************************************************/
#include <stdio.h>
#include <malloc.h>
#include "math.h"
#include "fir.h"
/*************************************************************************/
/* Global Variables
/*************************************************************************/
void Sinc(float *pY, int M, float s, float f, int len)
{
int i, m;
float x,h;
m = (len-1)/2;
for (i=1; i <= m; i++)
{
x = (float)(pi*f*i);
h = (float)cos(i*0.5*pi/m);
pY[m+i] = h*s*(float)sin(x)/x;
pY[m-i] = pY[m+i];
}
pY[m] = s;
}
void FIRCalcDCRemovalCoeff(struct _sFIRCoeff *pCoeff, int N)
{
int n;
float k = 1.f/N;
FIRInit(pCoeff, 0, N);
for (n=0; n < N; n++)
pCoeff->pB[n] = -k;
pCoeff->pB[0] += 1.f;
}
void FIR(struct _sFIRCoeff *pCoeff, float *xn, float *yn, int order, int len)
{
int i, n;
float *pB, *pX;
for (i=0; i<len; i++)
{
pB = &pCoeff->pB[order-1];
pX = &pCoeff->pX[order-1];
pCoeff->pX[0] = xn[i];
yn[i] = 0;
for (n=0; n < order; n++)
{
yn[i] += *(pB--) * *pX;
*(pX) = *(pX-1);
pX--;
}
}
}
void FIRInit(struct _sFIRCoeff *pCoeff, float *pB, int order)
{
int n;
if (pB)
{
for(n=0; n < order; n++)
{
pCoeff->pB[n] = pB[n];
pCoeff->pX[n] = 0;
}
}
else
{
for(n=0; n < order; n++)
{
pCoeff->pB[n] = 0;
pCoeff->pX[n] = 0;
}
}
}
void FIRAlloc(struct _sFIRCoeff *pCoeff, int order)
{
pCoeff->pB = malloc(order*sizeof(float));
pCoeff->pX = malloc(order*sizeof(float));
}
void FIRFree(struct _sFIRCoeff *pCoeff)
{
free(pCoeff->pB);
free(pCoeff->pX);
}
fir.h
Executable
+37
View File
@@ -0,0 +1,37 @@
/******************************************************************************/
/* iir.h
/******************************************************************************/
#ifndef FIR_H
#define FIR_H
#define pi 3.1415926535897932384626433832795
#if defined(__cplusplus)
extern "C" {
#endif
/******************************************************************************/
typedef struct _sFIRCoeff
{
float *pB;
float *pX;
}FIRCOEFF;
void Sinc(float *pY, int M, float s, float f, int len);
/******************************************************************************/
void FIRCalcDCRemovalCoeff(struct _sFIRCoeff *pCoeff, int N);
void FIR(struct _sFIRCoeff *pCoeff, float *xn, float *yn, int order, int len);
void FIRInit(struct _sFIRCoeff *pCoeff, float *pB, int order);
void FIRAlloc(struct _sFIRCoeff *pCoeff, int order);
void FIRFree(struct _sFIRCoeff *pCoeff);
/******************************************************************************/
#if defined(__cplusplus)
}
#endif
#endif /* FIR_H */
fir.h.old
Executable
+12
View File
@@ -0,0 +1,12 @@
#ifdef __cplusplus
extern "C" {
#endif
extern void LinearizePhase(double *Hk, double *hkReal, double *hkImag, unsigned N);
extern void FIRCalcFilterCoeffs(double *Hk, double *hkReal, double *hkImag, unsigned N);
extern void FIR(double *pReal, double *pImag, double *pHreal, double *pHimag, unsigned N);
#ifdef __cplusplus
}
#endif
fir2.c
Executable
+353
View File
@@ -0,0 +1,353 @@
/*************************************************************************/
/* 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;
}
}
fir2.h
Executable
+41
View File
@@ -0,0 +1,41 @@
/******************************************************************************/
/* fir2.h */
/******************************************************************************/
#ifndef FIR_H
#define FIR_H
#ifndef fir_pi
#define fir_pi 3.1415926535897932384626433832795
#endif
#ifndef fir_float_t
#define fir_float_t float
#endif
#if defined(__cplusplus)
extern "C" {
#endif
/******************************************************************************/
fir_float_t Sinc(fir_float_t x);
void CalcSincFilter(fir_float_t *pY, fir_float_t s, fir_float_t f, int len);
void FIRCalcLowpass(fir_float_t omega, fir_float_t *pCoeff, int N);
void FIRCalcHighpass(fir_float_t omega, fir_float_t *pCoeff, int N);
void FIRCalcBandpass(fir_float_t omega, fir_float_t bw, fir_float_t *pCoeff, int N);
void FIR(fir_float_t *pCoeff, fir_float_t *pState, int order, fir_float_t *x, fir_float_t *y, int len);
void FIR_upsample(fir_float_t *pCoeff, fir_float_t *pState, int order, fir_float_t *x, fir_float_t *y, int upsample_factor, int len);
void FIR_downsample(fir_float_t *pCoeff, fir_float_t *pState, int order, fir_float_t *x, fir_float_t *y, int downsample_factor, int len);
void CalcHamming(fir_float_t *pX, fir_float_t *pY, int len);
void CalcVonHann(fir_float_t *pX, fir_float_t *pY, int len);
void CalcBlackman(fir_float_t *pX, fir_float_t *pY, int len);
void CalcKaiser(fir_float_t *pX, fir_float_t *pY, fir_float_t b, int len);
void CalcKaiserSR(fir_float_t *pX, fir_float_t *pY, fir_float_t b, int len);
fir_float_t CalcFirRC(fir_float_t *pB, fir_float_t fa, fir_float_t Tsym, fir_float_t Alpha, int N);
fir_float_t CalcFirSRRC(fir_float_t *pB, fir_float_t fa, fir_float_t Tsym, fir_float_t Alpha, int N);
/******************************************************************************/
#if defined(__cplusplus)
}
#endif
#endif /* FIR_H */
firdecim.c
Executable
+77
View File
@@ -0,0 +1,77 @@
/*************************************************************************/
/* fir.c
/*************************************************************************/
#include <stdio.h>
#include <malloc.h>
#include "math.h"
#include "fir.h"
#include "firdecim.h"
/*************************************************************************/
/* Global Variables
/*************************************************************************/
int FDfilter(struct _sFIRDECIM *pObj, float *pX, float *pY, int len)
{
int len2, i, firLen, kL;
float *pSxt, *pBt, sum;
kL = pObj->kL;
firLen = pObj->firLen;
pBt = &pObj->pB[0];
pSxt = &pObj->pX[0];
len2 = 0;
while (len >= kL)
{
for (i = firLen-1; i >= kL; i--)
{
pObj->pX[i] = pObj->pX[i- kL];
}
for (i = kL-1; i >= 0; i--)
{
pObj->pX[i] = *(pX++);
}
len -= kL;
sum = 0.0;
for (i=0; i < firLen; i++)
sum += pObj->pX[i] * pObj->pB[i];
*(pY++) = sum;
len2++;
}
return len2;
}
void FDinit(struct _sFIRDECIM *pObj, int kL, int nCoefPerPhase)
{
int n;
pObj->kL = kL;
pObj->firLen = kL*nCoefPerPhase;
pObj->nCoefPerPhase = nCoefPerPhase;
if ((pObj->firLen%2) == 0)
pObj->firLen++;
pObj->pB = (float*)malloc(pObj->firLen*sizeof(float));
pObj->pX = (float*)malloc(pObj->firLen*sizeof(float));
for(n=0; n < pObj->firLen; n++)
{
pObj->pX[n] = 0;
}
Sinc(pObj->pB, kL, 1.f/kL, 1.f/kL, pObj->firLen);
}
void FDfree(struct _sFIRDECIM *pObj)
{
free(pObj->pB);
free(pObj->pX);
}
firdecim.h
Executable
+37
View File
@@ -0,0 +1,37 @@
/******************************************************************************/
/* iir.h
/******************************************************************************/
#ifndef FIRDECIM_H
#define FIRDECIM_H
#define pi 3.1415926535897932384626433832795
#if defined(__cplusplus)
extern "C" {
#endif
/******************************************************************************/
typedef struct _sFIRDECIM
{
float *pB;
float *pX;
int nCoefPerPhase;
int firLen;
int kL;
}FIRDECIM;
/******************************************************************************/
int FDfilter(struct _sFIRDECIM *pObj, float *pX, float *pY, int len);
void FDinit(struct _sFIRDECIM *pObj, int kL, int nIp);
void FDfree(struct _sFIRDECIM *pObj);
/******************************************************************************/
#if defined(__cplusplus)
}
#endif
#endif /* FIRDECIM_H */
firinterp.c
Executable
+75
View File
@@ -0,0 +1,75 @@
/*************************************************************************/
/* fir.c
/*************************************************************************/
#include <stdio.h>
#include <malloc.h>
#include "math.h"
#include "fir.h"
#include "firinterp.h"
/*************************************************************************/
/* Global Variables
/*************************************************************************/
int FIfilter(struct _sFIRIP *pObj, float *pX, float *pY, int len)
{
int n, len2, k, i, j, nIp, firLen, kL;
float *pSxt, *pBt, sum;
kL = pObj->kL;
nIp = pObj->nCoefPerPhase;
firLen = pObj->firLen;
pBt = &pObj->pB[0];
pSxt = &pObj->pX[0];
len2 = 0;
for (n=0; n < len; n++)
{
for (i=nIp; i > 0; i--)
pSxt[i] = pSxt[i-1];
pSxt[0] = pX[n];
for (k=0; k < kL; k++)
{
sum = 0.0;
i=0;
for (j=k; j < firLen; j += kL)
{
sum += pSxt[i] * pBt[j];
i++;
}
*(pY++) = sum;
len2++;
}
}
return len2;
}
void FIinit(struct _sFIRIP *pObj, int kL, int nCoefPerPhase)
{
int n;
pObj->kL = kL;
pObj->firLen = kL*nCoefPerPhase;
pObj->nCoefPerPhase = nCoefPerPhase;
if ((pObj->firLen%2) == 0)
pObj->firLen++;
pObj->pB = (float*)malloc(pObj->firLen*sizeof(float));
pObj->pX = (float*)malloc((pObj->nCoefPerPhase+1)*sizeof(float));
for(n=0; n <= pObj->nCoefPerPhase; n++)
{
pObj->pX[n] = 0;
}
Sinc(pObj->pB, kL, 1.f, 1.f/kL, pObj->firLen);
}
void FIfree(struct _sFIRIP *pObj)
{
free(pObj->pB);
free(pObj->pX);
}
firinterp.h
Executable
+38
View File
@@ -0,0 +1,38 @@
/******************************************************************************/
/* iir.h
/******************************************************************************/
#ifndef FIRINTERP_H
#define FIRINTERP_H
#define pi 3.1415926535897932384626433832795
#if defined(__cplusplus)
extern "C" {
#endif
/******************************************************************************/
typedef struct _sFIRIP
{
float *pB;
float *pX;
int nCoefPerPhase;
int firLen;
int kL;
}FIRIP;
/******************************************************************************/
int FIfilter(struct _sFIRIP *pObj, float *pX, float *pY, int len);
void FIinit(struct _sFIRIP *pObj, int kL, int nIp);
void FIfree(struct _sFIRIP *pObj);
/******************************************************************************/
#if defined(__cplusplus)
}
#endif
#endif /* FIRINTERP_H */
resample.c
Executable
+417
View File
@@ -0,0 +1,417 @@
/*************************************************************************/
/* fir.c
/*************************************************************************/
#include <stdio.h>
#include <malloc.h>
#include <memory.h>
#include "math.h"
#include "fir.h"
#include "firinterp.h"
#include "firdecim.h"
#include "resample.h"
/*************************************************************************/
/* Global Variables
/*************************************************************************/
/* Berechnet den ggT rekursiv mit Euklid's Algorithmus */
int GGT(int a, int b)
{
int r;
r = a % b;
if (r == 0)
return b;
else
return GGT(b, r); /* Rekursion */
}
/***************************************************************/
/* Primzahlenzerlegung */
int GetStages(int n, int *pBuf)
{
int z;
int c;
z = 2;
c = 0;
while (z <= n)
{
if ((n % z) == 0)
{
pBuf[c] = z;
c++;
n = n / z;
}
else
z = z + 1;
}
return c;
}
int GetNumStages(int n)
{
int z;
int c;
z = 2;
c = 0;
while (z <= n)
{
if ((n % z) == 0)
{
c++;
n = n / z;
}
else
z = z + 1;
}
return c;
}
/***************************************************************/
int ReduceStages(int *pBuf, int nMaxStages, int len)
{
int i;
if ((nMaxStages < len) && (len > 1))
{
pBuf[0] *= pBuf[1];
for (i=1; i < len; i++)
pBuf[i] = pBuf[i+1];
len--;
jsort(pBuf, len);
return ReduceStages(pBuf, nMaxStages, len);
}
return len;
}
/***************************************************************/
void jsort(int *pBuf, int len)
{
int i, temp, unsorted;
do
{
unsorted = 0;
for (i=0; i < len-1; i++)
{
if (pBuf[i] > pBuf[i+1])
{
temp = pBuf[i];
pBuf[i] = pBuf[i+1];
pBuf[i+1] = temp;
unsorted = 1;
}
}
} while(unsorted);
}
/***************************************************************/
void RSinit(struct _sRESAMPLER *pObj, int nCoef, float fs1, float fs2, int nMaxStagesL, int nMaxStagesM, int blockSize)
{
int ggt, s;
pObj->pFirL = 0;
pObj->pFirM = 0;
pObj->fs1 = fs1;
pObj->fs2 = fs2;
printf("Initial fs = %g Hz\n",pObj->fs1);
printf("Target fs = %g Hz\n",pObj->fs2);
ggt = GGT((int)fs1,(int)fs2);
printf("GGT = %d\n",ggt);
pObj->L = (int)fs2/ggt;
pObj->M = (int)fs1/ggt;
printf("Up factor = %d\n",pObj->L);
printf("Down factor = %d\n",pObj->M);
pObj->nStagesL = GetNumStages(pObj->L);
pObj->nStagesM = GetNumStages(pObj->M);
pObj->pStagesL = (int*)malloc((pObj->nStagesL+1)*sizeof(int));
pObj->pStagesM = (int*)malloc((pObj->nStagesM+1)*sizeof(int));
memset(pObj->pStagesL, 0, (pObj->nStagesL+1)*sizeof(int));
memset(pObj->pStagesM, 0, (pObj->nStagesM+1)*sizeof(int));
GetStages(pObj->L, pObj->pStagesL);
GetStages(pObj->M, pObj->pStagesM);
printf("N up stages [%d] = ",pObj->nStagesL);
for (s=0; s < pObj->nStagesL; s++)
printf("%d ",pObj->pStagesL[s]);
printf("\n");
printf("N down stages [%d] = ",pObj->nStagesM);
for (s=0; s < pObj->nStagesM; s++)
printf("%d ",pObj->pStagesM[s]);
printf("\n");
if (nMaxStagesL > 0)
{
pObj->nStagesL = ReduceStages(pObj->pStagesL, nMaxStagesL, pObj->nStagesL);
printf("N up stages required [%d] = ",pObj->nStagesL);
for (s=0; s < pObj->nStagesL; s++)
printf("%d ",pObj->pStagesL[s]);
printf("\n");
}
if (nMaxStagesM > 0)
{
pObj->nStagesM = ReduceStages(pObj->pStagesM, nMaxStagesM, pObj->nStagesM);
printf("N down stages required [%d] = ",pObj->nStagesM);
for (s=0; s < pObj->nStagesM; s++)
printf("%d ",pObj->pStagesM[s]);
printf("\n");
}
pObj->pTemp = (float**)malloc(2*sizeof(float*));
pObj->pTemp[0] = (float*)malloc(pObj->L*blockSize*sizeof(float));
pObj->pTemp[1] = (float*)malloc(pObj->L*blockSize*sizeof(float));
if (pObj->nStagesL > 0)
pObj->pFirL = (FIRIP*)malloc(pObj->nStagesL*sizeof(FIRIP));
if (pObj->nStagesM > 0)
pObj->pFirM = (FIRDECIM*)malloc(pObj->nStagesM*sizeof(FIRDECIM));
for (s=0; s < pObj->nStagesL; s++)
{
if (pObj->pStagesL[s] > 1)
FIinit(&pObj->pFirL[s], pObj->pStagesL[s], nCoef);
}
for (s=0; s < pObj->nStagesM; s++)
{
if (pObj->pStagesM[s] > 1)
FDinit(&pObj->pFirM[s], pObj->pStagesM[s], nCoef);
}
printf("\n");
pObj->currPhase = pObj->L;
pObj->nTapsPerPhase = nCoef;
pObj->sizeH = pObj->nTapsPerPhase*pObj->L;
pObj->pH = (float*)malloc(pObj->sizeH*sizeof(float));
pObj->pZ = (float*)malloc(pObj->sizeH*sizeof(float));
Sinc(pObj->pH, pObj->L, 1.f, 1.f/pObj->L, pObj->sizeH);
memset(pObj->pZ, 0, pObj->sizeH*sizeof(float));
}
void RSfree(struct _sRESAMPLER *pObj)
{
int s;
for (s=0; s < pObj->nStagesL; s++)
FIfree(&pObj->pFirL[s]);
for (s=0; s < pObj->nStagesM; s++)
FDfree(&pObj->pFirM[s]);
if (pObj->pFirL)
free(pObj->pFirL);
if (pObj->pFirM)
free(pObj->pFirM);
free(pObj->pStagesL);
free(pObj->pStagesM);
free(pObj->pTemp[0]);
free(pObj->pTemp[1]);
}
int RSresample(struct _sRESAMPLER *pObj, float *pIn, float *pOut, int len)
{
int s, id1, id2, t;
id1 = 0;
id2 = 1;
if (pObj->L > 1)
{
len = FIfilter(&pObj->pFirL[0], pIn, pObj->pTemp[id1], len);
for (s=1; s < pObj->nStagesL; s++)
{
len = FIfilter(&pObj->pFirL[s], pObj->pTemp[id1], pObj->pTemp[id2], len);
t = id1;
id1 = id2;
id2 = t;
}
}
else
{
memcpy(pObj->pTemp[id1], pIn, len*sizeof(float));
}
if (pObj->M > 1)
{
for (s=0; s < pObj->nStagesM-1; s++)
{
len = FDfilter(&pObj->pFirM[s], pObj->pTemp[id1], pObj->pTemp[id2], len);
t = id1;
id1 = id2;
id2 = t;
}
len = FDfilter(&pObj->pFirM[pObj->nStagesM-1], pObj->pTemp[id1], pOut, len);
}
else
{
memcpy(pOut, pObj->pTemp[id1], len*sizeof(float));
}
return len;
}
int RSresample2(struct _sRESAMPLER *pObj, float *pIn, float *pOut, int len)
{
//void resamp0(int interp_factor_L, int decim_factor_M, int num_taps_per_phase,
// int *p_current_phase, const double *const p_H,
// double *const p_Z, int num_inp, const double *p_inp,
// double *p_out, int *p_num_out)
int tap, num_out, phase_num = pObj->currPhase;
const float *p_coeff;
float *p_Z = pObj->pZ;
float *p_H = pObj->pH;
int interp_factor_L = pObj->L;
int decim_factor_M = pObj->M;
int num_taps_per_phase = pObj->nTapsPerPhase;
int num_inp = len;
float sum;
num_out = 0;
while (num_inp > 0)
{
/* shift input samples into Z delay line */
while (phase_num >= interp_factor_L)
{
/* decrease phase number by interpolation factor L */
phase_num -= interp_factor_L;
/* shift Z delay line up to make room for next sample */
for (tap = num_taps_per_phase - 1; tap >= 1; tap--)
{
p_Z[tap] = p_Z[tap - 1];
}
/* copy next sample from input buffer to bottom of Z delay line */
p_Z[0] = *pIn++;
if (--num_inp == 0)
{
break;
}
}
/* calculate outputs */
while (phase_num < interp_factor_L)
{
/* point to the current polyphase filter */
p_coeff = p_H + phase_num;
/* calculate FIR sum */
sum = 0.0;
for (tap = 0; tap < num_taps_per_phase; tap++)
{
sum += *p_coeff * p_Z[tap];
p_coeff += interp_factor_L; /* point to next coefficient */
}
*pOut++ = sum; /* store sum and point to next output */
num_out++;
/* increase phase number by decimation factor M */
phase_num += decim_factor_M;
}
}
/* pass phase number and number of outputs back to caller */
pObj->currPhase = phase_num;
return num_out;
}
int RSresample3(struct _sRESAMPLER *pObj, float *pIn, float *pOut, int len)
{
int tap, num_out, num_new_samples, phase_num = pObj->currPhase;
const float *p_coeff;
float *p_Z = pObj->pZ;
float *p_H = pObj->pH;
int interp_factor_L = pObj->L;
int decim_factor_M = pObj->M;
int num_taps_per_phase = pObj->nTapsPerPhase;
int num_inp = len;
float sum;
num_out = 0;
while (num_inp > 0)
{
/* figure out how many new samples to shift into Z delay line */
num_new_samples = 0;
while (phase_num >= interp_factor_L)
{
/* decrease phase number by interpolation factor L */
phase_num -= interp_factor_L;
num_new_samples++;
if (--num_inp == 0)
{
break;
}
}
if (num_new_samples >= num_taps_per_phase)
{
/* the new samples are bigger than the size of Z:
fill the entire Z with the tail of new inputs */
pIn += (num_new_samples - num_taps_per_phase);
num_new_samples = num_taps_per_phase;
}
/* copy new samples into Z */
/* shift Z delay line up to make room for next samples */
for (tap = num_taps_per_phase - 1; tap >= num_new_samples; tap--)
{
p_Z[tap] = p_Z[tap - num_new_samples];
}
/* copy next samples from input buffer to bottom of Z */
for (tap = num_new_samples - 1; tap >= 0; tap--)
{
p_Z[tap] = *pIn++;
}
/* calculate outputs */
while (phase_num < interp_factor_L)
{
/* point to the current polyphase filter */
p_coeff = p_H + phase_num;
/* calculate FIR sum */
sum = 0.0;
for (tap = 0; tap < num_taps_per_phase; tap++)
{
sum += *p_coeff * p_Z[tap];
p_coeff += interp_factor_L; /* point to next coefficient */
}
*pOut++ = sum; /* store sum and point to next output */
num_out++;
/* decrease phase number by decimation factor M */
phase_num += decim_factor_M;
}
}
/* pass back to caller phase number (for next call) and number of
outputs */
pObj->currPhase = phase_num;
return num_out;
}
resample.h
Executable
+52
View File
@@ -0,0 +1,52 @@
/******************************************************************************/
/* iir.h
/******************************************************************************/
#ifndef RESAMPLE_H
#define RESAMPLE_H
#include "fir.h"
#include "firinterp.h"
#include "firdecim.h"
#if defined(__cplusplus)
extern "C" {
#endif
/******************************************************************************/
typedef struct _sRESAMPLER
{
FIRIP *pFirL;
FIRDECIM *pFirM;
float fs1, fs2;
int L, M, *pStagesL, *pStagesM;
int nStagesL, nStagesM;
float **pTemp;
// Fast Resample
int currPhase;
float *pH, *pZ;
int nTapsPerPhase;
int sizeH;
}RESAMPLER;
/******************************************************************************/
int GGT2(int a, int b);
int GetNumStages(int n);
int PrimeFactor(int n, int *pBuf);
void jsort(int *pBuf, int len);
void RSinit(struct _sRESAMPLER *pObj, int nCoef, float fs1, float fs2, int nMaxStagesL, int nMaxStagesM, int blockSize);
void RSfree(struct _sRESAMPLER *pObj);
int RSresample(struct _sRESAMPLER *pObj, float *pIn, float *pOut, int len);
int RSresample2(struct _sRESAMPLER *pObj, float *pIn, float *pOut, int len);
int RSresample3(struct _sRESAMPLER *pObj, float *pIn, float *pOut, int len);
/******************************************************************************/
#if defined(__cplusplus)
}
#endif
#endif /* RESAMPLE_H */
window.c
Executable
+203
View File
@@ -0,0 +1,203 @@
/*=============================================================================
//
// This software has been released under the terms of the GNU General Public
// license. See http://www.gnu.org/copyleft/gpl.html for details.
//
// Copyright 2001 Anders Johansson ajh@atri.curtin.edu.au
//
//=============================================================================
*/
/* Calculates a number of window functions. The following window
functions are currently implemented: Boxcar, Triang, Hanning,
Hamming, Blackman, Flattop and Kaiser. In the function call n is
the number of filter taps and w the buffer in which the filter
coefficients will be stored.
*/
#include <math.h>
#include "dsp.h"
/*
// Boxcar
//
// n window length
// w buffer for the window parameters
*/
void boxcar(int n, _ftype_t* w)
{
int i;
// Calculate window coefficients
for (i=0 ; i<n ; i++)
w[i] = 1.0;
}
/*
// Triang a.k.a Bartlett
//
// | (N-1)|
// 2 * |k - -----|
// | 2 |
// w = 1.0 - ---------------
// N+1
// n window length
// w buffer for the window parameters
*/
void triang(int n, _ftype_t* w)
{
_ftype_t k1 = (_ftype_t)(n & 1);
_ftype_t k2 = 1/((_ftype_t)n + k1);
int end = (n + 1) >> 1;
int i;
// Calculate window coefficients
for (i=0 ; i<end ; i++)
w[i] = w[n-i-1] = (2.0*((_ftype_t)(i+1))-(1.0-k1))*k2;
}
/*
// Hanning
// 2*pi*k
// w = 0.5 - 0.5*cos(------), where 0 < k <= N
// N+1
// n window length
// w buffer for the window parameters
*/
void hanning(int n, _ftype_t* w)
{
int i;
_ftype_t k = 2*M_PI/((_ftype_t)(n+1)); // 2*pi/(N+1)
// Calculate window coefficients
for (i=0; i<n; i++)
*w++ = 0.5*(1.0 - cos(k*(_ftype_t)(i+1)));
}
/*
// Hamming
// 2*pi*k
// w(k) = 0.54 - 0.46*cos(------), where 0 <= k < N
// N-1
//
// n window length
// w buffer for the window parameters
*/
void hamming(int n,_ftype_t* w)
{
int i;
_ftype_t k = 2*M_PI/((_ftype_t)(n-1)); // 2*pi/(N-1)
// Calculate window coefficients
for (i=0; i<n; i++)
*w++ = 0.54 - 0.46*cos(k*(_ftype_t)i);
}
/*
// 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
//
// n window length
// w buffer for the window parameters
*/
void blackman(int n,_ftype_t* w)
{
int i;
_ftype_t k1 = 2*M_PI/((_ftype_t)(n-1)); // 2*pi/(N-1)
_ftype_t k2 = 2*k1; // 4*pi/(N-1)
// Calculate window coefficients
for (i=0; i<n; i++)
*w++ = 0.42 - 0.50*cos(k1*(_ftype_t)i) + 0.08*cos(k2*(_ftype_t)i);
}
/*
// Flattop
// 2*pi*k 4*pi*k
// w(k) = 0.2810638602 - 0.5208971735*cos(------) + 0.1980389663*cos(------), where 0 <= k < N
// N-1 N-1
//
// n window length
// w buffer for the window parameters
*/
void flattop(int n,_ftype_t* w)
{
int i;
_ftype_t k1 = 2*M_PI/((_ftype_t)(n-1)); // 2*pi/(N-1)
_ftype_t k2 = 2*k1; // 4*pi/(N-1)
// Calculate window coefficients
for (i=0; i<n; i++)
*w++ = 0.2810638602 - 0.5208971735*cos(k1*(_ftype_t)i) + 0.1980389663*cos(k2*(_ftype_t)i);
}
/* 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-21 // Max error acceptable
_ftype_t besselizero(_ftype_t x)
{
_ftype_t temp;
_ftype_t sum = 1.0;
_ftype_t u = 1.0;
_ftype_t halfx = x/2.0;
int n = 1;
do {
temp = halfx/(_ftype_t)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 kaiser(int n, _ftype_t* w, _ftype_t b)
{
_ftype_t tmp;
_ftype_t k1 = 1.0/besselizero(b);
int k2 = 1 - (n & 1);
int end = (n + 1) >> 1;
int i;
// Calculate window coefficients
for (i=0 ; i<end ; i++){
tmp = (_ftype_t)(2*i + k2) / ((_ftype_t)n - 1.0);
w[end-(1&(!k2))+i] = w[end-1-i] = k1 * besselizero(b*sqrt(1.0 - tmp*tmp));
}
}