SDRPlusPlus/core/src/dsp/resampling.h

267 lines
9.1 KiB
C
Raw Normal View History

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;
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
}
}
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) {
tapPhases[(_interp - 1) - phase][tap] = taps[currentTap++];
2020-12-26 21:33:33 +01:00
}
else{
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
};
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
}