From 0179c510fe679230ea8775dc575c0b75a3e4c752 Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Sat, 19 Jul 2014 07:44:42 +0000 Subject: [PATCH] Initial import git-svn-id: http://moon:8086/svn/software/trunk/libsrc/fir@1 b431acfa-c32f-4a4a-93f1-934dc6c82436 --- fir.c | 94 ++++++++++++ fir.h | 37 +++++ fir.h.old | 12 ++ fir2.c | 353 ++++++++++++++++++++++++++++++++++++++++++++ fir2.h | 41 ++++++ firdecim.c | 77 ++++++++++ firdecim.h | 37 +++++ firinterp.c | 75 ++++++++++ firinterp.h | 38 +++++ resample.c | 417 ++++++++++++++++++++++++++++++++++++++++++++++++++++ resample.h | 52 +++++++ window.c | 203 +++++++++++++++++++++++++ 12 files changed, 1436 insertions(+) create mode 100755 fir.c create mode 100755 fir.h create mode 100755 fir.h.old create mode 100755 fir2.c create mode 100755 fir2.h create mode 100755 firdecim.c create mode 100755 firdecim.h create mode 100755 firinterp.c create mode 100755 firinterp.h create mode 100755 resample.c create mode 100755 resample.h create mode 100755 window.c diff --git a/fir.c b/fir.c new file mode 100755 index 0000000..622c6eb --- /dev/null +++ b/fir.c @@ -0,0 +1,94 @@ +/*************************************************************************/ +/* fir.c +/*************************************************************************/ +#include +#include +#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; ipB[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); +} diff --git a/fir.h b/fir.h new file mode 100755 index 0000000..fe1b621 --- /dev/null +++ b/fir.h @@ -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 */ diff --git a/fir.h.old b/fir.h.old new file mode 100755 index 0000000..deb46b6 --- /dev/null +++ b/fir.h.old @@ -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 diff --git a/fir2.c b/fir2.c new file mode 100755 index 0000000..19a162d --- /dev/null +++ b/fir2.c @@ -0,0 +1,353 @@ +/*************************************************************************/ +/* fir.c */ +/*************************************************************************/ +#include +#include +#include +#include + +#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> 1; + int i; + + pW = (double*)malloc(len*sizeof(double)); + + // Calculate window coefficients + for (i=0 ; i +#include +#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); +} \ No newline at end of file diff --git a/firdecim.h b/firdecim.h new file mode 100755 index 0000000..44283ca --- /dev/null +++ b/firdecim.h @@ -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 */ diff --git a/firinterp.c b/firinterp.c new file mode 100755 index 0000000..da2fe31 --- /dev/null +++ b/firinterp.c @@ -0,0 +1,75 @@ +/*************************************************************************/ +/* fir.c +/*************************************************************************/ +#include +#include +#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); +} \ No newline at end of file diff --git a/firinterp.h b/firinterp.h new file mode 100755 index 0000000..65cb337 --- /dev/null +++ b/firinterp.h @@ -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 */ diff --git a/resample.c b/resample.c new file mode 100755 index 0000000..36b1fc1 --- /dev/null +++ b/resample.c @@ -0,0 +1,417 @@ +/*************************************************************************/ +/* fir.c +/*************************************************************************/ +#include +#include +#include +#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; +} diff --git a/resample.h b/resample.h new file mode 100755 index 0000000..ca9d6f8 --- /dev/null +++ b/resample.h @@ -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 */ diff --git a/window.c b/window.c new file mode 100755 index 0000000..c04913d --- /dev/null +++ b/window.c @@ -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 +#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> 1; + int i; + + // Calculate window coefficients + for (i=0 ; i= 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