You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
675 lines
18 KiB
675 lines
18 KiB
/////////////////////////////////////////////////////////////////////////////////////// |
|
// Copyright (C) 2012 maintech GmbH, Otto-Hahn-Str. 15, 97204 Hoechberg, Germany // |
|
// written by Christian Daniel // |
|
// Copyright (C) 2014-2015 John Greb <hexameron@spam.no> // |
|
// Copyright (C) 2015, 2017-2018, 2020, 2022-2023 Edouard Griffiths, F4EXB <f4exb06@gmail.com> // |
|
// Copyright (C) 2020 Kacper Michajłow <kasper93@gmail.com> // |
|
// // |
|
// This program 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 as version 3 of the License, or // |
|
// (at your option) any later version. // |
|
// // |
|
// This program 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 V3 for more details. // |
|
// // |
|
// You should have received a copy of the GNU General Public License // |
|
// along with this program. If not, see <http://www.gnu.org/licenses/>. // |
|
/////////////////////////////////////////////////////////////////////////////////////// |
|
// ---------------------------------------------------------------------------- |
|
// fftfilt.cxx -- Fast convolution Overlap-Add filter |
|
// |
|
// Filter implemented using overlap-add FFT convolution method |
|
// h(t) characterized by Windowed-Sinc impulse response |
|
// |
|
// Reference: |
|
// "The Scientist and Engineer's Guide to Digital Signal Processing" |
|
// by Dr. Steven W. Smith, http://www.dspguide.com |
|
// Chapters 16, 18 and 21 |
|
// |
|
// Copyright (C) 2006-2008 Dave Freese, W1HKJ |
|
// |
|
// This file is part of fldigi. |
|
// |
|
// Fldigi 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 of the License, or |
|
// (at your option) any later version. |
|
// |
|
// Fldigi 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 fldigi. If not, see <http://www.gnu.org/licenses/>. |
|
// |
|
// Augmented with more filter types |
|
// Copyright (C) 2015-2022 Edouard Griffiths, F4EXB |
|
// ---------------------------------------------------------------------------- |
|
|
|
#include <memory.h> |
|
#include <algorithm> |
|
#include <iostream> |
|
#include <fstream> |
|
#include <cstdlib> |
|
#include <cmath> |
|
#include <typeinfo> |
|
#include <array> |
|
|
|
#include <stdio.h> |
|
#include <sys/types.h> |
|
|
|
#include <dsp/misc.h> |
|
#include <dsp/fftfilt.h> |
|
|
|
//------------------------------------------------------------------------------ |
|
// initialize the filter |
|
// create forward and reverse FFTs |
|
//------------------------------------------------------------------------------ |
|
|
|
// Only need a single instance of g_fft, used for both forward and reverse |
|
void fftfilt::init_filter() |
|
{ |
|
flen2 = flen >> 1; |
|
fft = new g_fft<float>(flen); |
|
|
|
filter = new cmplx[flen]; |
|
filterOpp = new cmplx[flen]; |
|
data = new cmplx[flen]; |
|
output = new cmplx[flen2]; |
|
ovlbuf = new cmplx[flen2]; |
|
|
|
std::fill(filter, filter + flen, cmplx{0, 0}); |
|
std::fill(filterOpp, filterOpp + flen, cmplx{0, 0}); |
|
std::fill(data, data + flen , cmplx{0, 0}); |
|
std::fill(output, output + flen2, cmplx{0, 0}); |
|
std::fill(ovlbuf, ovlbuf + flen2, cmplx{0, 0}); |
|
|
|
inptr = 0; |
|
} |
|
|
|
//------------------------------------------------------------------------------ |
|
// fft filter |
|
// f1 < f2 ==> band pass filter |
|
// f1 > f2 ==> band reject filter |
|
// f1 == 0 ==> low pass filter |
|
// f2 == 0 ==> high pass filter |
|
//------------------------------------------------------------------------------ |
|
fftfilt::fftfilt(int len) : |
|
m_dnrScheme(FFTNoiseReduction::SchemeAverage), |
|
m_dnrAboveAvgFactor(0.0f), |
|
m_dnrSigmaFactor(0.0f), |
|
m_dnrNbPeaks(0), |
|
m_noiseReduction(len) |
|
{ |
|
flen = len; |
|
pass = 0; |
|
window = 0; |
|
m_dnr = false; |
|
init_filter(); |
|
} |
|
|
|
fftfilt::fftfilt(float f1, float f2, int len) : |
|
m_dnrScheme(FFTNoiseReduction::SchemeAverage), |
|
m_dnrAboveAvgFactor(0.0f), |
|
m_dnrSigmaFactor(0.0f), |
|
m_dnrNbPeaks(0), |
|
m_noiseReduction(len) |
|
{ |
|
flen = len; |
|
pass = 0; |
|
window = 0; |
|
m_dnr = false; |
|
init_filter(); |
|
create_filter(f1, f2); |
|
} |
|
|
|
fftfilt::fftfilt(float f2, int len) : |
|
m_dnrScheme(FFTNoiseReduction::SchemeAverage), |
|
m_dnrAboveAvgFactor(0.0f), |
|
m_dnrSigmaFactor(0.0f), |
|
m_dnrNbPeaks(0), |
|
m_noiseReduction(len) |
|
{ |
|
flen = len; |
|
pass = 0; |
|
window = 0; |
|
m_dnr = false; |
|
init_filter(); |
|
create_dsb_filter(f2); |
|
} |
|
|
|
fftfilt::~fftfilt() |
|
{ |
|
if (fft) delete fft; |
|
|
|
if (filter) delete [] filter; |
|
if (filterOpp) delete [] filterOpp; |
|
if (data) delete [] data; |
|
if (output) delete [] output; |
|
if (ovlbuf) delete [] ovlbuf; |
|
} |
|
|
|
void fftfilt::create_filter(float f1, float f2, FFTWindow::Function wf) |
|
{ |
|
// initialize the filter to zero |
|
std::fill(filter, filter + flen, cmplx{0, 0}); |
|
|
|
// create the filter shape coefficients by fft |
|
bool b_lowpass, b_highpass; |
|
b_lowpass = (f2 != 0); |
|
b_highpass = (f1 != 0); |
|
|
|
for (int i = 0; i < flen2; i++) { |
|
filter[i] = 0; |
|
// lowpass @ f2 |
|
if (b_lowpass) |
|
filter[i] += fsinc(f2, i, flen2); |
|
// highighpass @ f1 |
|
if (b_highpass) |
|
filter[i] -= fsinc(f1, i, flen2); |
|
} |
|
// highpass is delta[flen2/2] - h(t) |
|
if (b_highpass && f2 < f1) |
|
filter[flen2 / 2] += 1; |
|
|
|
FFTWindow fwin; |
|
fwin.create(wf, flen2); |
|
fwin.apply(filter); |
|
|
|
// for (int i = 0; i < flen2; i++) |
|
// filter[i] *= _blackman(i, flen2); |
|
|
|
fft->ComplexFFT(filter); // filter was expressed in the time domain (impulse response) |
|
|
|
// normalize the output filter for unity gain |
|
float scale = 0, mag; |
|
for (int i = 0; i < flen2; i++) { |
|
mag = abs(filter[i]); |
|
if (mag > scale) scale = mag; |
|
} |
|
if (scale != 0) { |
|
for (int i = 0; i < flen; i++) |
|
filter[i] /= scale; |
|
} |
|
} |
|
|
|
void fftfilt::create_filter(const std::vector<std::pair<float, float>>& limits, bool pass, FFTWindow::Function wf) |
|
{ |
|
std::vector<int> canvasNeg(flen2, pass ? 0 : 1); // initialize the negative frequencies filter canvas |
|
std::vector<int> canvasPos(flen2, pass ? 0 : 1); // initialize the positive frequencies filter canvas |
|
std::fill(filter, filter + flen, cmplx{0, 0}); // initialize the positive filter to zero |
|
std::fill(filterOpp, filterOpp + flen, cmplx{0, 0}); // initialize the negative filter to zero |
|
|
|
for (const auto& fs : limits) |
|
{ |
|
const float& f1 = fs.first + 0.5; |
|
const float& w = fs.second > 0.0 ? fs.second : 0.0; |
|
const float& f2 = f1 + w; |
|
|
|
for (int i = 0; i < flen; i++) |
|
{ |
|
if (pass) // pass |
|
{ |
|
if ((i >= f1*flen) && (i <= f2*flen)) |
|
{ |
|
if (i < flen2) { |
|
canvasNeg[flen2-1-i] = 1; |
|
} else { |
|
canvasPos[i-flen2] = 1; |
|
} |
|
} |
|
} |
|
else // reject |
|
{ |
|
if ((i >= f1*flen) && (i <= f2*flen)) |
|
{ |
|
if (i < flen2) { |
|
canvasNeg[flen2-1-i] = 0; |
|
} else { |
|
canvasPos[i-flen2] = 0; |
|
} |
|
} |
|
} |
|
} |
|
} |
|
|
|
std::vector<std::pair<int,int>> indexesNegList; |
|
std::vector<std::pair<int,int>> indexesPosList; |
|
int cn = 0; |
|
int cp = 0; |
|
int defaultSecond = pass ? 0 : flen2 - 1; |
|
|
|
for (int i = 0; i < flen2; i++) |
|
{ |
|
if ((canvasNeg[i] == 1) && (cn == 0)) { |
|
indexesNegList.push_back(std::pair<int,int>{i, defaultSecond}); |
|
} |
|
|
|
if ((canvasNeg[i] == 0) && (cn == 1)) { |
|
indexesNegList.back().second = i; |
|
} |
|
|
|
if ((canvasPos[i] == 1) && (cp == 0)) { |
|
indexesPosList.push_back(std::pair<int,int>{i, defaultSecond}); |
|
} |
|
|
|
if ((canvasPos[i] == 0) && (cp == 1)) { |
|
indexesPosList.back().second = i; |
|
} |
|
|
|
cn = canvasNeg[i]; |
|
cp = canvasPos[i]; |
|
} |
|
|
|
for (const auto& indexes : indexesPosList) |
|
{ |
|
const float f1 = indexes.first / (float) flen; |
|
const float f2 = indexes.second / (float) flen; |
|
|
|
for (int i = 0; i < flen2; i++) |
|
{ |
|
if (f2 != 0) { |
|
filter[i] += fsinc(f2, i, flen2); |
|
} |
|
if (f1 != 0) { |
|
filter[i] -= fsinc(f1, i, flen2); |
|
} |
|
} |
|
|
|
if (f2 == 0 && f1 != 0) { |
|
filter[flen2 / 2] += 1; |
|
} |
|
} |
|
|
|
for (const auto& indexes : indexesNegList) |
|
{ |
|
const float f1 = indexes.first / (float) flen; |
|
const float f2 = indexes.second / (float) flen; |
|
|
|
for (int i = 0; i < flen2; i++) |
|
{ |
|
if (f2 != 0) { |
|
filterOpp[i] += fsinc(f2, i, flen2); |
|
} |
|
if (f1 != 0) { |
|
filterOpp[i] -= fsinc(f1, i, flen2); |
|
} |
|
} |
|
|
|
if (f2 == 0 && f1 != 0) { |
|
filterOpp[flen2 / 2] += 1; |
|
} |
|
} |
|
|
|
FFTWindow fwin; |
|
fwin.create(wf, flen2); |
|
fwin.apply(filter); |
|
fwin.apply(filterOpp); |
|
|
|
fft->ComplexFFT(filter); // filter was expressed in the time domain (impulse response) |
|
fft->ComplexFFT(filterOpp); // filter was expressed in the time domain (impulse response) |
|
|
|
float scalen = 0, scalep = 0, magn, magp; // normalize the output filter for unity gain |
|
|
|
for (int i = 0; i < flen2; i++) |
|
{ |
|
magp = abs(filter[i]); |
|
|
|
if (magp > scalep) { |
|
scalep = magp; |
|
} |
|
|
|
magn = abs(filterOpp[i]); |
|
|
|
if (magn > scalen) { |
|
scalen = magn; |
|
} |
|
} |
|
|
|
if (scalep != 0) |
|
{ |
|
std::for_each( |
|
filter, |
|
filter + flen, |
|
[scalep](fftfilt::cmplx& s) { s /= scalep; } |
|
); |
|
} |
|
|
|
if (scalen != 0) |
|
{ |
|
std::for_each( |
|
filterOpp, |
|
filterOpp + flen, |
|
[scalen](fftfilt::cmplx& s) { s /= scalen; } |
|
); |
|
} |
|
} |
|
|
|
// Double the size of FFT used for equivalent SSB filter or assume FFT is half the size of the one used for SSB |
|
void fftfilt::create_dsb_filter(float f2, FFTWindow::Function wf) |
|
{ |
|
// initialize the filter to zero |
|
std::fill(filter, filter + flen, cmplx{0, 0}); |
|
|
|
for (int i = 0; i < flen2; i++) { |
|
filter[i] = fsinc(f2, i, flen2); |
|
// filter[i] *= _blackman(i, flen2); |
|
} |
|
|
|
FFTWindow fwin; |
|
fwin.create(wf, flen2); |
|
fwin.apply(filter); |
|
|
|
fft->ComplexFFT(filter); // filter was expressed in the time domain (impulse response) |
|
|
|
// normalize the output filter for unity gain |
|
float scale = 0, mag; |
|
for (int i = 0; i < flen2; i++) { |
|
mag = abs(filter[i]); |
|
if (mag > scale) scale = mag; |
|
} |
|
if (scale != 0) { |
|
for (int i = 0; i < flen; i++) |
|
filter[i] /= scale; |
|
} |
|
} |
|
|
|
// Double the size of FFT used for equivalent SSB filter or assume FFT is half the size of the one used for SSB |
|
// used with runAsym for in band / opposite band asymmetrical filtering. Can be used for vestigial sideband modulation. |
|
void fftfilt::create_asym_filter(float fopp, float fin, FFTWindow::Function wf) |
|
{ |
|
// in band |
|
// initialize the filter to zero |
|
std::fill(filter, filter + flen, cmplx{0, 0}); |
|
|
|
for (int i = 0; i < flen2; i++) { |
|
filter[i] = fsinc(fin, i, flen2); |
|
// filter[i] *= _blackman(i, flen2); |
|
} |
|
|
|
FFTWindow fwin; |
|
fwin.create(wf, flen2); |
|
fwin.apply(filter); |
|
|
|
fft->ComplexFFT(filter); // filter was expressed in the time domain (impulse response) |
|
|
|
// normalize the output filter for unity gain |
|
float scale = 0, mag; |
|
for (int i = 0; i < flen2; i++) { |
|
mag = abs(filter[i]); |
|
if (mag > scale) scale = mag; |
|
} |
|
if (scale != 0) { |
|
for (int i = 0; i < flen; i++) |
|
filter[i] /= scale; |
|
} |
|
|
|
// opposite band |
|
// initialize the filter to zero |
|
std::fill(filterOpp, filterOpp + flen, cmplx{0, 0}); |
|
|
|
for (int i = 0; i < flen2; i++) { |
|
filterOpp[i] = fsinc(fopp, i, flen2); |
|
// filterOpp[i] *= _blackman(i, flen2); |
|
} |
|
|
|
fwin.apply(filterOpp); |
|
fft->ComplexFFT(filterOpp); // filter was expressed in the time domain (impulse response) |
|
|
|
// normalize the output filter for unity gain |
|
scale = 0; |
|
for (int i = 0; i < flen2; i++) { |
|
mag = abs(filterOpp[i]); |
|
if (mag > scale) scale = mag; |
|
} |
|
if (scale != 0) { |
|
for (int i = 0; i < flen; i++) |
|
filterOpp[i] /= scale; |
|
} |
|
} |
|
|
|
// test bypass |
|
int fftfilt::noFilt(const cmplx & in, cmplx **out) |
|
{ |
|
data[inptr++] = in; |
|
if (inptr < flen2) |
|
return 0; |
|
inptr = 0; |
|
|
|
*out = data; |
|
return flen2; |
|
} |
|
|
|
// Filter with fast convolution (overlap-add algorithm). |
|
int fftfilt::runFilt(const cmplx & in, cmplx **out) |
|
{ |
|
data[inptr++] = in; |
|
if (inptr < flen2) |
|
return 0; |
|
inptr = 0; |
|
|
|
fft->ComplexFFT(data); |
|
for (int i = 0; i < flen; i++) |
|
data[i] *= filter[i]; |
|
|
|
fft->InverseComplexFFT(data); |
|
|
|
for (int i = 0; i < flen2; i++) { |
|
output[i] = ovlbuf[i] + data[i]; |
|
ovlbuf[i] = data[flen2 + i]; |
|
} |
|
std::fill(data, data + flen , cmplx{0, 0}); |
|
|
|
*out = output; |
|
return flen2; |
|
} |
|
|
|
// Second version for single sideband |
|
int fftfilt::runSSB(const cmplx & in, cmplx **out, bool usb, bool getDC) |
|
{ |
|
data[inptr++] = in; |
|
if (inptr < flen2) |
|
return 0; |
|
inptr = 0; |
|
|
|
fft->ComplexFFT(data); |
|
|
|
// get or reject DC component |
|
data[0] = getDC ? data[0]*filter[0] : 0; |
|
m_noiseReduction.setScheme(m_dnrScheme); |
|
m_noiseReduction.init(); |
|
|
|
// Discard frequencies for ssb |
|
if (usb) |
|
{ |
|
for (int i = 1; i < flen2; i++) |
|
{ |
|
data[i] *= filter[i]; |
|
data[flen2 + i] = 0; |
|
|
|
if (m_dnr) |
|
{ |
|
m_noiseReduction.push(data[i], i); |
|
m_noiseReduction.push(data[flen2 + i], flen2 + i); |
|
} |
|
} |
|
} |
|
else |
|
{ |
|
for (int i = 1; i < flen2; i++) |
|
{ |
|
data[i] = 0; |
|
data[flen2 + i] *= filter[flen2 + i]; |
|
|
|
if (m_dnr) |
|
{ |
|
m_noiseReduction.push(data[i], i); |
|
m_noiseReduction.push(data[flen2 + i], flen2 + i); |
|
} |
|
} |
|
} |
|
|
|
if (m_dnr) |
|
{ |
|
m_noiseReduction.m_aboveAvgFactor = m_dnrAboveAvgFactor; |
|
m_noiseReduction.m_sigmaFactor = m_dnrSigmaFactor; |
|
m_noiseReduction.m_nbPeaks = m_dnrNbPeaks; |
|
m_noiseReduction.calc(); |
|
|
|
for (int i = 0; i < flen; i++) |
|
{ |
|
if (m_noiseReduction.cut(i)) { |
|
data[i] = 0; |
|
} |
|
} |
|
} |
|
|
|
// in-place FFT: freqdata overwritten with filtered timedata |
|
fft->InverseComplexFFT(data); |
|
|
|
// overlap and add |
|
for (int i = 0; i < flen2; i++) { |
|
output[i] = ovlbuf[i] + data[i]; |
|
ovlbuf[i] = data[i+flen2]; |
|
} |
|
std::fill(data, data + flen , cmplx{0, 0}); |
|
|
|
*out = output; |
|
return flen2; |
|
} |
|
|
|
// Version for double sideband. You have to double the FFT size used for SSB. |
|
int fftfilt::runDSB(const cmplx & in, cmplx **out, bool getDC) |
|
{ |
|
data[inptr++] = in; |
|
if (inptr < flen2) |
|
return 0; |
|
inptr = 0; |
|
|
|
fft->ComplexFFT(data); |
|
|
|
for (int i = 0; i < flen2; i++) { |
|
data[i] *= filter[i]; |
|
data[flen2 + i] *= filter[flen2 + i]; |
|
} |
|
|
|
// get or reject DC component |
|
data[0] = getDC ? data[0] : 0; |
|
|
|
// in-place FFT: freqdata overwritten with filtered timedata |
|
fft->InverseComplexFFT(data); |
|
|
|
// overlap and add |
|
for (int i = 0; i < flen2; i++) { |
|
output[i] = ovlbuf[i] + data[i]; |
|
ovlbuf[i] = data[i+flen2]; |
|
} |
|
|
|
std::fill(data, data + flen , cmplx{0, 0}); |
|
|
|
*out = output; |
|
return flen2; |
|
} |
|
|
|
// Version for asymmetrical sidebands. You have to double the FFT size used for SSB. |
|
int fftfilt::runAsym(const cmplx & in, cmplx **out, bool usb) |
|
{ |
|
data[inptr++] = in; |
|
if (inptr < flen2) |
|
return 0; |
|
inptr = 0; |
|
|
|
fft->ComplexFFT(data); |
|
|
|
data[0] *= filter[0]; // always keep DC |
|
|
|
if (usb) |
|
{ |
|
for (int i = 1; i < flen2; i++) |
|
{ |
|
data[i] *= filter[i]; // usb |
|
data[flen2 + i] *= filterOpp[flen2 + i]; // lsb is the opposite |
|
} |
|
} |
|
else |
|
{ |
|
for (int i = 1; i < flen2; i++) |
|
{ |
|
data[i] *= filterOpp[i]; // usb is the opposite |
|
data[flen2 + i] *= filter[flen2 + i]; // lsb |
|
} |
|
} |
|
|
|
// in-place FFT: freqdata overwritten with filtered timedata |
|
fft->InverseComplexFFT(data); |
|
|
|
// overlap and add |
|
for (int i = 0; i < flen2; i++) { |
|
output[i] = ovlbuf[i] + data[i]; |
|
ovlbuf[i] = data[i+flen2]; |
|
} |
|
|
|
std::fill(data, data + flen , cmplx{0, 0}); |
|
|
|
*out = output; |
|
return flen2; |
|
} |
|
|
|
/* Sliding FFT from Fldigi */ |
|
|
|
struct sfft::vrot_bins_pair { |
|
cmplx vrot; |
|
cmplx bins; |
|
} ; |
|
|
|
sfft::sfft(int len) |
|
{ |
|
vrot_bins = new vrot_bins_pair[len]; |
|
delay = new cmplx[len]; |
|
fftlen = len; |
|
first = 0; |
|
last = len - 1; |
|
ptr = 0; |
|
double phi = 0.0, tau = 2.0 * M_PI/ len; |
|
k2 = 1.0; |
|
for (int i = 0; i < len; i++) { |
|
vrot_bins[i].vrot = cmplx( K1 * cos (phi), K1 * sin (phi) ); |
|
phi += tau; |
|
delay[i] = vrot_bins[i].bins = 0.0; |
|
k2 *= K1; |
|
} |
|
} |
|
|
|
sfft::~sfft() |
|
{ |
|
delete [] vrot_bins; |
|
delete [] delay; |
|
} |
|
|
|
// Sliding FFT, cmplx input, cmplx output |
|
// FFT is computed for each value from first to last |
|
// Values are not stable until more than "len" samples have been processed. |
|
void sfft::run(const cmplx& input) |
|
{ |
|
cmplx & de = delay[ptr]; |
|
const cmplx z( input.real() - k2 * de.real(), input.imag() - k2 * de.imag()); |
|
de = input; |
|
|
|
if (++ptr >= fftlen) |
|
ptr = 0; |
|
|
|
for (vrot_bins_pair *itr = vrot_bins + first, *end = vrot_bins + last; itr != end ; ++itr) |
|
itr->bins = (itr->bins + z) * itr->vrot; |
|
} |
|
|
|
// Copies the frequencies to a pointer. |
|
void sfft::fetch(float *result) |
|
{ |
|
for (vrot_bins_pair *itr = vrot_bins, *end = vrot_bins + last; itr != end; ++itr, ++result) |
|
*result = itr->bins.real() * itr->bins.real() |
|
+ itr->bins.imag() * itr->bins.imag(); |
|
}
|
|
|