1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
|
#define _USE_MATH_DEFINES
#include <math.h>
#include <vector>
#include "dsp/interpolator.h"
static std::vector<Real> createPolyphaseLowPass(
int phaseSteps,
double gain,
double sampleRateHz,
double cutoffFreqHz,
double transitionWidthHz,
double oobAttenuationdB)
{
int ntaps = (int)(oobAttenuationdB * sampleRateHz / (22.0 * transitionWidthHz));
if((ntaps % 2) != 0)
ntaps++;
ntaps *= phaseSteps;
std::vector<float> taps(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));
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];
}
double max = taps[0 + 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;
return taps;
}
Interpolator::Interpolator() :
m_taps(NULL),
m_alignedTaps(NULL)
{
}
Interpolator::~Interpolator()
{
free();
}
void Interpolator::create(int phaseSteps, double sampleRate, double cutoff)
{
free();
std::vector<Real> taps = createPolyphaseLowPass(
phaseSteps, // number of polyphases
1.0, // gain
phaseSteps * sampleRate, // sampling frequency
cutoff, // hz beginning of transition band
sampleRate / 5.0, // hz width of transition band
20.0); // out of band attenuation
// init state
m_ptr = 0;
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;
// 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];
}
// normalize phase filters
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;
}
// move taps around to match sse storage requirements
m_taps = new float[2 * taps.size() + 8];
for(int i = 0; i < 2 * taps.size() + 8; ++i)
m_taps[i] = 0;
m_alignedTaps = (float*)((((quint64)m_taps) + 15) & ~15);
for(int 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(int i = 0; i < 2 * taps.size() + 8; ++i)
m_taps2[i] = 0;
m_alignedTaps2 = (float*)((((quint64)m_taps2) + 15) & ~15);
for(int i = 1; i < taps.size(); ++i) {
m_alignedTaps2[2 * (i - 1) + 0] = polyphase[i];
m_alignedTaps2[2 * (i - 1) + 1] = polyphase[i];
}
}
void Interpolator::free()
{
if(m_taps != NULL) {
delete[] m_taps;
m_taps = NULL;
m_alignedTaps = NULL;
delete[] m_taps2;
m_taps2 = NULL;
m_alignedTaps2 = NULL;
}
}
|
