commit 0179c510fe679230ea8775dc575c0b75a3e4c752 Author: Jens Ahrensfeld Date: Sat Jul 19 07:44:42 2014 +0000 Initial import git-svn-id: http://moon:8086/svn/software/trunk/libsrc/fir@1 b431acfa-c32f-4a4a-93f1-934dc6c82436 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