benkyd/SDRPlusPlus
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

New stuff

Browse Source
This commit is contained in:
Ryzerth
2020-07-19 15:59:44 +02:00
parent 370324bc68
commit cbf0b6290d
28 changed files with 9162 additions and 393 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
src/dsp/correction.h
View File

@@ -25,7 +25,31 @@ namespace dsp {
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
if (!running) {
return;
}
_in->stopReader();
output.stopWriter();
_workerThread.join();
_in->clearReadStop();
output.clearWriteStop();
running = false;
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_bufferSize = blockSize;
output.setMaxLatency(blockSize * 2);
}
stream<complex_t> output;
@@ -37,9 +61,9 @@ namespace dsp {
float ibias = 0.0f;
float qbias = 0.0f;
while (true) {
_this->_in->read(buf, _this->_bufferSize);
if (_this->_in->read(buf, _this->_bufferSize) < 0) { break; };
if (_this->bypass) {
_this->output.write(buf, _this->_bufferSize);
if (_this->output.write(buf, _this->_bufferSize) < 0) { break; };
continue;
}
for (int i = 0; i < _this->_bufferSize; i++) {
@@ -52,12 +76,14 @@ namespace dsp {
buf[i].i -= ibias;
buf[i].q -= qbias;
}
_this->output.write(buf, _this->_bufferSize);
if (_this->output.write(buf, _this->_bufferSize) < 0) { break; };
}
delete[] buf;
}
stream<complex_t>* _in;
int _bufferSize;
std::thread _workerThread;
bool running = false;
};
};

116
src/dsp/demodulator.h
View File

