jens/avpflms
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
master
avpflms/pfft.c.bak
T
jens e93e7927cb Initial import 
git-svn-id: http://moon:8086/svn/software/trunk/libsrc/avpflms@1 b431acfa-c32f-4a4a-93f1-934dc6c82436
2014-07-19 07:44:42 +00:00

333 lines
9.9 KiB
Plaintext
Executable File
Raw Permalink Blame History

/*-------------------------------------------------------------------------
* pfft.c Partioned Fast Fourier Transform
* $Id: $
*-------------------------------------------------------------------------
*
* Copyright (C) 2001 Algo Vision Systems GmbH
*
*-------------------------------------------------------------------------
*/
#define AVNEEDFLOAT
#ifdef _DEBUG
#include <stdio.h>
#endif
#include <math.h>
#include "avtypes.h"
#include "averror.h"
#include "avrtl.h"
#include "avfft.h"
#include "pfft.h"
/*************************************************************************/
AVERR PfftInit(
PFFT *pObj, /* Zeiger auf Objekt */
UINT32 N, /* Filterlaenge N */
UINT32 P, /* Anzahl Filterpartitionen */
UINT32 S, /* Anzahl Filtersegmente pro Partition */
UINT32 L, /* Laenge der Eingangsdaten (Blocklaenge */
UINT32 C /* FFT-Laenge */
)
{
UINT32 i, s, NP, C_L;
CMPXBUF *pX; /* Temporaerer Zeiger fuer Bufferinit.*/
/* Auto-Param */
if ((N*L)==0)
return AV_E_FAIL;
if (P==0)
{
if ((N%L) != 0)
return AV_E_FAIL;
P = N/L;
}
/* Auto-guess FFT-Groesse C */
if (C==0)
pObj->C = (UINT32)pow(2,ceil(log(L+N/P-1)/log(2.0)));
else
pObj->C = C;
/* Objekt initialisieren */
pObj->L = L;
pObj->N = N;
pObj->P = P;
pObj->S = S;
NP = pObj->S * pObj->L;
C_L = pObj->C - pObj->L;
/*-------------------------------------------------------*/
/* Speicher allokieren */
/*-------------------------------------------------------*/
/* Speicher fuer X[P*S][C] (komplex) */
pObj->pBufX = (CMPXBUF*)AvMemAlloc(pObj->P*pObj->S*sizeof(CMPXBUF));
for (i=0; i < pObj->P*pObj->S; i++)
{
pObj->pBufX[i].cmpxData.pReal = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
pObj->pBufX[i].cmpxData.pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
AvZeroMem(pObj->pBufX[i].cmpxData.pReal,pObj->C*sizeof(avfloat_t));
AvZeroMem(pObj->pBufX[i].cmpxData.pImag,pObj->C*sizeof(avfloat_t));
pObj->pBufX[i].pNext = &pObj->pBufX[i+1];
pObj->pBufX[i].pLast = &pObj->pBufX[i-1];
pObj->pBufX[i].pLastPS = NULL;
pObj->pBufX[i].user = i;
}
pObj->pBufX[i-1].pNext = &pObj->pBufX[0];
pObj->pBufX[0].pLast = &pObj->pBufX[i-1];
/* Zeiger auf X[k-p*S] */
for (i=0; i < pObj->P*pObj->S; i++)
{
pX = pObj->pBufX[i].pLast;
for (s=1; s < pObj->S; s++)
pX = pX->pLast;
pObj->pBufX[i].pLastPS = pX;
}
/* Ergebnis 'Y' der Faltung (komplex) */
pObj->pY = (COMPLEX*)AvMemAlloc(sizeof(COMPLEX));
pObj->pY->pReal = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
pObj->pY->pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
AvZeroMem(pObj->pY->pReal,pObj->C*sizeof(avfloat_t));
AvZeroMem(pObj->pY->pImag,pObj->C*sizeof(avfloat_t));
/* Overlap-Save 'xs' (reell) */
pObj->pBufXsave = (avfloat_t*)AvMemAlloc(C_L*sizeof(avfloat_t));
AvZeroMem(pObj->pBufXsave, C_L*sizeof(avfloat_t));
/* FFT initialisieren */
pObj->pFFT = (FFT*)AvMemAlloc(sizeof(FFT));
FFTinit(pObj->pFFT, pObj->C);
/* Arbeitszeiger initialisieren */
pObj->pX = pObj->pBufX;
return AV_E_OK;
}
/*-----------------------------------------------------------------------*/
/* Partitioned FLMS Filterinitialisierung
/* 1. Partitionierung der Filterstartwerte in P-Partitionen
/* 2. Transformation der P Teil-Filter in den Frequenzbereich
/* Element pBufWS wird veraendert
/*-----------------------------------------------------------------------*/
AVERR PfftFilterInit(
PFFT *pObj, /* Zeiger auf Objekt */
avfloat_t *pWTD,
CMPXBUF *pWS) /* N Filterkoeffizienten im Zeitbereich */
{
UINT32 p, NP, C_SL;
NP = pObj->S * pObj->L;
C_SL = pObj->C - NP;
for (p=0; p < pObj->P; p++)
{
/* Arbeitspuffer WS auffuellen
Re{WS[p][0..C-L-1]}=wi[p*N/P..(p+1)*N/P-1] */
AvMemCpy(pWS[p].cmpxData.pReal,
&pWTD[p*NP], NP*sizeof(avfloat_t));
/* Re{WS[C-L..C]} = {0} (vorsichtshalber) */
AvZeroMem(&pWS[p].cmpxData.pReal[NP], C_SL *sizeof(avfloat_t));
/* Im{WS[0..C-1]} = {0} */
AvZeroMem(pWS[p].cmpxData.pImag, pObj->C *sizeof(avfloat_t));
/* In den Frequenzbereich transformieren
WS[p][0..C-1] = 1/C*FFT{wi[p*N/P..(p+1)*N/P-1]} */
ffts(pObj->pFFT, pWS[p].cmpxData.pReal, pWS[p].cmpxData.pImag);
}
return AV_E_OK;
}
/*---------------------------------------------------------------*/
/* Speicher zuweisen fuer Koeffizienten H
/*---------------------------------------------------------------*/
AVERR PfftFilterAlloc(PFFT *pObj, CMPXBUF **ppH)
{
UINT32 i;
/* Speicher fuer WS[P][C] (komplex) */
*ppH = (CMPXBUF*)AvMemAlloc(pObj->P*pObj->S*sizeof(CMPXBUF));
for (i=0; i < pObj->P*pObj->S; i++)
{
(*ppH)[i].cmpxData.pReal = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
(*ppH)[i].cmpxData.pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
AvZeroMem((*ppH)[i].cmpxData.pReal,pObj->C*sizeof(avfloat_t));
AvZeroMem((*ppH)[i].cmpxData.pImag,pObj->C*sizeof(avfloat_t));
(*ppH)[i].pNext = &(*ppH)[i+1];
(*ppH)[i].pLast = &(*ppH)[i-1];
(*ppH)[i].pLastPS = NULL;
(*ppH)[i].user = i;
}
(*ppH)[i-1].pNext = &(*ppH)[0];
(*ppH)[0].pLast = &(*ppH)[i-1];
return AV_E_OK;
}
/*---------------------------------------------------------------*/
/* Partitioned FFT-Filterung
/*---------------------------------------------------------------*/
AVERR PfftFilter(PFFT *pObj, CMPXBUF *pH, avfloat_t *px, avfloat_t *py)
{
UINT32 p, NP, C_L;
CMPXBUF *pX;
COMPLEX *pY;
/* Arbeitszeiger */
pX = pObj->pX; /* Aktueller Zeiger X[k] */
pY = pObj->pY; /* 'Y' wird nach der Filterung als
temporaerer Speicher benutzt */
NP = pObj->S*pObj->L; /* N/P = S*L */
C_L = pObj->C - pObj->L; /* C-L */
/* Arbeitspuffer 'X' auffuellen Re{X[S*L..C-1]} = x[0..L-1] */
AvMemCpy(&pX->cmpxData.pReal[C_L],px,
pObj->L*sizeof(avfloat_t));
/* Saveblock 'xs' anfuegen Re{X[0..C-L-1]} = xs[0..C-L-1] */
AvMemCpy(pX->cmpxData.pReal,pObj->pBufXsave,
C_L*sizeof(avfloat_t));
/* Saveblock aktualisieren xs[0..C-L-1] = x[L..C-1] */
AvMemCpy(pObj->pBufXsave, &pX->cmpxData.pReal[pObj->L],
C_L*sizeof(avfloat_t));
/* Imaginaerteil von 'X' auf Null setzen Im{X[0..C-1]} = {0} */
AvZeroMem(pX->cmpxData.pImag,pObj->C*sizeof(avfloat_t));
/* X = 1/C*FFT{x} */
ffts(pObj->pFFT, pX->cmpxData.pReal, pX->cmpxData.pImag);
/* 1. Partition Multiplikation im Frequenzbereich
Y = X[k][0..C-1] * H[0][0..C-1] * C */
CmpxVectMulS(&pX->cmpxData, &pH->cmpxData,
pY, pObj->C);
/* 2. Partition bis P-te Partition */
for (p=1; p < pObj->P; p++)
{
pH = pH->pNext;
/* X[k-p*S] suchen */
pX = pX->pLastPS;
/* Y = X[k-p*S][0..C-1] * H[p][0..C-1] * C */
CmpxVectMacS(&pX->cmpxData, &pH->cmpxData,
pY, pObj->C);
}
/* In den Zeitbereich transformieren, y = IFFT{Y} */
ifft(pObj->pFFT, pY->pReal, pY->pImag);
/* Abspeichern der letzten L Daten ys[0..L-1] = Re{Y[C-L..C-1] */
AvMemCpy(py, &pY->pReal[C_L], pObj->L*sizeof(avfloat_t));
/* Fuer den naechsten Aufruf Zeiger aktualisieren */
/* Naechstes X[k] ist: */
pObj->pX = pObj->pX->pNext;
return AV_E_OK;
}
/*-----------------------------------------------------------------*/
/* Complex-Funktionen
/*-----------------------------------------------------------------*/
void CmpxVectMul(
struct _sCOMPLEX *pA,
struct _sCOMPLEX *pB,
struct _sCOMPLEX *pAB,
UINT32 len)
{
UINT32 i;
for (i=0; i < len; i++)
{
pAB->pReal[i] = pA->pReal[i]*pB->pReal[i]
- pA->pImag[i]*pB->pImag[i];
pAB->pImag[i] = pA->pReal[i]*pB->pImag[i]
+ pA->pImag[i]*pB->pReal[i];
}
}
void CmpxVectAdd(
struct _sCOMPLEX *pA,
struct _sCOMPLEX *pB,
struct _sCOMPLEX *pAB,
UINT32 len)
{
UINT32 i;
for (i=0; i < len; i++)
{
pAB->pReal[i] = pA->pReal[i] + pB->pReal[i];
pAB->pImag[i] = pA->pImag[i] + pB->pImag[i];
}
}
void CmpxVectMac(
struct _sCOMPLEX *pA,
struct _sCOMPLEX *pB,
struct _sCOMPLEX *pAB,
UINT32 len)
{
UINT32 i;
for (i=0; i < len; i++)
{
pAB->pReal[i] += pA->pReal[i]*pB->pReal[i]
- pA->pImag[i]*pB->pImag[i];
pAB->pImag[i] += pA->pReal[i]*pB->pImag[i]
+ pA->pImag[i]*pB->pReal[i];
}
}
void CmpxVectMulS(
struct _sCOMPLEX *pA,
struct _sCOMPLEX *pB,
struct _sCOMPLEX *pAB,
UINT32 len)
{
UINT32 i;
for (i=0; i < len; i++)
{
pAB->pReal[i] = (pA->pReal[i]*pB->pReal[i]
- pA->pImag[i]*pB->pImag[i])*len;
pAB->pImag[i] = (pA->pReal[i]*pB->pImag[i]
+ pA->pImag[i]*pB->pReal[i])*len;
}
}
void CmpxVectMacS(
struct _sCOMPLEX *pA,
struct _sCOMPLEX *pB,
struct _sCOMPLEX *pAB,
UINT32 len)
{
UINT32 i;
for (i=0; i < len; i++)
{
pAB->pReal[i] += (pA->pReal[i]*pB->pReal[i]
- pA->pImag[i]*pB->pImag[i])*len;
pAB->pImag[i] += (pA->pReal[i]*pB->pImag[i]
+ pA->pImag[i]*pB->pReal[i])*len;
}
}
/*------------------------------------------------------------------*/
Reference in New Issue View Git Blame Copy Permalink