jens
0179c510fe
git-svn-id: http://moon:8086/svn/software/trunk/libsrc/fir@1 b431acfa-c32f-4a4a-93f1-934dc6c82436
418 lines
10 KiB
C
Executable File
418 lines
10 KiB
C
Executable File
/*************************************************************************/
|
|
/* 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;
|
|
}
|