static void CalculateKBDWindow(float* win, float alpha, int length)
{
int i;
float IBeta;
float tmp;
float sum = 0.0;
alpha *= PI;
IBeta = 1.0f/Izero(alpha);
/* calculate lower half of Kaiser Bessel window */
for(i=0; i<(length>>1); i++) {
tmp = 4.0f*(float)i/(float)length - 1.0f;
win[i] = Izero(alpha*(float)sqrt(1.0f-tmp*tmp))*IBeta;
sum += win[i];
}
sum = 1.0f/sum;
tmp = 0.0f;
/* calculate lower half of window */
for(i=0; i<(length>>1); i++) {
tmp += win[i];
win[i] = (float)sqrt(tmp*sum);
}
}
void LpFilter(double c[], int N, double frq, double Beta, int Num)
{
double IBeta, temp, inm1;
int i;
/* Calculate ideal lowpass filter impulse response coefficients: */
c[0] = 2.0*frq;
for (i=1; i<N; i++) {
temp = PI*(double)i/(double)Num;
c[i] = sin(2.0*temp*frq)/temp; /* Analog sinc function, cutoff = frq */
}
/*
* Calculate and Apply Kaiser window to ideal lowpass filter.
* Note: last window value is IBeta which is NOT zero.
* You're supposed to really truncate the window here, not ramp
* it to zero. This helps reduce the first sidelobe.
*/
IBeta = 1.0/Izero(Beta);
inm1 = 1.0/((double)(N-1));
for (i=1; i<N; i++) {
temp = (double)i * inm1;
c[i] *= Izero(Beta*sqrt(1.0-temp*temp)) * IBeta;
}
}
void CResample::lrsLpFilter(double c[], int Nf, double frq, double Beta, int Num)
{
double IBeta, temp, temp1, inm1;
int i;
/* Calculate ideal lowpass filter impulse response coefficients: */
c[0] = 2.0*frq;
for (i=1; i<Nf; i++) {
temp = PI*(double)i/(double)Num;
c[i] = sin(2.0*temp*frq)/temp; /* Analog sinc function, cutoff = frq */
}
/*
* Calculate and Apply Kaiser window to ideal lowpass filter.
* Note: last window value is IBeta which is NOT zero.
* You're supposed to really truncate the window here, not ramp
* it to zero. This helps reduce the first sidelobe.
*/
IBeta = 1.0/Izero(Beta);
inm1 = 1.0/((double)(Nf-1));
for (i=1; i<Nf; i++) {
temp = (double)i * inm1;
temp1 = 1.0 - temp*temp;
temp1 = (temp1<0? 0: temp1); /* make sure it's not negative since
we're taking the square root - this
happens on Pentium 4's due to tiny
roundoff errors */
c[i] *= Izero(Beta*sqrt(temp1)) * IBeta;
}
}
/*
* KaiserLowpass() creates a Kaiser-windowed lowpass filter.
*
* length is length of filter wing.
* cutoff is normalized cutoff freq (-6dB down).
* beta is the Kaiser window parameter.
* gain is the desired dc gain.
*
* The Kaiser window is given by:
* w[n] = Io(beta * sqrt(1 - (n/M)^2)) / Io(beta) (0 <= n <= M)
*
* Note: sampling of the "analog" lowpass is offset by 0.5 sample,
* so that all phases are an even length.
*/
void
KaiserLowpass(int length, float cutoff, float beta, float gain, double *filter)
{
double ibeta, ilength, x, w;
int i;
ibeta = 1.0 / Izero(beta);
ilength = 1.0 / (length - 0.5); /* last window value should be ibeta */
for (i = 0; i < length; i++) {
x = i + 0.5;
/* Kaiser window */
w = x * ilength;
w = 1.0 - w * w;
w = MAX(w, 0.0);
w = Izero(beta * sqrt(w)) * ibeta;
/* windowed ideal lowpass filter */
filter[i] = w * gain * sin(cutoff * M_PI * x) / (M_PI * x);
}
}
static void LpFilter(double *c, long N, double frq, double Beta, long Num)
{
long i;
/* Calculate filter coeffs: */
c[0] = frq;
for (i=1; i<N; i++) {
double x = M_PI*(double)i/(double)(Num);
c[i] = sin(x*frq)/x;
}
if (Beta>2) { /* Apply Kaiser window to filter coeffs: */
double IBeta = 1.0/Izero(Beta);
for (i=1; i<N; i++) {
double x = (double)i / (double)(N);
c[i] *= Izero(Beta*sqrt(1.0-x*x)) * IBeta;
}
} else { /* Apply Nuttall window: */
for(i = 0; i < N; i++) {
double x = M_PI*i / N;
c[i] *= 0.36335819 + 0.4891775*cos(x) + 0.1365995*cos(2*x) + 0.0106411*cos(3*x);
}
}
}
////////////////////////////////////////////////////////////////////
// Create a FIR high Pass filter with scaled amplitude 'Scale'
// NumTaps if non-zero, forces filter design to be this number of taps
// Astop = Stopband Atenuation in dB (ie 40dB is 40dB stopband attenuation)
// Fpass = Highpass passband frequency in Hz
// Fstop = Highpass stopband frequency in Hz
// Fsamprate = Sample Rate in Hz
//
// Apass -------------
// /
// /
// /
// /
// Astop ---------
// Fstop Fpass
////////////////////////////////////////////////////////////////////
int CFir::InitHPFilter(int NumTaps, TYPEREAL Scale, TYPEREAL Astop, TYPEREAL Fpass, TYPEREAL Fstop, TYPEREAL Fsamprate)
{
int n;
TYPEREAL Beta;
//m_Mutex.lock();
m_SampleRate = Fsamprate;
//create normalized frequency parameters
TYPEREAL normFpass = Fpass/Fsamprate;
TYPEREAL normFstop = Fstop/Fsamprate;
TYPEREAL normFcut = (normFstop + normFpass)/2.0; //high pass filter 6dB cutoff
//calculate Kaiser-Bessel window shape factor, Beta, from stopband attenuation
if(Astop < 20.96)
Beta = 0;
else if(Astop >= 50.0)
Beta = .1102 * (Astop - 8.71);
else
Beta = .5842 * pow( (Astop-20.96), 0.4) + .07886 * (Astop - 20.96);
//Now Estimate number of filter taps required based on filter specs
m_NumTaps = (Astop - 8.0) / (2.285*K_2PI*(normFpass - normFstop ) ) + 1;
//clamp range of filter taps
if(m_NumTaps>(MAX_NUMCOEF-1) )
m_NumTaps = MAX_NUMCOEF-1;
if(m_NumTaps < 3)
m_NumTaps = 3;
m_NumTaps |= 1; //force to next odd number
if(NumTaps) //if need to force to to a number of taps
m_NumTaps = NumTaps;
TYPEREAL izb = Izero(Beta); //precalculate denominator since is same for all points
TYPEREAL fCenter = .5*(TYPEREAL)(m_NumTaps-1);
for( n=0; n < m_NumTaps; n++)
{
TYPEREAL x = (TYPEREAL)n - (TYPEREAL)(m_NumTaps-1)/2.0;
TYPEREAL c;
// create ideal Sinc() HP filter with normFcut
if( (TYPEREAL)n == fCenter ) //deal with odd size filter singularity where sin(0)/0==1
c = 1.0 - 2.0 * normFcut;
else
c = (TYPEREAL) (sin(K_PI*x)/(K_PI*x) - sin(K_2PI*x*normFcut)/(K_PI*x) );
//calculate Kaiser window and multiply to get coefficient
x = ((TYPEREAL)n - ((TYPEREAL)m_NumTaps-1.0)/2.0 ) / (((TYPEREAL)m_NumTaps-1.0)/2.0);
m_Coef[n] = Scale * c * Izero( Beta * sqrt(1 - (x*x) ) ) / izb;
}
//make a 2x length array for FIR flat calculation efficiency
for (n = 0; n < m_NumTaps; n++)
m_Coef[n+m_NumTaps] = m_Coef[n];
//copy into complex coef buffers
for (n = 0; n < m_NumTaps*2; n++)
{
m_ICoef[n] = m_Coef[n];
m_QCoef[n] = m_Coef[n];
}
//Initialize the FIR buffers and state
for(int i=0; i<m_NumTaps; i++)
{
m_rZBuf[i] = 0.0;
m_cZBuf[i].re = 0.0;
m_cZBuf[i].im = 0.0;
}
m_State = 0;
//m_Mutex.unlock();
#if 0 //debug hack to write m_Coef to a file for analysis
QDir::setCurrent("d:/");
QFile File;
File.setFileName("hpcoef.txt");
if(File.open(QIODevice::WriteOnly))
{
qDebug()<<"file Opened OK";
char Buf[256];
for(n=0; n<m_NumTaps; n++)
{
sprintf( Buf, "%g\r\n", m_Coef[n]);
File.write(Buf);
}
}
else
qDebug()<<"file Failed to Open";
qDebug()<<"HP taps="<<m_NumTaps;
#endif
return m_NumTaps;
}
