kopia lustrzana https://github.com/f4exb/sdrangel
Reformat rational interpolator code
f4exb
rodzic
c546e40191
commit
f2e3059099
|
|
@ -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<float> 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<Real> 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;
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>. //
|
||||
///////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#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
|
||||
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue