2020-06-22 16:45:57 +02:00
|
|
|
#pragma once
|
2020-11-02 03:57:44 +01:00
|
|
|
#include <dsp/block.h>
|
|
|
|
#include <dsp/window.h>
|
2020-11-02 17:48:17 +01:00
|
|
|
#include <numeric>
|
2020-11-12 00:53:38 +01:00
|
|
|
#include <string.h>
|
2020-06-22 16:45:57 +02:00
|
|
|
|
|
|
|
namespace dsp {
|
|
|
|
template <class T>
|
2020-11-02 03:57:44 +01:00
|
|
|
class PolyphaseResampler : public generic_block<PolyphaseResampler<T>> {
|
2020-06-22 16:45:57 +02:00
|
|
|
public:
|
2020-11-02 03:57:44 +01:00
|
|
|
PolyphaseResampler() {}
|
2020-06-22 16:45:57 +02:00
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
PolyphaseResampler(stream<T>* in, dsp::filter_window::generic_window* window, float inSampleRate, float outSampleRate) { init(in, window, inSampleRate, outSampleRate); }
|
2020-06-22 16:45:57 +02:00
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
~PolyphaseResampler() {
|
|
|
|
generic_block<PolyphaseResampler<T>>::stop();
|
|
|
|
volk_free(buffer);
|
|
|
|
volk_free(taps);
|
2020-12-26 21:33:33 +01:00
|
|
|
freeTapPhases();
|
2020-06-22 16:45:57 +02:00
|
|
|
}
|
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
void init(stream<T>* in, dsp::filter_window::generic_window* window, float inSampleRate, float outSampleRate) {
|
|
|
|
_in = in;
|
|
|
|
_window = window;
|
|
|
|
_inSampleRate = inSampleRate;
|
|
|
|
_outSampleRate = outSampleRate;
|
2020-06-22 16:45:57 +02:00
|
|
|
|
2020-11-02 21:13:28 +01:00
|
|
|
int _gcd = std::gcd((int)_inSampleRate, (int)_outSampleRate);
|
|
|
|
_interp = _outSampleRate / _gcd;
|
|
|
|
_decim = _inSampleRate / _gcd;
|
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
tapCount = _window->getTapCount();
|
|
|
|
taps = (float*)volk_malloc(tapCount * sizeof(float), volk_get_alignment());
|
2020-11-04 00:42:39 +01:00
|
|
|
_window->createTaps(taps, tapCount, _interp);
|
2020-06-22 16:45:57 +02:00
|
|
|
|
2020-12-26 21:33:33 +01:00
|
|
|
buildTapPhases();
|
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
buffer = (T*)volk_malloc(STREAM_BUFFER_SIZE * sizeof(T) * 2, volk_get_alignment());
|
2020-11-04 00:42:39 +01:00
|
|
|
memset(buffer, 0, STREAM_BUFFER_SIZE * sizeof(T) * 2);
|
2020-11-02 03:57:44 +01:00
|
|
|
generic_block<PolyphaseResampler<T>>::registerInput(_in);
|
|
|
|
generic_block<PolyphaseResampler<T>>::registerOutput(&out);
|
2020-06-22 16:45:57 +02:00
|
|
|
}
|
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
void setInput(stream<T>* in) {
|
|
|
|
std::lock_guard<std::mutex> lck(generic_block<PolyphaseResampler<T>>::ctrlMtx);
|
|
|
|
generic_block<PolyphaseResampler<T>>::tempStop();
|
2020-11-02 17:48:17 +01:00
|
|
|
generic_block<PolyphaseResampler<T>>::unregisterInput(_in);
|
2020-11-02 03:57:44 +01:00
|
|
|
_in = in;
|
2020-11-02 17:48:17 +01:00
|
|
|
generic_block<PolyphaseResampler<T>>::registerInput(_in);
|
2020-11-02 03:57:44 +01:00
|
|
|
generic_block<PolyphaseResampler<T>>::tempStart();
|
2020-06-22 16:45:57 +02:00
|
|
|
}
|
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
void setInSampleRate(float inSampleRate) {
|
|
|
|
std::lock_guard<std::mutex> lck(generic_block<PolyphaseResampler<T>>::ctrlMtx);
|
|
|
|
generic_block<PolyphaseResampler<T>>::tempStop();
|
|
|
|
_inSampleRate = inSampleRate;
|
|
|
|
int _gcd = std::gcd((int)_inSampleRate, (int)_outSampleRate);
|
|
|
|
_interp = _outSampleRate / _gcd;
|
|
|
|
_decim = _inSampleRate / _gcd;
|
2020-12-26 21:33:33 +01:00
|
|
|
buildTapPhases();
|
2020-11-02 03:57:44 +01:00
|
|
|
generic_block<PolyphaseResampler<T>>::tempStart();
|
2020-06-22 16:45:57 +02:00
|
|
|
}
|
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
void setOutSampleRate(float outSampleRate) {
|
|
|
|
std::lock_guard<std::mutex> lck(generic_block<PolyphaseResampler<T>>::ctrlMtx);
|
|
|
|
generic_block<PolyphaseResampler<T>>::tempStop();
|
|
|
|
_outSampleRate = outSampleRate;
|
|
|
|
int _gcd = std::gcd((int)_inSampleRate, (int)_outSampleRate);
|
|
|
|
_interp = _outSampleRate / _gcd;
|
|
|
|
_decim = _inSampleRate / _gcd;
|
2020-12-26 21:33:33 +01:00
|
|
|
buildTapPhases();
|
2020-11-02 03:57:44 +01:00
|
|
|
generic_block<PolyphaseResampler<T>>::tempStart();
|
2020-06-22 16:45:57 +02:00
|
|
|
}
|
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
int getInterpolation() {
|
|
|
|
return _interp;
|
2020-06-22 16:45:57 +02:00
|
|
|
}
|
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
int getDecimation() {
|
|
|
|
return _decim;
|
2020-06-22 16:45:57 +02:00
|
|
|
}
|
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
void updateWindow(dsp::filter_window::generic_window* window) {
|
2020-11-02 21:13:28 +01:00
|
|
|
std::lock_guard<std::mutex> lck(generic_block<PolyphaseResampler<T>>::ctrlMtx);
|
|
|
|
generic_block<PolyphaseResampler<T>>::tempStop();
|
2020-11-02 03:57:44 +01:00
|
|
|
_window = window;
|
|
|
|
volk_free(taps);
|
|
|
|
tapCount = window->getTapCount();
|
|
|
|
taps = (float*)volk_malloc(tapCount * sizeof(float), volk_get_alignment());
|
2020-11-04 00:42:39 +01:00
|
|
|
window->createTaps(taps, tapCount, _interp);
|
2020-12-26 21:33:33 +01:00
|
|
|
buildTapPhases();
|
2020-11-02 21:13:28 +01:00
|
|
|
generic_block<PolyphaseResampler<T>>::tempStart();
|
2020-06-22 16:45:57 +02:00
|
|
|
}
|
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
int calcOutSize(int in) {
|
|
|
|
return (in * _interp) / _decim;
|
2020-06-22 16:45:57 +02:00
|
|
|
}
|
|
|
|
|
2020-11-03 19:22:53 +01:00
|
|
|
virtual int run() override {
|
2020-11-02 03:57:44 +01:00
|
|
|
count = _in->read();
|
2020-11-02 16:16:21 +01:00
|
|
|
if (count < 0) {
|
|
|
|
return -1;
|
|
|
|
}
|
2020-07-19 15:59:44 +02:00
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
int outCount = calcOutSize(count);
|
2020-07-19 15:59:44 +02:00
|
|
|
|
2020-12-26 21:33:33 +01:00
|
|
|
memcpy(&buffer[tapsPerPhase], _in->readBuf, count * sizeof(T));
|
2020-11-02 03:57:44 +01:00
|
|
|
_in->flush();
|
2020-07-19 15:59:44 +02:00
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
// Write to output
|
2020-07-19 15:59:44 +02:00
|
|
|
int outIndex = 0;
|
2020-12-31 14:26:12 +01:00
|
|
|
int _interp_m_1 = _interp - 1;
|
2020-11-03 19:22:53 +01:00
|
|
|
if constexpr (std::is_same_v<T, float>) {
|
|
|
|
for (int i = 0; outIndex < outCount; i += _decim) {
|
2020-12-26 21:33:33 +01:00
|
|
|
int phase = i % _interp;
|
|
|
|
volk_32f_x2_dot_prod_32f(&out.writeBuf[outIndex], &buffer[i / _interp], tapPhases[phase], tapsPerPhase);
|
2020-11-03 19:22:53 +01:00
|
|
|
outIndex++;
|
|
|
|
}
|
|
|
|
}
|
2021-02-20 15:27:43 +01:00
|
|
|
if constexpr (std::is_same_v<T, complex_t> || std::is_same_v<T, stereo_t>) {
|
2020-11-02 03:57:44 +01:00
|
|
|
for (int i = 0; outIndex < outCount; i += _decim) {
|
2020-12-26 21:33:33 +01:00
|
|
|
int phase = i % _interp;
|
|
|
|
volk_32fc_32f_dot_prod_32fc((lv_32fc_t*)&out.writeBuf[outIndex], (lv_32fc_t*)&buffer[(i / _interp)], tapPhases[phase], tapsPerPhase);
|
2020-11-02 03:57:44 +01:00
|
|
|
outIndex++;
|
2020-07-19 15:59:44 +02:00
|
|
|
}
|
|
|
|
}
|
2020-12-25 16:58:07 +01:00
|
|
|
if (!out.swap(outCount)) { return -1; }
|
2020-08-20 18:29:23 +02:00
|
|
|
|
2020-12-26 21:33:33 +01:00
|
|
|
memmove(buffer, &buffer[count], tapsPerPhase * sizeof(T));
|
2020-08-20 18:29:23 +02:00
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
return count;
|
|
|
|
}
|
2020-08-20 18:29:23 +02:00
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
stream<T> out;
|
2020-08-20 18:29:23 +02:00
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
private:
|
2020-12-26 21:33:33 +01:00
|
|
|
void buildTapPhases(){
|
|
|
|
if(!taps){
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
if(!tapPhases.empty()){
|
|
|
|
freeTapPhases();
|
|
|
|
}
|
|
|
|
|
|
|
|
int phases = _interp;
|
|
|
|
tapsPerPhase = (tapCount+phases-1)/phases; //Integer division ceiling
|
|
|
|
|
|
|
|
bufStart = &buffer[tapsPerPhase];
|
|
|
|
|
|
|
|
for(int i = 0; i < phases; i++){
|
|
|
|
tapPhases.push_back((float*)volk_malloc(tapsPerPhase * sizeof(float), volk_get_alignment()));
|
|
|
|
}
|
|
|
|
|
|
|
|
int currentTap = 0;
|
|
|
|
for(int tap = 0; tap < tapsPerPhase; tap++) {
|
|
|
|
for (int phase = 0; phase < phases; phase++) {
|
|
|
|
if(currentTap < tapCount) {
|
2020-12-31 14:26:12 +01:00
|
|
|
tapPhases[(_interp - 1) - phase][tap] = taps[currentTap++];
|
2020-12-26 21:33:33 +01:00
|
|
|
}
|
|
|
|
else{
|
2020-12-31 14:26:12 +01:00
|
|
|
tapPhases[(_interp - 1) - phase][tap] = 0;
|
2020-12-26 21:33:33 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void freeTapPhases(){
|
|
|
|
for(auto & tapPhase : tapPhases){
|
|
|
|
volk_free(tapPhase);
|
|
|
|
}
|
|
|
|
tapPhases.clear();
|
|
|
|
}
|
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
int count;
|
|
|
|
stream<T>* _in;
|
2020-08-20 18:29:23 +02:00
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
dsp::filter_window::generic_window* _window;
|
2020-09-18 00:23:03 +02:00
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
T* bufStart;
|
|
|
|
T* buffer;
|
|
|
|
int tapCount;
|
|
|
|
int _interp, _decim;
|
|
|
|
float _inSampleRate, _outSampleRate;
|
|
|
|
float* taps;
|
2020-09-18 00:23:03 +02:00
|
|
|
|
2020-12-26 21:33:33 +01:00
|
|
|
int tapsPerPhase;
|
|
|
|
std::vector<float*> tapPhases;
|
|
|
|
|
2020-11-02 03:57:44 +01:00
|
|
|
};
|
2020-12-31 14:26:12 +01:00
|
|
|
|
|
|
|
class PowerDecimator : public generic_block<PowerDecimator> {
|
|
|
|
public:
|
|
|
|
PowerDecimator() {}
|
|
|
|
|
|
|
|
PowerDecimator(stream<complex_t>* in, unsigned int power) { init(in, power); }
|
|
|
|
|
|
|
|
~PowerDecimator() {
|
|
|
|
generic_block<PowerDecimator>::stop();
|
|
|
|
}
|
|
|
|
|
|
|
|
void init(stream<complex_t>* in, unsigned int power) {
|
|
|
|
_in = in;
|
|
|
|
_power = power;
|
|
|
|
generic_block<PowerDecimator>::registerInput(_in);
|
|
|
|
generic_block<PowerDecimator>::registerOutput(&out);
|
|
|
|
}
|
|
|
|
|
|
|
|
void setInput(stream<complex_t>* in) {
|
|
|
|
std::lock_guard<std::mutex> lck(generic_block<PowerDecimator>::ctrlMtx);
|
|
|
|
generic_block<PowerDecimator>::tempStop();
|
|
|
|
generic_block<PowerDecimator>::unregisterInput(_in);
|
|
|
|
_in = in;
|
|
|
|
generic_block<PowerDecimator>::registerInput(_in);
|
|
|
|
generic_block<PowerDecimator>::tempStart();
|
|
|
|
}
|
|
|
|
|
|
|
|
void setPower(unsigned int power) {
|
|
|
|
std::lock_guard<std::mutex> lck(generic_block<PowerDecimator>::ctrlMtx);
|
|
|
|
generic_block<PowerDecimator>::tempStop();
|
|
|
|
generic_block<PowerDecimator>::unregisterInput(_in);
|
|
|
|
_power = power;
|
|
|
|
generic_block<PowerDecimator>::registerInput(_in);
|
|
|
|
generic_block<PowerDecimator>::tempStart();
|
|
|
|
}
|
|
|
|
|
|
|
|
int run() {
|
|
|
|
count = _in->read();
|
|
|
|
if (count < 0) { return -1; }
|
|
|
|
|
|
|
|
if (_power == 0) {
|
|
|
|
memcpy(out.writeBuf, _in->readBuf, count * sizeof(complex_t));
|
|
|
|
}
|
|
|
|
else if (_power == 1) {
|
|
|
|
for (int j = 0; j < count; j += 2) {
|
|
|
|
out.writeBuf[j / 2].i = (_in->readBuf[j].i + _in->readBuf[j + 1].i) * 0.5f;
|
|
|
|
out.writeBuf[j / 2].q = (_in->readBuf[j].q + _in->readBuf[j + 1].q) * 0.5f;
|
|
|
|
}
|
|
|
|
count /= 2;
|
|
|
|
}
|
|
|
|
|
|
|
|
_in->flush();
|
|
|
|
|
|
|
|
if (_power > 1) {
|
|
|
|
for (int i = 1; i < _power; i++) {
|
|
|
|
for (int j = 0; j < count; j += 2) {
|
|
|
|
out.writeBuf[j / 2].i = (_in->readBuf[j].i + _in->readBuf[j + 1].i) * 0.5f;
|
|
|
|
out.writeBuf[j / 2].q = (_in->readBuf[j].q + _in->readBuf[j + 1].q) * 0.5f;
|
|
|
|
}
|
|
|
|
count /= 2;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!out.swap(count)) { return -1; }
|
|
|
|
return count;
|
|
|
|
}
|
|
|
|
|
|
|
|
stream<complex_t> out;
|
|
|
|
|
|
|
|
|
|
|
|
private:
|
|
|
|
int count;
|
|
|
|
unsigned int _power = 0;
|
|
|
|
stream<complex_t>* _in;
|
|
|
|
|
|
|
|
};
|
2020-11-02 03:57:44 +01:00
|
|
|
}
|