@@ -2,6 +2,8 @@
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <dsp/source.h>
#include <dsp/math.h>
/*
TODO:
@@ -42,6 +44,8 @@ namespace dsp {
_input = in;
_blockSize = blockSize;
_phase = 0.0f;
_deviation = deviation;
_sampleRate = sampleRate;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / deviation);
}
@@ -82,6 +86,16 @@ namespace dsp {
output.setMaxLatency(_blockSize * 2);
}
void setSampleRate(float sampleRate) {
_sampleRate = sampleRate;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / _deviation);
}
void setDeviation(float deviation) {
_deviation = deviation;
_phasorSpeed = (2 * 3.1415926535) / (_sampleRate / _deviation);
}
stream<float> output;
private:
@@ -109,6 +123,8 @@ namespace dsp {
int _blockSize;
float _phase;
float _phasorSpeed;
float _deviation;
float _sampleRate;
std::thread _workerThread;
};
@@ -195,4 +211,104 @@ namespace dsp {
int _blockSize;
std::thread _workerThread;
};
class SSBDemod {
public:
SSBDemod() {
}
void init(stream<complex_t>* input, float sampleRate, float bandWidth, int blockSize) {
_blockSize = blockSize;
_bandWidth = bandWidth;
_mode = MODE_USB;
output.init(blockSize * 2);
lo.init(bandWidth / 2.0f, sampleRate, blockSize);
mixer.init(input, &lo.output, blockSize);
lo.start();
}
void start() {
mixer.start();
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
mixer.stop();
mixer.output.stopReader();
output.stopWriter();
_workerThread.join();
mixer.output.clearReadStop();
output.clearWriteStop();
running = false;
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
}
void setMode(int mode) {
if (mode < 0 && mode >= _MODE_COUNT) {
return;
}
_mode = mode;
if (mode == MODE_USB) {
lo.setFrequency(_bandWidth / 2.0f);
}
else if (mode == MODE_LSB) {
lo.setFrequency(-_bandWidth / 2.0f);
}
}
stream<float> output;
enum {
MODE_USB,
MODE_LSB,
_MODE_COUNT
};
private:
static void _worker(SSBDemod* _this) {
complex_t* inBuf = new complex_t[_this->_blockSize];
float* outBuf = new float[_this->_blockSize];
float min, max, factor;
while (true) {
if (_this->mixer.output.read(inBuf, _this->_blockSize) < 0) { break; };
min = INFINITY;
max = -INFINITY;
for (int i = 0; i < _this->_blockSize; i++) {
outBuf[i] = inBuf[i].q;
if (inBuf[i].q < min) {
min = inBuf[i].q;
}
if (inBuf[i].q > max) {
max = inBuf[i].q;
}
}
factor = (max - min) / 2;
for (int i = 0; i < _this->_blockSize; i++) {
outBuf[i] /= factor;
}
if (_this->output.write(outBuf, _this->_blockSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
std::thread _workerThread;
SineSource lo;
Multiplier mixer;
int _blockSize;
float _bandWidth;
int _mode;
bool running = false;
};
};

19
src/dsp/filter.h
View File

@@ -10,8 +10,17 @@
namespace dsp {
inline void BlackmanWindow(std::vector<float>& taps, float sampleRate, float cutoff, float transWidth) {
taps.clear();
float fc = cutoff / sampleRate;
if (fc > 1.0f) {
fc = 1.0f;
}
int _M = 4.0f / (transWidth / sampleRate);
if (_M < 4) {
_M = 4;
}
if (_M % 2 == 0) { _M++; }
float M = _M;
float sum = 0.0f;
@@ -131,7 +140,11 @@ namespace dsp {
return;
}
_blockSize = blockSize;
output.setMaxLatency((_blockSize * 2) / _decim);
output.setMaxLatency(getOutputBlockSize() * 2);
}
int getOutputBlockSize() {
return _blockSize / _decim;
}
stream<complex_t> output;
@@ -302,6 +315,10 @@ namespace dsp {
output.setMaxLatency((_blockSize * 2) / _decim);
}
int getOutputBlockSize() {
return _blockSize / _decim;
}
stream<float> output;
private:

218
src/dsp/legacy_stream.h
View File

@@ -1,218 +0,0 @@
#pragma once
#include <condition_variable>
#include <algorithm>
#include <math.h>
#define STREAM_BUF_SZ 1000000
namespace dsp {
template <class T>
class stream {
public:
stream() {
}
stream(int maxLatency) {
size = STREAM_BUF_SZ;
_buffer = new T[size];
_stopReader = false;
_stopWriter = false;
this->maxLatency = maxLatency;
writec = 0;
readc = size - 1;
}
void init(int maxLatency) {
size = STREAM_BUF_SZ;
_buffer = new T[size];
_stopReader = false;
_stopWriter = false;
this->maxLatency = maxLatency;
writec = 0;
readc = size - 1;
}
int read(T* data, int len) {
int dataRead = 0;
while (dataRead < len) {
int canRead = waitUntilReadable();
if (canRead < 0) {
clearReadStop();
return -1;
}
int toRead = std::min(canRead, len - dataRead);
int len1 = (toRead >= (size - readc) ? (size - readc) : (toRead));
memcpy(&data[dataRead], &_buffer[readc], len1 * sizeof(T));
if (len1 < toRead) {
memcpy(&data[dataRead + len1], _buffer, (toRead - len1) * sizeof(T));
}
dataRead += toRead;
readc_mtx.lock();
readc = (readc + toRead) % size;
readc_mtx.unlock();
canWriteVar.notify_one();
}
return len;
}
int readAndSkip(T* data, int len, int skip) {
int dataRead = 0;
while (dataRead < len) {
int canRead = waitUntilReadable();
if (canRead < 0) {
clearReadStop();
return -1;
}
int toRead = std::min(canRead, len - dataRead);
int len1 = (toRead >= (size - readc) ? (size - readc) : (toRead));
memcpy(&data[dataRead], &_buffer[readc], len1 * sizeof(T));
if (len1 < toRead) {
memcpy(&data[dataRead + len1], _buffer, (toRead - len1) * sizeof(T));
}
dataRead += toRead;
readc_mtx.lock();
readc = (readc + toRead) % size;
readc_mtx.unlock();
canWriteVar.notify_one();
}
// Skip
dataRead = 0;
while (dataRead < skip) {
int canRead = waitUntilReadable();
int toRead = std::min(canRead, skip - dataRead);
dataRead += toRead;
readc_mtx.lock();
readc = (readc + toRead) % size;
readc_mtx.unlock();
canWriteVar.notify_one();
}
return len;
}
int waitUntilReadable() {
int canRead = readable();
if (canRead > 0) {
return canRead;
}
std::unique_lock<std::mutex> lck(writec_mtx);
canReadVar.wait(lck, [=](){ return ((this->readable(false) > 0) || this->getReadStop()); });
if (this->getReadStop()) {
return -1;
}
return this->readable(false);
}
int readable(bool lock = true) {
if (lock) { writec_mtx.lock(); }
int _wc = writec;
if (lock) { writec_mtx.unlock(); }
int readable = (_wc - readc) % this->size;
if (_wc < readc) {
readable = (this->size + readable);
}
return readable - 1;
}
int write(T* data, int len) {
int dataWrite = 0;
while (dataWrite < len) {
int canWrite = waitUntilWriteable();
if (canWrite < 0) {
clearWriteStop();
return -1;
}
int toWrite = std::min(canWrite, len - dataWrite);
int len1 = (toWrite >= (size - writec) ? (size - writec) : (toWrite));
memcpy(&_buffer[writec], &data[dataWrite], len1 * sizeof(T));
if (len1 < toWrite) {
memcpy(_buffer, &data[dataWrite + len1], (toWrite - len1) * sizeof(T));
}
dataWrite += toWrite;
writec_mtx.lock();
writec = (writec + toWrite) % size;
writec_mtx.unlock();
canReadVar.notify_one();
}
return len;
}
int waitUntilWriteable() {
int canWrite = writeable();
if (canWrite > 0) {
return canWrite;
}
std::unique_lock<std::mutex> lck(readc_mtx);
canWriteVar.wait(lck, [=](){ return ((this->writeable(false) > 0) || this->getWriteStop()); });
if (this->getWriteStop()) {
return -1;
}
return this->writeable(false);
}
int writeable(bool lock = true) {
if (lock) { readc_mtx.lock(); }
int _rc = readc;
if (lock) { readc_mtx.unlock(); }
int writeable = (_rc - writec) % this->size;
if (_rc < writec) {
writeable = (this->size + writeable);
}
return std::min<float>(writeable - 1, maxLatency - readable(false) - 1);
}
void stopReader() {
_stopReader = true;
canReadVar.notify_one();
}
void stopWriter() {
_stopWriter = true;
canWriteVar.notify_one();
}
bool getReadStop() {
return _stopReader;
}
bool getWriteStop() {
return _stopWriter;
}
void clearReadStop() {
_stopReader = false;
}
void clearWriteStop() {
_stopWriter = false;
}
void setMaxLatency(int maxLatency) {
this->maxLatency = maxLatency;
}
private:
T* _buffer;
int size;
int readc;
int writec;
int maxLatency;
bool _stopReader;
bool _stopWriter;
std::mutex readc_mtx;
std::mutex writec_mtx;
std::condition_variable canReadVar;
std::condition_variable canWriteVar;
};
};

417
src/dsp/resampling.h
View File

@@ -79,11 +79,20 @@ namespace dsp {
T* inBuf = new T[_this->_blockSize];
T* outBuf = new T[_this->_blockSize * _this->_interpolation];
int outCount = _this->_blockSize * _this->_interpolation;
int interp = _this->_interpolation;
int count = 0;
while (true) {
if (_this->_input->read(inBuf, _this->_blockSize) < 0) { break; };
for (int i = 0; i < outCount; i++) {
outBuf[i] = inBuf[(int)((float)i / _this->_interpolation)];
}
// for (int i = 0; i < outCount; i += interp) {
// outBuf[i] = inBuf[count];
// count++;
// }
count = 0;
if (_this->output.write(outBuf, outCount) < 0) { break; };
}
delete[] inBuf;
@@ -121,6 +130,7 @@ namespace dsp {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
@@ -136,9 +146,13 @@ namespace dsp {
}
void setBlockSize(int blockSize) {
printf("%d\n", blockSize);
if (running) {
return;
}
if (blockSize < 1 ) {
return;
}
_blockSize = blockSize;
output.setMaxLatency(blockSize * 2);
}
@@ -147,6 +161,9 @@ namespace dsp {
if (running) {
return;
}
if (skip < 0 ) {
skip = 0;
}
_skip = skip;
}
@@ -156,9 +173,11 @@ namespace dsp {
static void _worker(BlockDecimator* _this) {
complex_t* buf = new complex_t[_this->_blockSize];
while (true) {
_this->_input->readAndSkip(buf, _this->_blockSize, _this->_skip);
_this->output.write(buf, _this->_blockSize);
int read = _this->_input->readAndSkip(buf, _this->_blockSize, _this->_skip);
if (read < 0) { break; };
if (_this->output.write(buf, _this->_blockSize) < 0) { break; };
}
delete[] buf;
}
stream<complex_t>* _input;
@@ -222,6 +241,8 @@ namespace dsp {
_interp = _outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
printf("Resampler.setInputSampleRate(): %d %d\n", _interp, _decim);
dsp::BlackmanWindow(_taps, inputSampleRate * _interp, _outputSampleRate / 2.0f, _outputSampleRate / 2.0f);
decim.setTaps(_taps);
@@ -250,6 +271,8 @@ namespace dsp {
_interp = outputSampleRate / _gcd;
_decim = _inputSampleRate / _gcd;
printf("Resampler.setOutputSampleRate(): %d %d\n", _interp, _decim);
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, outputSampleRate / 2.0f, outputSampleRate / 2.0f);
decim.setTaps(_taps);
@@ -285,6 +308,10 @@ namespace dsp {
}
}
int getOutputBlockSize() {
return decim.getOutputBlockSize();
}
stream<complex_t>* output;
private:
@@ -357,6 +384,8 @@ namespace dsp {
_interp = _outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
printf("FloatResampler.setInputSampleRate(): %d %d\n", _interp, _decim);
dsp::BlackmanWindow(_taps, inputSampleRate * _interp, _outputSampleRate / 2.0f, _outputSampleRate / 2.0f);
decim.setTaps(_taps);
@@ -384,6 +413,8 @@ namespace dsp {
_interp = outputSampleRate / _gcd;
_decim = _inputSampleRate / _gcd;
printf("FloatResampler.setOutputSampleRate(): %d %d\n", _interp, _decim);
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, outputSampleRate / 2.0f, outputSampleRate / 2.0f);
decim.setTaps(_taps);
@@ -419,6 +450,10 @@ namespace dsp {
}
}
int getOutputBlockSize() {
return decim.getOutputBlockSize();
}
stream<float>* output;
private:
@@ -434,6 +469,384 @@ namespace dsp {
float _blockSize;
bool running = false;
};
class FIRResampler {
public:
FIRResampler() {
}
void init(stream<complex_t>* in, float inputSampleRate, float outputSampleRate, int blockSize, float passBand = -1.0f, float transWidth = -1.0f) {
_input = in;
_outputSampleRate = outputSampleRate;
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
_blockSize = blockSize;
outputBlockSize = (blockSize * _interp) / _decim;
output.init(outputBlockSize * 2);
float cutoff = std::min<float>(_outputSampleRate / 2.0f, _inputSampleRate / 2.0f);
if (passBand > 0.0f && transWidth > 0.0f) {
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, passBand, transWidth);
}
else {
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, cutoff, cutoff);
}
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
if (!running) {
return;
}
_input->stopReader();
output.stopWriter();
_workerThread.join();
_input->clearReadStop();
output.clearWriteStop();
running = false;
}
void setInputSampleRate(float inputSampleRate, int blockSize = -1, float passBand = -1.0f, float transWidth = -1.0f) {
stop();
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)_outputSampleRate);
_interp = _outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
printf("FIRResampler.setInputSampleRate(): %d %d\n", _interp, _decim);
float cutoff = std::min<float>(_outputSampleRate / 2.0f, _inputSampleRate / 2.0f);
if (passBand > 0.0f && transWidth > 0.0f) {
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, passBand, transWidth);
}
else {
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, cutoff, cutoff);
}
if (blockSize > 0) {
_blockSize = blockSize;
}
outputBlockSize = (_blockSize * _interp) / _decim;
output.setMaxLatency(outputBlockSize * 2);
start();
}
void setOutputSampleRate(float outputSampleRate, float passBand = -1.0f, float transWidth = -1.0f) {
stop();
_outputSampleRate = outputSampleRate;
int _gcd = std::gcd((int)_inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = _inputSampleRate / _gcd;
outputBlockSize = (_blockSize * _interp) / _decim;
output.setMaxLatency(outputBlockSize * 2);
printf("FIRResampler.setOutputSampleRate(): %d %d\n", _interp, _decim);
float cutoff = std::min<float>(_outputSampleRate / 2.0f, _inputSampleRate / 2.0f);
if (passBand > 0.0f && transWidth > 0.0f) {
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, passBand, transWidth);
}
else {
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, cutoff, cutoff);
}
start();
}
void setFilterParams(float passBand, float transWidth) {
stop();
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, passBand, transWidth);
start();
}
void setBlockSize(int blockSize) {
stop();
_blockSize = blockSize;
outputBlockSize = (_blockSize * _interp) / _decim;
output.setMaxLatency(outputBlockSize * 2);
start();
}
void setInput(stream<complex_t>* input) {
if (running) {
return;
}
_input = input;
}
int getOutputBlockSize() {
return outputBlockSize;
}
stream<complex_t> output;
private:
static void _worker(FIRResampler* _this) {
complex_t* inBuf = new complex_t[_this->_blockSize];
complex_t* outBuf = new complex_t[_this->outputBlockSize];
int outCount = _this->outputBlockSize;
printf("%d %d\n", _this->_blockSize, _this->outputBlockSize);
float* taps = _this->_taps.data();
int tapCount = _this->_taps.size();
complex_t* delayBuf = new complex_t[tapCount];
complex_t* delayStart = &inBuf[_this->_blockSize - tapCount];
int delaySize = tapCount * sizeof(complex_t);
int interp = _this->_interp;
int decim = _this->_decim;
float correction = (float)sqrt((float)interp);
int afterInterp = _this->_blockSize * interp;
int outIndex = 0;
complex_t val;
while (true) {
if (_this->_input->read(inBuf, _this->_blockSize) < 0) { break; };
for (int i = 0; outIndex < outCount; i += decim) {
outBuf[outIndex].q = 0;
outBuf[outIndex].i = 0;
for (int j = 0; j < tapCount; j++) {
if ((i - j) % interp != 0) {
continue;
}
val = GET_FROM_RIGHT_BUF(inBuf, delayBuf, tapCount, (i - j) / interp);
outBuf[outIndex].i += val.i * taps[j] * correction;
outBuf[outIndex].q += val.q * taps[j] * correction;
}
outIndex++;
}
outIndex = 0;
memcpy(delayBuf, delayStart, delaySize);
if (_this->output.write(outBuf, _this->outputBlockSize) < 0) { break; };
}
printf("DEBUG: %d\n", delaySize);
delete[] inBuf;
delete[] outBuf;
delete[] delayBuf;
}
std::thread _workerThread;
stream<complex_t>* _input;
std::vector<float> _taps;
int _interp;
int _decim;
int outputBlockSize;
float _outputSampleRate;
float _inputSampleRate;
int _blockSize;
bool running = false;
};
class FloatFIRResampler {
public:
FloatFIRResampler() {
}
void init(stream<float>* in, float inputSampleRate, float outputSampleRate, int blockSize, float passBand = -1.0f, float transWidth = -1.0f) {
_input = in;
_outputSampleRate = outputSampleRate;
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
_blockSize = blockSize;
outputBlockSize = (blockSize * _interp) / _decim;
output.init(outputBlockSize * 2);
float cutoff = std::min<float>(_outputSampleRate / 2.0f, _inputSampleRate / 2.0f);
if (passBand > 0.0f && transWidth > 0.0f) {
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, passBand, transWidth);
}
else {
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, cutoff, cutoff);
}
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
if (!running) {
return;
}
_input->stopReader();
output.stopWriter();
_workerThread.join();
_input->clearReadStop();
output.clearWriteStop();
running = false;
}
void setInputSampleRate(float inputSampleRate, int blockSize = -1, float passBand = -1.0f, float transWidth = -1.0f) {
stop();
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)_outputSampleRate);
_interp = _outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
printf("FloatFIRResampler.setInputSampleRate(): %d %d\n", _interp, _decim);
float cutoff = std::min<float>(_outputSampleRate / 2.0f, _inputSampleRate / 2.0f);
if (passBand > 0.0f && transWidth > 0.0f) {
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, passBand, transWidth);
}
else {
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, cutoff, cutoff);
}
if (blockSize > 0) {
_blockSize = blockSize;
}
outputBlockSize = (blockSize * _interp) / _decim;
output.setMaxLatency(outputBlockSize * 2);
start();
}
void setOutputSampleRate(float outputSampleRate, float passBand = -1.0f, float transWidth = -1.0f) {
stop();
_outputSampleRate = outputSampleRate;
int _gcd = std::gcd((int)_inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = _inputSampleRate / _gcd;
outputBlockSize = (_blockSize * _interp) / _decim;
output.setMaxLatency(outputBlockSize * 2);
printf("FloatResampler.setOutputSampleRate(): %d %d\n", _interp, _decim);
float cutoff = std::min<float>(_outputSampleRate / 2.0f, _inputSampleRate / 2.0f);
if (passBand > 0.0f && transWidth > 0.0f) {
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, passBand, transWidth);
}
else {
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, cutoff, cutoff);
}
start();
}
void setFilterParams(float passBand, float transWidth) {
stop();
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, passBand, transWidth);
start();
}
void setBlockSize(int blockSize) {
stop();
_blockSize = blockSize;
outputBlockSize = (_blockSize * _interp) / _decim;
output.setMaxLatency(outputBlockSize * 2);
start();
}
void setInput(stream<float>* input) {
if (running) {
return;
}
_input = input;
}
int getOutputBlockSize() {
return outputBlockSize;
}
stream<float> output;
private:
static void _worker(FloatFIRResampler* _this) {
float* inBuf = new float[_this->_blockSize];
float* outBuf = new float[_this->outputBlockSize];
int outCount = _this->outputBlockSize;
float* taps = _this->_taps.data();
int tapCount = _this->_taps.size();
float* delayBuf = new float[tapCount];
float* delayStart = &inBuf[_this->_blockSize - tapCount];
int delaySize = tapCount * sizeof(float);
int interp = _this->_interp;
int decim = _this->_decim;
float correction = (float)sqrt((float)interp);
printf("FloatResamp: %d %d", _this->_blockSize, _this->outputBlockSize);
int afterInterp = _this->_blockSize * interp;
int outIndex = 0;
while (true) {
if (_this->_input->read(inBuf, _this->_blockSize) < 0) { break; };
for (int i = 0; outIndex < outCount; i += decim) {
outBuf[outIndex] = 0;
for (int j = 0; j < tapCount; j++) {
if ((i - j) % interp != 0) {
continue;
}
outBuf[outIndex] += GET_FROM_RIGHT_BUF(inBuf, delayBuf, tapCount, (i - j) / interp) * taps[j] * correction;
}
outIndex++;
}
outIndex = 0;
memcpy(delayBuf, delayStart, delaySize);
if (_this->output.write(outBuf, _this->outputBlockSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
delete[] delayBuf;
}
std::thread _workerThread;
stream<float>* _input;
std::vector<float> _taps;
int _interp;
int _decim;
int outputBlockSize;
float _outputSampleRate;
float _inputSampleRate;
int _blockSize;
bool running = false;
};
};

9
src/dsp/routing.h
View File

@@ -47,6 +47,15 @@ namespace dsp {
running = false;
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_bufferSize = blockSize;
output_a.setMaxLatency(blockSize * 2);
output_b.setMaxLatency(blockSize * 2);
}
stream<complex_t> output_a;
stream<complex_t> output_b;

17
src/dsp/sink.h
View File

@@ -26,7 +26,21 @@ namespace dsp {
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
if (!running) {
return;
}
_in->stopReader();
_workerThread.join();
_in->clearReadStop();
running = false;
}
bool bypass;
@@ -34,7 +48,7 @@ namespace dsp {
private:
static void _worker(HandlerSink* _this) {
while (true) {
_this->_in->read(_this->_buffer, _this->_bufferSize);
if (_this->_in->read(_this->_buffer, _this->_bufferSize) < 0) { break; };
_this->_handler(_this->_buffer);
}
}
@@ -44,6 +58,7 @@ namespace dsp {
complex_t* _buffer;
std::thread _workerThread;
void (*_handler)(complex_t*);
bool running = false;
};
class NullSink {

9
src/dsp/source.h
View File

@@ -14,6 +14,7 @@ namespace dsp {
SineSource(float frequency, long sampleRate, int blockSize) : output(blockSize * 2) {
_blockSize = blockSize;
_sampleRate = sampleRate;
_frequency = frequency;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / frequency);
_phase = 0;
}
@@ -22,6 +23,7 @@ namespace dsp {
output.init(blockSize * 2);
_sampleRate = sampleRate;
_blockSize = blockSize;
_frequency = frequency;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / frequency);
_phase = 0;
}
@@ -53,6 +55,12 @@ namespace dsp {
return;
}
_blockSize = blockSize;
output.setMaxLatency(blockSize * 2);
}
void setSampleRate(float sampleRate) {
_sampleRate = sampleRate;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / _frequency);
}
stream<complex_t> output;
@@ -76,6 +84,7 @@ namespace dsp {
float _phasorSpeed;
float _phase;
long _sampleRate;
float _frequency;
std::thread _workerThread;
bool running = false;
};

99
src/dsp/vfo.h
View File

@@ -15,112 +15,54 @@ namespace dsp {
_input = in;
_outputSampleRate = outputSampleRate;
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
_bandWidth = bandWidth;
_blockSize = blockSize;
output = &decim.output;
dsp::BlackmanWindow(_taps, inputSampleRate * _interp, bandWidth / 2.0f, bandWidth / 2.0f);
output = &resamp.output;
lo.init(offset, inputSampleRate, blockSize);
mixer.init(in, &lo.output, blockSize);
interp.init(&mixer.output, _interp, blockSize);
if (_interp == 1) {
decim.init(&mixer.output, _taps, blockSize, _decim);
}
else {
decim.init(&interp.output, _taps, blockSize * _interp, _decim);
}
resamp.init(&mixer.output, inputSampleRate, outputSampleRate, blockSize, _bandWidth * 0.8f, _bandWidth);
}
void start() {
lo.start();
mixer.start();
if (_interp != 1) {
interp.start();
}
decim.start();
resamp.start();
}
void stop() {
void stop(bool resampler = true) {
lo.stop();
mixer.stop();
interp.stop();
decim.stop();
if (resampler) { resamp.stop(); };
}
void setInputSampleRate(float inputSampleRate, int blockSize = -1) {
interp.stop();
decim.stop();
lo.stop();
lo.setSampleRate(inputSampleRate);
_inputSampleRate = inputSampleRate;
int _gcd = std::gcd((int)inputSampleRate, (int)_outputSampleRate);
_interp = _outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
dsp::BlackmanWindow(_taps, inputSampleRate * _interp, _bandWidth / 2.0f, _bandWidth / 2.0f);
interp.setInterpolation(_interp);
decim.setDecimation(_decim);
if (blockSize > 0) {
lo.stop();
mixer.stop();
_blockSize = blockSize;
mixer.stop();
lo.setBlockSize(_blockSize);
mixer.setBlockSize(_blockSize);
interp.setBlockSize(_blockSize);
lo.start();
mixer.start();
}
decim.setBlockSize(_blockSize * _interp);
if (_interp == 1) {
decim.setInput(&mixer.output);
}
else {
decim.setInput(&interp.output);
interp.start();
}
decim.start();
resamp.setInputSampleRate(inputSampleRate, _blockSize, _bandWidth * 0.8f, _bandWidth);
lo.start();
}
void setOutputSampleRate(float outputSampleRate, float bandWidth = -1) {
interp.stop();
decim.stop();
if (bandWidth > 0) {
_bandWidth = bandWidth;
}
_outputSampleRate = outputSampleRate;
int _gcd = std::gcd((int)_inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = _inputSampleRate / _gcd;
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, _bandWidth / 2.0f, _bandWidth / 2.0f);
decim.setTaps(_taps);
interp.setInterpolation(_interp);
decim.setDecimation(_decim);
decim.setBlockSize(_blockSize * _interp);
if (_interp == 1) {
decim.setInput(&mixer.output);
}
else {
decim.setInput(&interp.output);
interp.start();
}
decim.start();
resamp.setOutputSampleRate(outputSampleRate, _bandWidth * 0.8f, _bandWidth);
}
void setBandwidth(float bandWidth) {
decim.stop();
dsp::BlackmanWindow(_taps, _inputSampleRate * _interp, _bandWidth / 2.0f, _bandWidth / 2.0f);
decim.setTaps(_taps);
decim.start();
_bandWidth = bandWidth;
resamp.setFilterParams(_bandWidth * 0.8f, _bandWidth);
}
void setOffset(float offset) {
@@ -128,27 +70,26 @@ namespace dsp {
}
void setBlockSize(int blockSize) {
stop();
stop(false);
_blockSize = blockSize;
lo.setBlockSize(_blockSize);
mixer.setBlockSize(_blockSize);
interp.setBlockSize(_blockSize);
decim.setBlockSize(_blockSize * _interp);
resamp.setBlockSize(_blockSize);
start();
}
int getOutputBlockSize() {
return resamp.getOutputBlockSize();
}
stream<complex_t>* output;
private:
SineSource lo;
Multiplier mixer;
Interpolator<complex_t> interp;
DecimatingFIRFilter decim;
FIRResampler resamp;
stream<complex_t>* _input;
std::vector<float> _taps;
int _interp;
int _decim;
float _outputSampleRate;
float _inputSampleRate;
float _bandWidth;