From f2e3059099aa9426333352cd2ed72c450c9ed0b1 Mon Sep 17 00:00:00 2001 From: f4exb Date: Sun, 9 Dec 2018 22:11:39 +0100 Subject: [PATCH] Reformat rational interpolator code --- sdrbase/dsp/interpolator.cpp | 90 +++++++++++++++++++++++++----------- sdrbase/dsp/interpolator.h | 44 ++++++++++++++---- 2 files changed, 96 insertions(+), 38 deletions(-) diff --git a/sdrbase/dsp/interpolator.cpp b/sdrbase/dsp/interpolator.cpp index a8ff5c88f..467ed0a48 100644 --- a/sdrbase/dsp/interpolator.cpp +++ b/sdrbase/dsp/interpolator.cpp @@ -28,31 +28,43 @@ void Interpolator::createPolyphaseLowPass( { int ntaps = (int)(nbTapsPerPhase * phaseSteps); qDebug("Interpolator::createPolyphaseLowPass: ntaps: %d", ntaps); - if((ntaps % 2) != 0) - ntaps++; + + if ((ntaps % 2) != 0) { + ntaps++; + } + ntaps *= phaseSteps; taps.resize(ntaps); std::vector window(ntaps); - for(int n = 0; n < ntaps; n++) - window[n] = 0.54 - 0.46 * cos ((2 * M_PI * n) / (ntaps - 1)); + for (int n = 0; n < ntaps; n++) { + window[n] = 0.54 - 0.46 * cos ((2 * M_PI * n) / (ntaps - 1)); + } int M = (ntaps - 1) / 2; double fwT0 = 2 * M_PI * cutoffFreqHz / sampleRateHz; - for(int n = -M; n <= M; n++) { - if(n == 0) taps[n + M] = fwT0 / M_PI * window[n + M]; - else taps[n + M] = sin (n * fwT0) / (n * M_PI) * window[n + M]; + + for (int n = -M; n <= M; n++) + { + if (n == 0) { + taps[n + M] = fwT0 / M_PI * window[n + M]; + } else { + taps[n + M] = sin (n * fwT0) / (n * M_PI) * window[n + M]; + } } double max = taps[0 + M]; - for(int n = 1; n <= M; n++) - max += 2.0 * taps[n + M]; + + for (int n = 1; n <= M; n++) { + max += 2.0 * taps[n + M]; + } gain /= max; - for(int i = 0; i < ntaps; i++) - taps[i] *= gain; + for (int i = 0; i < ntaps; i++) { + taps[i] *= gain; + } } Interpolator::Interpolator() : @@ -90,39 +102,60 @@ void Interpolator::create(int phaseSteps, double sampleRate, double cutoff, doub m_nTaps = taps.size() / phaseSteps; m_phaseSteps = phaseSteps; m_samples.resize(m_nTaps + 2); - for(int i = 0; i < m_nTaps + 2; i++) - m_samples[i] = 0; + + for (int i = 0; i < m_nTaps + 2; i++) { + m_samples[i] = 0; + } // reorder into polyphase std::vector polyphase(taps.size()); - for(int phase = 0; phase < phaseSteps; phase++) { - for(int i = 0; i < m_nTaps; i++) - polyphase[phase * m_nTaps + i] = taps[i * phaseSteps + phase]; + + for (int phase = 0; phase < phaseSteps; phase++) + { + for (int i = 0; i < m_nTaps; i++) { + polyphase[phase * m_nTaps + i] = taps[i * phaseSteps + phase]; + } } // normalize phase filters - for(int phase = 0; phase < phaseSteps; phase++) { + for (int phase = 0; phase < phaseSteps; phase++) + { Real sum = 0; - for(int i = phase * m_nTaps; i < phase * m_nTaps + m_nTaps; i++) - sum += polyphase[i]; - for(int i = phase * m_nTaps; i < phase * m_nTaps + m_nTaps; i++) - polyphase[i] /= sum; + + for (int i = phase * m_nTaps; i < phase * m_nTaps + m_nTaps; i++) { + sum += polyphase[i]; + } + + for (int i = phase * m_nTaps; i < phase * m_nTaps + m_nTaps; i++) { + polyphase[i] /= sum; + } } // move taps around to match sse storage requirements m_taps = new float[2 * taps.size() + 8]; - for(uint i = 0; i < 2 * taps.size() + 8; ++i) - m_taps[i] = 0; + + for (uint i = 0; i < 2 * taps.size() + 8; ++i) { + m_taps[i] = 0; + } + m_alignedTaps = (float*)((((quint64)m_taps) + 15) & ~15); - for(uint i = 0; i < taps.size(); ++i) { + + for (uint i = 0; i < taps.size(); ++i) + { m_alignedTaps[2 * i + 0] = polyphase[i]; m_alignedTaps[2 * i + 1] = polyphase[i]; } + m_taps2 = new float[2 * taps.size() + 8]; - for(uint i = 0; i < 2 * taps.size() + 8; ++i) - m_taps2[i] = 0; + + for (uint i = 0; i < 2 * taps.size() + 8; ++i) { + m_taps2[i] = 0; + } + m_alignedTaps2 = (float*)((((quint64)m_taps2) + 15) & ~15); - for(uint i = 1; i < taps.size(); ++i) { + + for (uint i = 1; i < taps.size(); ++i) + { m_alignedTaps2[2 * (i - 1) + 0] = polyphase[i]; m_alignedTaps2[2 * (i - 1) + 1] = polyphase[i]; } @@ -130,7 +163,8 @@ void Interpolator::create(int phaseSteps, double sampleRate, double cutoff, doub void Interpolator::free() { - if(m_taps != NULL) { + if (m_taps != NULL) + { delete[] m_taps; m_taps = NULL; m_alignedTaps = NULL; diff --git a/sdrbase/dsp/interpolator.h b/sdrbase/dsp/interpolator.h index 07d5171d0..e413d8907 100644 --- a/sdrbase/dsp/interpolator.h +++ b/sdrbase/dsp/interpolator.h @@ -1,3 +1,19 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015 Edouard Griffiths, F4EXB. // +// // +// 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 // +// // +// 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 . // +/////////////////////////////////////////////////////////////////////////////////// + #ifndef INCLUDE_INTERPOLATOR_H #define INCLUDE_INTERPOLATOR_H @@ -17,13 +33,13 @@ public: void free(); // Original code allowed for upsampling, but was never used that way + // The decimation factor should always be lower than 2 for proper work bool decimate(Real *distance, const Complex& next, Complex* result) { advanceFilter(next); *distance -= 1.0; - if (*distance >= 1.0) - { + if (*distance >= 1.0) { return false; } @@ -53,9 +69,9 @@ public: // sampling frequency must be the highest of the two bool resample(Real* distance, const Complex& next, bool* consumed, Complex* result) { - while(*distance >= 1.0) + while (*distance >= 1.0) { - if(!(*consumed)) + if (!(*consumed)) { advanceFilter(next); *distance -= 1.0; @@ -104,24 +120,31 @@ private: void advanceFilter(const Complex& next) { m_ptr--; - if(m_ptr < 0) - m_ptr = m_nTaps - 1; + + if (m_ptr < 0) { + m_ptr = m_nTaps - 1; + } + m_samples[m_ptr] = next; } void advanceFilter() { m_ptr--; - if(m_ptr < 0) + + if (m_ptr < 0) { m_ptr = m_nTaps - 1; + } + m_samples[m_ptr].real(0.0); m_samples[m_ptr].imag(0.0); } void doInterpolate(int phase, Complex* result) { - if (phase < 0) - phase = 0; + if (phase < 0) { + phase = 0; + } #if USE_SSE2 // beware of the ringbuffer if(m_ptr == 0) { @@ -182,12 +205,13 @@ private: Real rAcc = 0; Real iAcc = 0; - for(int i = 0; i < m_nTaps; i++) { + for (int i = 0; i < m_nTaps; i++) { rAcc += *coeff * m_samples[sample].real(); iAcc += *coeff * m_samples[sample].imag(); sample = (sample + 1) % m_nTaps; coeff += 2; } + *result = Complex(rAcc, iAcc); #endif