mirror of
https://github.com/f4exb/sdrangel.git
synced 2024-08-21 13:36:35 -04:00
201 lines
4.7 KiB
C++
201 lines
4.7 KiB
C++
#ifndef INCLUDE_INTERPOLATOR_H
|
|
#define INCLUDE_INTERPOLATOR_H
|
|
|
|
#ifdef USE_SSE2
|
|
#include <emmintrin.h>
|
|
#endif
|
|
#include "dsp/dsptypes.h"
|
|
#include "export.h"
|
|
#include <stdio.h>
|
|
#ifndef __WINDOWS__
|
|
#include <unistd.h>
|
|
#endif
|
|
|
|
class SDRBASE_API Interpolator {
|
|
public:
|
|
Interpolator();
|
|
~Interpolator();
|
|
|
|
void create(int phaseSteps, double sampleRate, double cutoff, double nbTapsPerPhase = 4.5);
|
|
void free();
|
|
|
|
// Original code allowed for upsampling, but was never used that way
|
|
bool decimate(Real *distance, const Complex& next, Complex* result)
|
|
{
|
|
advanceFilter(next);
|
|
*distance -= 1.0;
|
|
|
|
if (*distance >= 1.0)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
doInterpolate((int) floor(*distance * (Real)m_phaseSteps), result);
|
|
|
|
return true;
|
|
}
|
|
|
|
// interpolation simplified from the generalized resampler
|
|
bool interpolate(Real *distance, const Complex& next, Complex* result)
|
|
{
|
|
bool consumed = false;
|
|
|
|
if (*distance >= 1.0)
|
|
{
|
|
advanceFilter(next);
|
|
*distance -= 1.0;
|
|
consumed = true;
|
|
}
|
|
|
|
doInterpolate((int)floor(*distance * (Real)m_phaseSteps), result);
|
|
|
|
return consumed;
|
|
}
|
|
|
|
// original interpolator which is actually an arbitrary rational resampler P/Q for any positive P, Q
|
|
// sampling frequency must be the highest of the two
|
|
bool resample(Real* distance, const Complex& next, bool* consumed, Complex* result)
|
|
{
|
|
while(*distance >= 1.0)
|
|
{
|
|
if(!(*consumed))
|
|
{
|
|
advanceFilter(next);
|
|
*distance -= 1.0;
|
|
*consumed = true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
doInterpolate((int)floor(*distance * (Real)m_phaseSteps), result);
|
|
|
|
return true;
|
|
}
|
|
|
|
private:
|
|
float* m_taps;
|
|
float* m_alignedTaps;
|
|
float* m_taps2;
|
|
float* m_alignedTaps2;
|
|
std::vector<Complex> m_samples;
|
|
int m_ptr;
|
|
int m_phaseSteps;
|
|
int m_nTaps;
|
|
|
|
static void createPolyphaseLowPass(
|
|
std::vector<Real>& taps,
|
|
int phaseSteps,
|
|
double gain,
|
|
double sampleRateHz,
|
|
double cutoffFreqHz,
|
|
double transitionWidthHz,
|
|
double oobAttenuationdB);
|
|
|
|
static void createPolyphaseLowPass(
|
|
std::vector<Real>& taps,
|
|
int phaseSteps,
|
|
double gain,
|
|
double sampleRateHz,
|
|
double cutoffFreqHz,
|
|
double nbTapsPerPhase);
|
|
|
|
void createTaps(int nTaps, double sampleRate, double cutoff, std::vector<Real>* taps);
|
|
|
|
void advanceFilter(const Complex& next)
|
|
{
|
|
m_ptr--;
|
|
if(m_ptr < 0)
|
|
m_ptr = m_nTaps - 1;
|
|
m_samples[m_ptr] = next;
|
|
}
|
|
|
|
void advanceFilter()
|
|
{
|
|
m_ptr--;
|
|
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 USE_SSE2
|
|
// beware of the ringbuffer
|
|
if(m_ptr == 0) {
|
|
// only one straight block
|
|
const float* src = (const float*)&m_samples[0];
|
|
const __m128* filter = (const __m128*)&m_alignedTaps[phase * m_nTaps * 2];
|
|
__m128 sum = _mm_setzero_ps();
|
|
int todo = m_nTaps / 2;
|
|
|
|
for(int i = 0; i < todo; i++) {
|
|
sum = _mm_add_ps(sum, _mm_mul_ps(_mm_loadu_ps(src), *filter));
|
|
src += 4;
|
|
filter += 1;
|
|
}
|
|
|
|
// add upper half to lower half and store
|
|
_mm_storel_pi((__m64*)result, _mm_add_ps(sum, _mm_shuffle_ps(sum, _mm_setzero_ps(), _MM_SHUFFLE(1, 0, 3, 2))));
|
|
} else {
|
|
// two blocks
|
|
const float* src = (const float*)&m_samples[m_ptr];
|
|
const __m128* filter = (const __m128*)&m_alignedTaps[phase * m_nTaps * 2];
|
|
__m128 sum = _mm_setzero_ps();
|
|
|
|
// first block
|
|
int block = m_nTaps - m_ptr;
|
|
int todo = block / 2;
|
|
if(block & 1)
|
|
todo++;
|
|
for(int i = 0; i < todo; i++) {
|
|
sum = _mm_add_ps(sum, _mm_mul_ps(_mm_loadu_ps(src), *filter));
|
|
src += 4;
|
|
filter += 1;
|
|
}
|
|
if(block & 1) {
|
|
// one sample beyond the end -> switch coefficient table
|
|
filter = (const __m128*)&m_alignedTaps2[phase * m_nTaps * 2 + todo * 4 - 4];
|
|
}
|
|
// second block
|
|
src = (const float*)&m_samples[0];
|
|
block = m_ptr;
|
|
todo = block / 2;
|
|
for(int i = 0; i < todo; i++) {
|
|
sum = _mm_add_ps(sum, _mm_mul_ps(_mm_loadu_ps(src), *filter));
|
|
src += 4;
|
|
filter += 1;
|
|
}
|
|
if(block & 1) {
|
|
// one sample remaining
|
|
sum = _mm_add_ps(sum, _mm_mul_ps(_mm_loadl_pi(_mm_setzero_ps(), (const __m64*)src), filter[0]));
|
|
}
|
|
|
|
// add upper half to lower half and store
|
|
_mm_storel_pi((__m64*)result, _mm_add_ps(sum, _mm_shuffle_ps(sum, _mm_setzero_ps(), _MM_SHUFFLE(1, 0, 3, 2))));
|
|
}
|
|
#else
|
|
int sample = m_ptr;
|
|
const Real* coeff = &m_alignedTaps[phase * m_nTaps * 2];
|
|
Real rAcc = 0;
|
|
Real iAcc = 0;
|
|
|
|
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
|
|
|
|
}
|
|
};
|
|
|
|
#endif // INCLUDE_INTERPOLATOR_H
|