Permalink
773 lines (623 sloc) 20.9 KB
/* -*- c++ -*- */
/*
* Copyright 2002,2007,2008,2012,2013,2018 Free Software Foundation, Inc.
*
* This file is part of GNU Radio
*
* GNU Radio is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3, or (at your option)
* any later version.
*
* GNU Radio is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNU Radio; see the file COPYING. If not, write to
* the Free Software Foundation, Inc., 51 Franklin Street,
* Boston, MA 02110-1301, USA.
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <gnuradio/filter/firdes.h>
#include <gnuradio/math.h>
#include <stdexcept>
using std::vector;
namespace gr {
namespace filter {
std::vector<float>
firdes::window(win_type type, int ntaps, double beta)
{
return fft::window::build(static_cast<fft::window::win_type>(type), ntaps, beta);
}
//
// === Low Pass ===
//
vector<float>
firdes::low_pass_2(double gain,
double sampling_freq, // Hz
double cutoff_freq, // Hz BEGINNING of transition band
double transition_width, // Hz width of transition band
double attenuation_dB, // attenuation dB
win_type window_type,
double beta) // used only with Kaiser
{
sanity_check_1f(sampling_freq, cutoff_freq, transition_width);
int ntaps = compute_ntaps_windes(sampling_freq,
transition_width,
attenuation_dB);
// construct the truncated ideal impulse response
// [sin(x)/x for the low pass case]
vector<float> taps(ntaps);
vector<float> w = window(window_type, ntaps, beta);
int M = (ntaps - 1) / 2;
double fwT0 = 2 * GR_M_PI * cutoff_freq / sampling_freq;
for(int n = -M; n <= M; n++) {
if (n == 0)
taps[n + M] = fwT0 / GR_M_PI * w[n + M];
else {
// a little algebra gets this into the more familiar sin(x)/x form
taps[n + M] = sin(n * fwT0) / (n * GR_M_PI) * w[n + M];
}
}
// find the factor to normalize the gain, fmax.
// For low-pass, gain @ zero freq = 1.0
double fmax = taps[0 + M];
for(int n = 1; n <= M; n++)
fmax += 2 * taps[n + M];
gain /= fmax; // normalize
for(int i = 0; i < ntaps; i++)
taps[i] *= gain;
return taps;
}
vector<float>
firdes::low_pass(double gain,
double sampling_freq,
double cutoff_freq, // Hz center of transition band
double transition_width, // Hz width of transition band
win_type window_type,
double beta) // used only with Kaiser
{
sanity_check_1f(sampling_freq, cutoff_freq, transition_width);
int ntaps = compute_ntaps(sampling_freq,
transition_width,
window_type, beta);
// construct the truncated ideal impulse response
// [sin(x)/x for the low pass case]
vector<float> taps(ntaps);
vector<float> w = window(window_type, ntaps, beta);
int M = (ntaps - 1) / 2;
double fwT0 = 2 * GR_M_PI * cutoff_freq / sampling_freq;
for(int n = -M; n <= M; n++) {
if(n == 0)
taps[n + M] = fwT0 / GR_M_PI * w[n + M];
else {
// a little algebra gets this into the more familiar sin(x)/x form
taps[n + M] = sin(n * fwT0) / (n * GR_M_PI) * w[n + M];
}
}
// find the factor to normalize the gain, fmax.
// For low-pass, gain @ zero freq = 1.0
double fmax = taps[0 + M];
for(int n = 1; n <= M; n++)
fmax += 2 * taps[n + M];
gain /= fmax; // normalize
for(int i = 0; i < ntaps; i++)
taps[i] *= gain;
return taps;
}
//
// === High Pass ===
//
vector<float>
firdes::high_pass_2(double gain,
double sampling_freq,
double cutoff_freq, // Hz center of transition band
double transition_width, // Hz width of transition band
double attenuation_dB, // attenuation dB
win_type window_type,
double beta) // used only with Kaiser
{
sanity_check_1f(sampling_freq, cutoff_freq, transition_width);
int ntaps = compute_ntaps_windes(sampling_freq,
transition_width,
attenuation_dB);
// construct the truncated ideal impulse response times the window function
vector<float> taps(ntaps);
vector<float> w = window(window_type, ntaps, beta);
int M = (ntaps - 1) / 2;
double fwT0 = 2 * GR_M_PI * cutoff_freq / sampling_freq;
for(int n = -M; n <= M; n++) {
if(n == 0)
taps[n + M] = (1 - (fwT0 / GR_M_PI)) * w[n + M];
else {
// a little algebra gets this into the more familiar sin(x)/x form
taps[n + M] = -sin(n * fwT0) / (n * GR_M_PI) * w[n + M];
}
}
// find the factor to normalize the gain, fmax.
// For high-pass, gain @ fs/2 freq = 1.0
double fmax = taps[0 + M];
for(int n = 1; n <= M; n++)
fmax += 2 * taps[n + M] * cos(n * GR_M_PI);
gain /= fmax; // normalize
for(int i = 0; i < ntaps; i++)
taps[i] *= gain;
return taps;
}
vector<float>
firdes::high_pass(double gain,
double sampling_freq,
double cutoff_freq, // Hz center of transition band
double transition_width, // Hz width of transition band
win_type window_type,
double beta) // used only with Kaiser
{
sanity_check_1f(sampling_freq, cutoff_freq, transition_width);
int ntaps = compute_ntaps(sampling_freq,
transition_width,
window_type, beta);
// construct the truncated ideal impulse response times the window function
vector<float> taps(ntaps);
vector<float> w = window(window_type, ntaps, beta);
int M = (ntaps - 1) / 2;
double fwT0 = 2 * GR_M_PI * cutoff_freq / sampling_freq;
for(int n = -M; n <= M; n++) {
if(n == 0)
taps[n + M] = (1 - (fwT0 / GR_M_PI)) * w[n + M];
else {
// a little algebra gets this into the more familiar sin(x)/x form
taps[n + M] = -sin(n * fwT0) / (n * GR_M_PI) * w[n + M];
}
}
// find the factor to normalize the gain, fmax.
// For high-pass, gain @ fs/2 freq = 1.0
double fmax = taps[0 + M];
for(int n = 1; n <= M; n++)
fmax += 2 * taps[n + M] * cos(n * GR_M_PI);
gain /= fmax; // normalize
for(int i = 0; i < ntaps; i++)
taps[i] *= gain;
return taps;
}
//
// === Band Pass ===
//
vector<float>
firdes::band_pass_2(double gain,
double sampling_freq,
double low_cutoff_freq, // Hz center of transition band
double high_cutoff_freq, // Hz center of transition band
double transition_width, // Hz width of transition band
double attenuation_dB, // attenuation dB
win_type window_type,
double beta) // used only with Kaiser
{
sanity_check_2f(sampling_freq,
low_cutoff_freq,
high_cutoff_freq, transition_width);
int ntaps = compute_ntaps_windes(sampling_freq,
transition_width,
attenuation_dB);
vector<float> taps(ntaps);
vector<float> w = window(window_type, ntaps, beta);
int M = (ntaps - 1) / 2;
double fwT0 = 2 * GR_M_PI * low_cutoff_freq / sampling_freq;
double fwT1 = 2 * GR_M_PI * high_cutoff_freq / sampling_freq;
for(int n = -M; n <= M; n++) {
if (n == 0)
taps[n + M] = (fwT1 - fwT0) / GR_M_PI * w[n + M];
else {
taps[n + M] = (sin(n * fwT1) - sin(n * fwT0)) / (n * GR_M_PI) * w[n + M];
}
}
// find the factor to normalize the gain, fmax.
// For band-pass, gain @ center freq = 1.0
double fmax = taps[0 + M];
for(int n = 1; n <= M; n++)
fmax += 2 * taps[n + M] * cos(n * (fwT0 + fwT1) * 0.5);
gain /= fmax; // normalize
for(int i = 0; i < ntaps; i++)
taps[i] *= gain;
return taps;
}
vector<float>
firdes::band_pass(double gain,
double sampling_freq,
double low_cutoff_freq, // Hz center of transition band
double high_cutoff_freq, // Hz center of transition band
double transition_width, // Hz width of transition band
win_type window_type,
double beta) // used only with Kaiser
{
sanity_check_2f(sampling_freq,
low_cutoff_freq,
high_cutoff_freq,
transition_width);
int ntaps = compute_ntaps(sampling_freq,
transition_width,
window_type, beta);
// construct the truncated ideal impulse response times the window function
vector<float> taps(ntaps);
vector<float> w = window(window_type, ntaps, beta);
int M = (ntaps - 1) / 2;
double fwT0 = 2 * GR_M_PI * low_cutoff_freq / sampling_freq;
double fwT1 = 2 * GR_M_PI * high_cutoff_freq / sampling_freq;
for(int n = -M; n <= M; n++) {
if (n == 0)
taps[n + M] = (fwT1 - fwT0) / GR_M_PI * w[n + M];
else {
taps[n + M] = (sin(n * fwT1) - sin(n * fwT0)) / (n * GR_M_PI) * w[n + M];
}
}
// find the factor to normalize the gain, fmax.
// For band-pass, gain @ center freq = 1.0
double fmax = taps[0 + M];
for(int n = 1; n <= M; n++)
fmax += 2 * taps[n + M] * cos(n * (fwT0 + fwT1) * 0.5);
gain /= fmax; // normalize
for(int i = 0; i < ntaps; i++)
taps[i] *= gain;
return taps;
}
//
// === Complex Band Pass ===
//
vector<gr_complex>
firdes::complex_band_pass_2(double gain,
double sampling_freq,
double low_cutoff_freq, // Hz center of transition band
double high_cutoff_freq, // Hz center of transition band
double transition_width, // Hz width of transition band
double attenuation_dB, // attenuation dB
win_type window_type,
double beta) // used only with Kaiser
{
sanity_check_2f_c(sampling_freq,
low_cutoff_freq,
high_cutoff_freq,
transition_width);
int ntaps = compute_ntaps_windes(sampling_freq,
transition_width,
attenuation_dB);
vector<gr_complex> taps(ntaps);
vector<float> lptaps(ntaps);
vector<float> w = window(window_type, ntaps, beta);
lptaps = low_pass_2(gain, sampling_freq,
(high_cutoff_freq - low_cutoff_freq)/2,
transition_width, attenuation_dB,
window_type, beta);
gr_complex *optr = &taps[0];
float *iptr = &lptaps[0];
float freq = GR_M_PI * (high_cutoff_freq + low_cutoff_freq)/sampling_freq;
float phase = 0;
if (lptaps.size() & 01) {
phase = - freq * ( lptaps.size() >> 1 );
}
else
phase = - freq/2.0 * ((1 + 2*lptaps.size()) >> 1);
for(unsigned int i = 0; i < lptaps.size(); i++) {
*optr++ = gr_complex(*iptr * cos(phase), *iptr * sin(phase));
iptr++, phase += freq;
}
return taps;
}
vector<gr_complex>
firdes::complex_band_pass(double gain,
double sampling_freq,
double low_cutoff_freq, // Hz center of transition band
double high_cutoff_freq, // Hz center of transition band
double transition_width, // Hz width of transition band
win_type window_type,
double beta) // used only with Kaiser
{
sanity_check_2f_c (sampling_freq,
low_cutoff_freq,
high_cutoff_freq,
transition_width);
int ntaps = compute_ntaps(sampling_freq,
transition_width,
window_type, beta);
// construct the truncated ideal impulse response times the window function
vector<gr_complex> taps(ntaps);
vector<float> lptaps(ntaps);
vector<float> w = window(window_type, ntaps, beta);
lptaps = low_pass(gain, sampling_freq,
(high_cutoff_freq - low_cutoff_freq)/2,
transition_width, window_type, beta);
gr_complex *optr = &taps[0];
float *iptr = &lptaps[0];
float freq = GR_M_PI * (high_cutoff_freq + low_cutoff_freq)/sampling_freq;
float phase = 0;
if(lptaps.size() & 01) {
phase = - freq * ( lptaps.size() >> 1 );
}
else
phase = - freq/2.0 * ((1 + 2*lptaps.size()) >> 1);
for(unsigned int i=0;i<lptaps.size();i++) {
*optr++ = gr_complex(*iptr * cos(phase), *iptr * sin(phase));
iptr++, phase += freq;
}
return taps;
}
//
// === Band Reject ===
//
vector<float>
firdes::band_reject_2(double gain,
double sampling_freq,
double low_cutoff_freq, // Hz center of transition band
double high_cutoff_freq, // Hz center of transition band
double transition_width, // Hz width of transition band
double attenuation_dB, // attenuation dB
win_type window_type,
double beta) // used only with Kaiser
{
sanity_check_2f(sampling_freq,
low_cutoff_freq,
high_cutoff_freq,
transition_width);
int ntaps = compute_ntaps_windes(sampling_freq,
transition_width,
attenuation_dB);
// construct the truncated ideal impulse response times the window function
vector<float> taps(ntaps);
vector<float> w = window(window_type, ntaps, beta);
int M = (ntaps - 1) / 2;
double fwT0 = 2 * GR_M_PI * low_cutoff_freq / sampling_freq;
double fwT1 = 2 * GR_M_PI * high_cutoff_freq / sampling_freq;
for(int n = -M; n <= M; n++) {
if (n == 0)
taps[n + M] = 1.0 + ((fwT0 - fwT1) / GR_M_PI * w[n + M]);
else {
taps[n + M] = (sin(n * fwT0) - sin(n * fwT1)) / (n * GR_M_PI) * w[n + M];
}
}
// find the factor to normalize the gain, fmax.
// For band-reject, gain @ zero freq = 1.0
double fmax = taps[0 + M];
for(int n = 1; n <= M; n++)
fmax += 2 * taps[n + M];
gain /= fmax; // normalize
for(int i = 0; i < ntaps; i++)
taps[i] *= gain;
return taps;
}
vector<float>
firdes::band_reject(double gain,
double sampling_freq,
double low_cutoff_freq, // Hz center of transition band
double high_cutoff_freq, // Hz center of transition band
double transition_width, // Hz width of transition band
win_type window_type,
double beta) // used only with Kaiser
{
sanity_check_2f(sampling_freq,
low_cutoff_freq,
high_cutoff_freq,
transition_width);
int ntaps = compute_ntaps(sampling_freq,
transition_width,
window_type, beta);
// construct the truncated ideal impulse response times the window function
vector<float> taps(ntaps);
vector<float> w = window(window_type, ntaps, beta);
int M = (ntaps - 1) / 2;
double fwT0 = 2 * GR_M_PI * low_cutoff_freq / sampling_freq;
double fwT1 = 2 * GR_M_PI * high_cutoff_freq / sampling_freq;
for(int n = -M; n <= M; n++) {
if (n == 0)
taps[n + M] = 1.0 + ((fwT0 - fwT1) / GR_M_PI * w[n + M]);
else {
taps[n + M] = (sin(n * fwT0) - sin(n * fwT1)) / (n * GR_M_PI) * w[n + M];
}
}
// find the factor to normalize the gain, fmax.
// For band-reject, gain @ zero freq = 1.0
double fmax = taps[0 + M];
for(int n = 1; n <= M; n++)
fmax += 2 * taps[n + M];
gain /= fmax; // normalize
for(int i = 0; i < ntaps; i++)
taps[i] *= gain;
return taps;
}
//
// Hilbert Transform
//
vector<float>
firdes::hilbert(unsigned int ntaps,
win_type windowtype,
double beta)
{
if(!(ntaps & 1))
throw std::out_of_range("Hilbert: Must have odd number of taps");
vector<float> taps(ntaps);
vector<float> w = window (windowtype, ntaps, beta);
unsigned int h = (ntaps-1)/2;
float gain=0;
for(unsigned int i = 1; i <= h; i++) {
if(i & 1) {
float x = 1/(float)i;
taps[h+i] = x * w[h+i];
taps[h-i] = -x * w[h-i];
gain = taps[h+i] - gain;
}
else
taps[h+i] = taps[h-i] = 0;
}
gain = 2 * fabs(gain);
for(unsigned int i = 0; i < ntaps; i++)
taps[i] /= gain;
return taps;
}
//
// Gaussian
//
vector<float>
firdes::gaussian(double gain,
double spb,
double bt,
int ntaps)
{
vector<float> taps(ntaps);
double scale = 0;
double dt = 1.0/spb;
double s = 1.0/(sqrt(log(2.0)) / (2*GR_M_PI*bt));
double t0 = -0.5 * ntaps;
double ts;
for(int i=0;i<ntaps;i++) {
t0++;
ts = s*dt*t0;
taps[i] = exp(-0.5*ts*ts);
scale += taps[i];
}
for(int i=0;i<ntaps;i++)
taps[i] = taps[i] / scale * gain;
return taps;
}
//
// Root Raised Cosine
//
vector<float>
firdes::root_raised_cosine(double gain,
double sampling_freq,
double symbol_rate,
double alpha,
int ntaps)
{
ntaps |= 1; // ensure that ntaps is odd
double spb = sampling_freq/symbol_rate; // samples per bit/symbol
vector<float> taps(ntaps);
double scale = 0;
for(int i = 0; i < ntaps; i++) {
double x1,x2,x3,num,den;
double xindx = i - ntaps/2;
x1 = GR_M_PI * xindx/spb;
x2 = 4 * alpha * xindx / spb;
x3 = x2*x2 - 1;
if(fabs(x3) >= 0.000001) { // Avoid Rounding errors...
if(i != ntaps/2)
num = cos((1+alpha)*x1) + sin((1-alpha)*x1)/(4*alpha*xindx/spb);
else
num = cos((1+alpha)*x1) + (1-alpha) * GR_M_PI / (4*alpha);
den = x3 * GR_M_PI;
}
else {
if(alpha==1) {
taps[i] = -1;
continue;
}
x3 = (1-alpha)*x1;
x2 = (1+alpha)*x1;
num = (sin(x2)*(1+alpha)*GR_M_PI
- cos(x3)*((1-alpha)*GR_M_PI*spb)/(4*alpha*xindx)
+ sin(x3)*spb*spb/(4*alpha*xindx*xindx));
den = -32 * GR_M_PI * alpha * alpha * xindx/spb;
}
taps[i] = 4 * alpha * num / den;
scale += taps[i];
}
for(int i = 0; i < ntaps; i++)
taps[i] = taps[i] * gain / scale;
return taps;
}
//
// === Utilities ===
//
int
firdes::compute_ntaps_windes(double sampling_freq,
double transition_width, // this is frequency, not relative frequency
double attenuation_dB)
{
// Based on formula from Multirate Signal Processing for
// Communications Systems, fredric j harris
int ntaps = (int)(attenuation_dB*sampling_freq/(22.0*transition_width));
if ((ntaps & 1) == 0) // if even...
ntaps++; // ...make odd
return ntaps;
}
int
firdes::compute_ntaps(double sampling_freq,
double transition_width,
win_type window_type,
double beta)
{
double a = fft::window::max_attenuation(static_cast<fft::window::win_type>(window_type), beta);
int ntaps = (int)(a*sampling_freq/(22.0*transition_width));
if((ntaps & 1) == 0) // if even...
ntaps++; // ...make odd
return ntaps;
}
double
firdes::bessi0(double x)
{
double ax,ans;
double y;
ax=fabs(x);
if (ax < 3.75) {
y=x/3.75;
y*=y;
ans=1.0+y*(3.5156229+y*(3.0899424+y*(1.2067492
+y*(0.2659732+y*(0.360768e-1+y*0.45813e-2)))));
}
else {
y=3.75/ax;
ans=(exp(ax)/sqrt(ax))*(0.39894228+y*(0.1328592e-1
+y*(0.225319e-2+y*(-0.157565e-2+y*(0.916281e-2
+y*(-0.2057706e-1+y*(0.2635537e-1+y*(-0.1647633e-1
+y*0.392377e-2))))))));
}
return ans;
}
void
firdes::sanity_check_1f(double sampling_freq,
double fa, // cutoff freq
double transition_width)
{
if(sampling_freq <= 0.0)
throw std::out_of_range("firdes check failed: sampling_freq > 0");
if(fa <= 0.0 || fa > sampling_freq / 2)
throw std::out_of_range("firdes check failed: 0 < fa <= sampling_freq / 2");
if(transition_width <= 0)
throw std::out_of_range("firdes check failed: transition_width > 0");
}
void
firdes::sanity_check_2f(double sampling_freq,
double fa, // first cutoff freq
double fb, // second cutoff freq
double transition_width)
{
if (sampling_freq <= 0.0)
throw std::out_of_range("firdes check failed: sampling_freq > 0");
if (fa <= 0.0 || fa > sampling_freq / 2)
throw std::out_of_range("firdes check failed: 0 < fa <= sampling_freq / 2");
if (fb <= 0.0 || fb > sampling_freq / 2)
throw std::out_of_range("firdes check failed: 0 < fb <= sampling_freq / 2");
if (fa > fb)
throw std::out_of_range("firdes check failed: fa <= fb");
if (transition_width <= 0)
throw std::out_of_range("firdes check failed: transition_width > 0");
}
void
firdes::sanity_check_2f_c(double sampling_freq,
double fa, // first cutoff freq
double fb, // second cutoff freq
double transition_width)
{
if(sampling_freq <= 0.0)
throw std::out_of_range("firdes check failed: sampling_freq > 0");
if(fa < -sampling_freq / 2 || fa > sampling_freq / 2)
throw std::out_of_range("firdes check failed: 0 < fa <= sampling_freq / 2");
if(fb < -sampling_freq / 2 || fb > sampling_freq / 2)
throw std::out_of_range("firdes check failed: 0 < fb <= sampling_freq / 2");
if(fa > fb)
throw std::out_of_range("firdes check failed: fa <= fb");
if(transition_width <= 0)
throw std::out_of_range("firdes check failed: transition_width > 0");
}
} /* namespace filter */
} /* namespace gr */