mirror/SDRPlusPlus
0
0
Fork 0
mirror of https://github.com/AlexandreRouma/SDRPlusPlus.git synced 2025-06-25 20:07:51 +02:00
Code Issues Actions Packages Projects Releases Wiki Activity

new modole system

Browse Source
This commit is contained in:
Ryzerth
2020-09-19 12:48:34 +02:00
parent 1ef31f0f8b
commit d6b9e1d86a
164 changed files with 414 additions and 413 deletions
Download Patch File Download Diff File Expand all files Collapse all files

121
core/src/dsp/block.h Normal file
View File

@ -0,0 +1,121 @@
#pragma once
#include <vector>
#include <dsp/stream.h>
namespace dsp {
template <class D, class I, class O, int IC, int OC>
class Block {
public:
Block(std::vector<int> inBs, std::vector<int> outBs, D* inst, void (*workerFunc)(D* _this)) {
derived = inst;
worker = workerFunc;
inputBlockSize = inBs;
outputBlockSize = outBs;
in.reserve(IC);
out.reserve(OC);
for (int i = 0; i < IC; i++) {
in.push_back(NULL);
}
for (int i = 0; i < OC; i++) {
out.push_back(new stream<I>(outBs[i] * 2));
}
}
void start() {
if (running) {
return;
}
running = true;
startHandler();
workerThread = std::thread(worker, derived);
}
void stop() {
if (!running) {
return;
}
stopHandler();
for (auto is : in) {
is->stopReader();
}
for (auto os : out) {
os->stopWriter();
}
workerThread.join();
for (auto is : in) {
is->clearReadStop();
}
for (auto os : out) {
os->clearWriteStop();
}
running = false;
}
virtual void setBlockSize(int blockSize) {
if (running) {
return;
}
for (int i = 0; i < IC; i++) {
in[i]->setMaxLatency(blockSize * 2);
inputBlockSize[i] = blockSize;
}
for (int i = 0; i < OC; i++) {
out[i]->setMaxLatency(blockSize * 2);
outputBlockSize[i] = blockSize;
}
}
std::vector<stream<I>*> out;
protected:
virtual void startHandler() {}
virtual void stopHandler() {}
std::vector<stream<I>*> in;
std::vector<int> inputBlockSize;
std::vector<int> outputBlockSize;
bool running = false;
private:
void (*worker)(D* _this);
std::thread workerThread;
D* derived;
};
class DemoMultiplier : public Block<DemoMultiplier, complex_t, complex_t, 2, 1> {
public:
DemoMultiplier() : Block({2}, {1}, this, worker) {}
void init(stream<complex_t>* a, stream<complex_t>* b, int blockSize) {
in[0] = a;
in[1] = b;
inputBlockSize[0] = blockSize;
inputBlockSize[1] = blockSize;
out[0]->setMaxLatency(blockSize * 2);
outputBlockSize[0] = blockSize;
}
private:
static void worker(DemoMultiplier* _this) {
int blockSize = _this->inputBlockSize[0];
stream<complex_t>* inA = _this->in[0];
stream<complex_t>* inB = _this->in[1];
stream<complex_t>* out = _this->out[0];
complex_t* aBuf = (complex_t*)volk_malloc(sizeof(complex_t) * blockSize, volk_get_alignment());
complex_t* bBuf = (complex_t*)volk_malloc(sizeof(complex_t) * blockSize, volk_get_alignment());
complex_t* outBuf = (complex_t*)volk_malloc(sizeof(complex_t) * blockSize, volk_get_alignment());
while (true) {
if (inA->read(aBuf, blockSize) < 0) { break; };
if (inB->read(bBuf, blockSize) < 0) { break; };
volk_32fc_x2_multiply_32fc((lv_32fc_t*)outBuf, (lv_32fc_t*)aBuf, (lv_32fc_t*)bBuf, blockSize);
if (out->write(outBuf, blockSize) < 0) { break; };
}
volk_free(aBuf);
volk_free(bBuf);
volk_free(outBuf);
}
};
};

158
core/src/dsp/correction.h Normal file
View File

@ -0,0 +1,158 @@
#pragma once
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <vector>
namespace dsp {
class DCBiasRemover {
public:
DCBiasRemover() {
}
DCBiasRemover(stream<complex_t>* input, int bufferSize) : output(bufferSize * 2) {
_in = input;
_bufferSize = bufferSize;
bypass = false;
}
void init(stream<complex_t>* input, int bufferSize) {
output.init(bufferSize * 2);
_in = input;
_bufferSize = bufferSize;
bypass = false;
}
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;
bool bypass;
private:
static void _worker(DCBiasRemover* _this) {
complex_t* buf = new complex_t[_this->_bufferSize];
float ibias = 0.0f;
float qbias = 0.0f;
while (true) {
if (_this->_in->read(buf, _this->_bufferSize) < 0) { break; };
if (_this->bypass) {
if (_this->output.write(buf, _this->_bufferSize) < 0) { break; };
continue;
}
for (int i = 0; i < _this->_bufferSize; i++) {
ibias += buf[i].i;
qbias += buf[i].q;
}
ibias /= _this->_bufferSize;
qbias /= _this->_bufferSize;
for (int i = 0; i < _this->_bufferSize; i++) {
buf[i].i -= ibias;
buf[i].q -= qbias;
}
if (_this->output.write(buf, _this->_bufferSize) < 0) { break; };
}
delete[] buf;
}
stream<complex_t>* _in;
int _bufferSize;
std::thread _workerThread;
bool running = false;
};
class ComplexToStereo {
public:
ComplexToStereo() {
}
ComplexToStereo(stream<complex_t>* input, int bufferSize) : output(bufferSize * 2) {
_in = input;
_bufferSize = bufferSize;
}
void init(stream<complex_t>* input, int bufferSize) {
output.init(bufferSize * 2);
_in = input;
_bufferSize = bufferSize;
}
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<StereoFloat_t> output;
private:
static void _worker(ComplexToStereo* _this) {
complex_t* inBuf = new complex_t[_this->_bufferSize];
StereoFloat_t* outBuf = new StereoFloat_t[_this->_bufferSize];
while (true) {
if (_this->_in->read(inBuf, _this->_bufferSize) < 0) { break; };
for (int i = 0; i < _this->_bufferSize; i++) {
outBuf[i].l = inBuf[i].i;
outBuf[i].r = inBuf[i].q;
}
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<complex_t>* _in;
int _bufferSize;
std::thread _workerThread;
bool running = false;
};
};

424
core/src/dsp/demodulator.h Normal file
View File

@ -0,0 +1,424 @@
#pragma once
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <dsp/source.h>
#include <dsp/math.h>
/*
TODO:
- Add a sample rate ajustment function to all demodulators
*/
#define FAST_ATAN2_COEF1 3.1415926535f / 4.0f
#define FAST_ATAN2_COEF2 3.0f * FAST_ATAN2_COEF1
inline float fast_arctan2(float y, float x) {
float abs_y = fabs(y) + (1e-10);
float r, angle;
if (x>=0) {
r = (x - abs_y) / (x + abs_y);
angle = FAST_ATAN2_COEF1 - FAST_ATAN2_COEF1 * r;
}
else {
r = (x + abs_y) / (abs_y - x);
angle = FAST_ATAN2_COEF2 - FAST_ATAN2_COEF1 * r;
}
if (y < 0) {
return -angle;
}
return angle;
}
namespace dsp {
class FMDemodulator {
public:
FMDemodulator() {
}
FMDemodulator(stream<complex_t>* in, float deviation, long sampleRate, int blockSize) : output(blockSize * 2) {
running = false;
_input = in;
_blockSize = blockSize;
_phase = 0.0f;
_deviation = deviation;
_sampleRate = sampleRate;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / deviation);
}
void init(stream<complex_t>* in, float deviation, long sampleRate, int blockSize) {
output.init(blockSize * 2);
running = false;
_input = in;
_blockSize = blockSize;
_phase = 0.0f;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / deviation);
}
void start() {
if (running) {
return;
}
running = true;
_workerThread = std::thread(_worker, this);
}
void stop() {
if (!running) {
return;
}
_input->stopReader();
output.stopWriter();
_workerThread.join();
running = false;
_input->clearReadStop();
output.clearWriteStop();
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
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:
static void _worker(FMDemodulator* _this) {
complex_t* inBuf = new complex_t[_this->_blockSize];
float* outBuf = new float[_this->_blockSize];
float diff = 0;
float currentPhase = 0;
while (true) {
if (_this->_input->read(inBuf, _this->_blockSize) < 0) { return; };
for (int i = 0; i < _this->_blockSize; i++) {
currentPhase = fast_arctan2(inBuf[i].i, inBuf[i].q);
diff = currentPhase - _this->_phase;
if (diff > 3.1415926535f) { diff -= 2 * 3.1415926535f; }
else if (diff <= -3.1415926535f) { diff += 2 * 3.1415926535f; }
outBuf[i] = diff / _this->_phasorSpeed;
_this->_phase = currentPhase;
}
if (_this->output.write(outBuf, _this->_blockSize) < 0) { return; };
}
}
stream<complex_t>* _input;
bool running;
int _blockSize;
float _phase;
float _phasorSpeed;
float _deviation;
float _sampleRate;
std::thread _workerThread;
};
class AMDemodulator {
public:
AMDemodulator() {
}
AMDemodulator(stream<complex_t>* in, int blockSize) : output(blockSize * 2) {
running = false;
_input = in;
_blockSize = blockSize;
}
void init(stream<complex_t>* in, int blockSize) {
output.init(blockSize * 2);
running = false;
_input = in;
_blockSize = blockSize;
}
void start() {
if (running) {
return;
}
running = true;
_workerThread = std::thread(_worker, this);
}
void stop() {
if (!running) {
return;
}
_input->stopReader();
output.stopWriter();
_workerThread.join();
running = false;
_input->clearReadStop();
output.clearWriteStop();
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency(_blockSize * 2);
}
stream<float> output;
private:
static void _worker(AMDemodulator* _this) {
complex_t* inBuf = new complex_t[_this->_blockSize];
float* outBuf = new float[_this->_blockSize];
float min, max, amp;
while (true) {
if (_this->_input->read(inBuf, _this->_blockSize) < 0) { break; };
min = INFINITY;
max = 0.0f;
for (int i = 0; i < _this->_blockSize; i++) {
outBuf[i] = sqrt((inBuf[i].i*inBuf[i].i) + (inBuf[i].q*inBuf[i].q));
if (outBuf[i] < min) {
min = outBuf[i];
}
if (outBuf[i] > max) {
max = outBuf[i];
}
}
amp = (max - min) / 2.0f;
for (int i = 0; i < _this->_blockSize; i++) {
outBuf[i] = (outBuf[i] - min - amp) / amp;
}
if (_this->output.write(outBuf, _this->_blockSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<complex_t>* _input;
bool running;
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);
}
else if (mode == MODE_LSB) {
lo.setFrequency(0);
}
}
void setBandwidth(float bandwidth) {
_bandWidth = bandwidth;
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_DSB,
_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;
};
// class CWDemod {
// public:
// CWDemod() {
// }
// 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;
// private:
// static void _worker(CWDemod* _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;
// };
};

489
core/src/dsp/filter.h Normal file
View File

@ -0,0 +1,489 @@
#pragma once
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <vector>
#include <dsp/math.h>
#include <spdlog/spdlog.h>
#define GET_FROM_RIGHT_BUF(buffer, delayLine, delayLineSz, n) (((n) < 0) ? delayLine[(delayLineSz) + (n)] : buffer[(n)])
namespace dsp {
inline void BlackmanWindow(std::vector<float>& taps, float sampleRate, float cutoff, float transWidth, int addedTaps = 0) {
taps.clear();
float fc = cutoff / sampleRate;
if (fc > 1.0f) {
fc = 1.0f;
}
int _M = (4.0f / (transWidth / sampleRate)) + (float)addedTaps;
if (_M < 4) {
_M = 4;
}
if (_M % 2 == 0) { _M++; }
float M = _M;
float sum = 0.0f;
float val;
for (int i = 0; i < _M; i++) {
val = (sin(2.0f * M_PI * fc * ((float)i - (M / 2))) / ((float)i - (M / 2))) * (0.42f - (0.5f * cos(2.0f * M_PI / M)) + (0.8f * cos(4.0f * M_PI / M)));
taps.push_back(val);
sum += val;
}
for (int i = 0; i < M; i++) {
taps[i] /= sum;
}
}
class DecimatingFIRFilter {
public:
DecimatingFIRFilter() {
}
DecimatingFIRFilter(stream<complex_t>* input, std::vector<float> taps, int blockSize, float decim) {
output.init((blockSize * 2) / decim);
_in = input;
_blockSize = blockSize;
_tapCount = taps.size();
delayBuf = new complex_t[_tapCount];
_taps = new float[_tapCount];
for (int i = 0; i < _tapCount; i++) {
_taps[i] = taps[i];
}
_decim = decim;
for (int i = 0; i < _tapCount; i++) {
delayBuf[i].i = 0.0f;
delayBuf[i].q = 0.0f;
}
running = false;
}
void init(stream<complex_t>* input, std::vector<float>& taps, int blockSize, float decim) {
output.init((blockSize * 2) / decim);
_in = input;
_blockSize = blockSize;
_tapCount = taps.size();
delayBuf = new complex_t[_tapCount];
_taps = new float[_tapCount];
for (int i = 0; i < _tapCount; i++) {
_taps[i] = taps[i];
}
_decim = decim;
for (int i = 0; i < _tapCount; i++) {
delayBuf[i].i = 0.0f;
delayBuf[i].q = 0.0f;
}
running = false;
}
void start() {
if (running) {
return;
}
running = true;
_workerThread = std::thread(_worker, this);
}
void stop() {
if (!running) {
return;
}
_in->stopReader();
output.stopWriter();
_workerThread.join();
_in->clearReadStop();
output.clearWriteStop();
running = false;
}
void setTaps(std::vector<float>& taps) {
if (running) {
return;
}
_tapCount = taps.size();
delete[] _taps;
delete[] delayBuf;
_taps = new float[_tapCount];
delayBuf = new complex_t[_tapCount];
for (int i = 0; i < _tapCount; i++) {
_taps[i] = taps[i];
delayBuf[i].i = 0;
delayBuf[i].q = 0;
}
}
void setInput(stream<complex_t>* input) {
if (running) {
return;
}
_in = input;
}
void setDecimation(float decimation) {
if (running) {
return;
}
_decim = decimation;
output.setMaxLatency((_blockSize * 2) / _decim);
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency(getOutputBlockSize() * 2);
}
int getOutputBlockSize() {
return _blockSize / _decim;
}
stream<complex_t> output;
private:
static void _worker(DecimatingFIRFilter* _this) {
int outputSize = _this->_blockSize / _this->_decim;
complex_t* inBuf = new complex_t[_this->_blockSize];
complex_t* outBuf = new complex_t[outputSize];
float tap = 0.0f;
int delayOff;
void* delayStart = &inBuf[_this->_blockSize - (_this->_tapCount - 1)];
int delaySize = (_this->_tapCount - 1) * sizeof(complex_t);
int blockSize = _this->_blockSize;
int outBufferLength = outputSize * sizeof(complex_t);
int tapCount = _this->_tapCount;
int decim = _this->_decim;
complex_t* delayBuf = _this->delayBuf;
int id = 0;
while (true) {
if (_this->_in->read(inBuf, blockSize) < 0) { break; };
memset(outBuf, 0, outBufferLength);
for (int t = 0; t < tapCount; t++) {
tap = _this->_taps[t];
if (tap == 0.0f) {
continue;
}
delayOff = tapCount - t;
id = 0;
for (int i = 0; i < blockSize; i += decim) {
if (i < t) {
outBuf[id].i += tap * delayBuf[delayOff + i].i;
outBuf[id].q += tap * delayBuf[delayOff + i].q;
}
else {
outBuf[id].i += tap * inBuf[i - t].i;
outBuf[id].q += tap * inBuf[i - t].q;
}
id++;
}
}
memcpy(delayBuf, delayStart, delaySize);
if (_this->output.write(outBuf, outputSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<complex_t>* _in;
complex_t* delayBuf;
int _blockSize;
int _tapCount = 0;
float _decim;
std::thread _workerThread;
float* _taps;
bool running;
};
class FloatDecimatingFIRFilter {
public:
FloatDecimatingFIRFilter() {
}
FloatDecimatingFIRFilter(stream<float>* input, std::vector<float> taps, int blockSize, float decim) {
output.init((blockSize * 2) / decim);
_in = input;
_blockSize = blockSize;
_tapCount = taps.size();
delayBuf = new float[_tapCount];
_taps = new float[_tapCount];
for (int i = 0; i < _tapCount; i++) {
_taps[i] = taps[i];
}
_decim = decim;
for (int i = 0; i < _tapCount; i++) {
delayBuf[i] = 0.0f;
}
running = false;
}
void init(stream<float>* input, std::vector<float>& taps, int blockSize, float decim) {
output.init((blockSize * 2) / decim);
_in = input;
_blockSize = blockSize;
_tapCount = taps.size();
delayBuf = new float[_tapCount];
_taps = new float[_tapCount];
for (int i = 0; i < _tapCount; i++) {
_taps[i] = taps[i];
}
_decim = decim;
for (int i = 0; i < _tapCount; i++) {
delayBuf[i] = 0.0f;
}
running = false;
}
void start() {
if (running) {
return;
}
running = true;
_workerThread = std::thread(_worker, this);
}
void stop() {
if (!running) {
return;
}
_in->stopReader();
output.stopWriter();
_workerThread.join();
_in->clearReadStop();
output.clearWriteStop();
running = false;
}
void setTaps(std::vector<float>& taps) {
if (running) {
return;
}
_tapCount = taps.size();
delete[] _taps;
delete[] delayBuf;
_taps = new float[_tapCount];
delayBuf = new float[_tapCount];
for (int i = 0; i < _tapCount; i++) {
_taps[i] = taps[i];
delayBuf[i] = 0;
}
}
void setInput(stream<float>* input) {
if (running) {
return;
}
_in = input;
}
void setDecimation(float decimation) {
if (running) {
return;
}
_decim = decimation;
output.setMaxLatency((_blockSize * 2) / _decim);
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency((_blockSize * 2) / _decim);
}
int getOutputBlockSize() {
return _blockSize / _decim;
}
stream<float> output;
private:
static void _worker(FloatDecimatingFIRFilter* _this) {
int outputSize = _this->_blockSize / _this->_decim;
float* inBuf = new float[_this->_blockSize];
float* outBuf = new float[outputSize];
float tap = 0.0f;
int delayOff;
void* delayStart = &inBuf[_this->_blockSize - (_this->_tapCount - 1)];
int delaySize = (_this->_tapCount - 1) * sizeof(float);
int blockSize = _this->_blockSize;
int outBufferLength = outputSize * sizeof(float);
int tapCount = _this->_tapCount;
int decim = _this->_decim;
float* delayBuf = _this->delayBuf;
int id = 0;
while (true) {
if (_this->_in->read(inBuf, blockSize) < 0) { break; };
memset(outBuf, 0, outBufferLength);
for (int t = 0; t < tapCount; t++) {
tap = _this->_taps[t];
if (tap == 0.0f) {
continue;
}
delayOff = tapCount - t;
id = 0;
for (int i = 0; i < blockSize; i += decim) {
if (i < t) {
outBuf[id] += tap * delayBuf[delayOff + i];
id++;
continue;
}
outBuf[id] += tap * inBuf[i - t];
id++;
}
}
memcpy(delayBuf, delayStart, delaySize);
if (_this->output.write(outBuf, outputSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<float>* _in;
float* delayBuf;
int _blockSize;
int _tapCount = 0;
float _decim;
std::thread _workerThread;
float* _taps;
bool running;
};
class FMDeemphasis {
public:
FMDeemphasis() {
}
FMDeemphasis(stream<float>* input, int bufferSize, float tau, float sampleRate) : output(bufferSize * 2) {
_in = input;
_bufferSize = bufferSize;
bypass = false;
_tau = tau;
_sampleRate = sampleRate;
}
void init(stream<float>* input, int bufferSize, float tau, float sampleRate) {
output.init(bufferSize * 2);
_in = input;
_bufferSize = bufferSize;
bypass = false;
_tau = tau;
_sampleRate = sampleRate;
}
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);
}
void setSamplerate(float sampleRate) {
if (running) {
return;
}
_sampleRate = sampleRate;
}
void setTau(float tau) {
if (running) {
return;
}
_tau = tau;
}
stream<float> output;
bool bypass;
private:
static void _worker(FMDeemphasis* _this) {
float* inBuf = new float[_this->_bufferSize];
float* outBuf = new float[_this->_bufferSize];
int count = _this->_bufferSize;
float lastOut = 0.0f;
float dt = 1.0f / _this->_sampleRate;
float alpha = dt / (_this->_tau + dt);
while (true) {
if (_this->_in->read(inBuf, count) < 0) { break; };
if (_this->bypass) {
if (_this->output.write(inBuf, count) < 0) { break; };
continue;
}
if (isnan(lastOut)) {
lastOut = 0.0f;
}
outBuf[0] = (alpha * inBuf[0]) + ((1-alpha) * lastOut);
for (int i = 1; i < count; i++) {
outBuf[i] = (alpha * inBuf[i]) + ((1 - alpha) * outBuf[i - 1]);
}
lastOut = outBuf[count - 1];
if (_this->output.write(outBuf, count) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<float>* _in;
int _bufferSize;
std::thread _workerThread;
bool running = false;
float _sampleRate;
float _tau;
};
};

85
core/src/dsp/math.h Normal file
View File

@ -0,0 +1,85 @@
#pragma once
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <volk.h>
#ifndef M_PI
#define M_PI 3.1415926535f
#endif
namespace dsp {
class Multiplier {
public:
Multiplier() {
}
Multiplier(stream<complex_t>* a, stream<complex_t>* b, int blockSize) : output(blockSize * 2) {
_a = a;
_b = b;
_blockSize = blockSize;
}
void init(stream<complex_t>* a, stream<complex_t>* b, int blockSize) {
output.init(blockSize * 2);
_a = a;
_b = b;
_blockSize = blockSize;
}
void start() {
if (running) {
return;
}
running = true;
_workerThread = std::thread(_worker, this);
}
void stop() {
if (!running) {
return;
}
_a->stopReader();
_b->stopReader();
output.stopWriter();
_workerThread.join();
running = false;
_a->clearReadStop();
_b->clearReadStop();
output.clearWriteStop();
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency(blockSize * 2);
}
stream<complex_t> output;
private:
static void _worker(Multiplier* _this) {
complex_t* aBuf = (complex_t*)volk_malloc(sizeof(complex_t) * _this->_blockSize, volk_get_alignment());
complex_t* bBuf = (complex_t*)volk_malloc(sizeof(complex_t) * _this->_blockSize, volk_get_alignment());
complex_t* outBuf = (complex_t*)volk_malloc(sizeof(complex_t) * _this->_blockSize, volk_get_alignment());
while (true) {
if (_this->_a->read(aBuf, _this->_blockSize) < 0) { break; };
if (_this->_b->read(bBuf, _this->_blockSize) < 0) { break; };
volk_32fc_x2_multiply_32fc((lv_32fc_t*)outBuf, (lv_32fc_t*)aBuf, (lv_32fc_t*)bBuf, _this->_blockSize);
if (_this->output.write(outBuf, _this->_blockSize) < 0) { break; };
}
volk_free(aBuf);
volk_free(bBuf);
volk_free(outBuf);
}
stream<complex_t>* _a;
stream<complex_t>* _b;
int _blockSize;
bool running = false;
std::thread _workerThread;
};
};

958
core/src/dsp/resampling.h Normal file
View File

@ -0,0 +1,958 @@
#pragma once
#include <thread>
#include <dsp/filter.h>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <numeric>
#include <algorithm>
namespace dsp {
template <class T>
class Interpolator {
public:
Interpolator() {
}
Interpolator(stream<T>* in, float interpolation, int blockSize) : output(blockSize * interpolation * 2) {
_input = in;
_interpolation = interpolation;
_blockSize = blockSize;
}
void init(stream<T>* in, float interpolation, int blockSize) {
output.init(blockSize * 2 * interpolation);
_input = in;
_interpolation = interpolation;
_blockSize = blockSize;
}
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 setInterpolation(float interpolation) {
if (running) {
return;
}
_interpolation = interpolation;
output.setMaxLatency(_blockSize * _interpolation * 2);
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency(_blockSize * _interpolation * 2);
}
void setInput(stream<T>* input) {
if (running) {
return;
}
_input = input;
}
stream<T> output;
private:
static void _worker(Interpolator<T>* _this) {
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;
delete[] outBuf;
}
stream<T>* _input;
int _blockSize;
float _interpolation;
std::thread _workerThread;
bool running = false;
};
class BlockDecimator {
public:
BlockDecimator() {
}
BlockDecimator(stream<complex_t>* in, int skip, int blockSize) : output(blockSize * 2) {
_input = in;
_skip = skip;
_blockSize = blockSize;
}
void init(stream<complex_t>* in, int skip, int blockSize) {
output.init(blockSize * 2);
_input = in;
_skip = skip;
_blockSize = blockSize;
}
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 setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency(blockSize * 2);
}
void setSkip(int skip) {
if (running) {
return;
}
_skip = skip;
}
stream<complex_t> output;
private:
static void _worker(BlockDecimator* _this) {
complex_t* buf = new complex_t[_this->_blockSize];
bool delay = _this->_skip < 0;
int readCount = std::min<int>(_this->_blockSize + _this->_skip, _this->_blockSize);
int skip = std::max<int>(_this->_skip, 0);
int delaySize = (-_this->_skip) * sizeof(complex_t);
complex_t* start = &buf[std::max<int>(-_this->_skip, 0)];
complex_t* delayStart = &buf[_this->_blockSize + _this->_skip];
while (true) {
if (delay) {
memmove(buf, delayStart, delaySize);
}
if (_this->_input->readAndSkip(start, readCount, skip) < 0) { break; };
if (_this->output.write(buf, _this->_blockSize) < 0) { break; };
}
delete[] buf;
}
stream<complex_t>* _input;
int _blockSize;
int _skip;
std::thread _workerThread;
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;
// 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);
// 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 inCount = _this->_blockSize;
// int outCount = _this->outputBlockSize;
// int interp = _this->_interp;
// int decim = _this->_decim;
// float correction = interp;//(float)sqrt((float)interp);
// int tapCount = _this->_taps.size();
// float* taps = new float[tapCount];
// for (int i = 0; i < tapCount; i++) {
// taps[i] = _this->_taps[i] * correction;
// }
// complex_t* delayBuf = new complex_t[tapCount];
// complex_t* delayStart = &inBuf[std::max<int>(inCount - tapCount, 0)];
// int delaySize = tapCount * sizeof(complex_t);
// complex_t* delayBufEnd = &delayBuf[std::max<int>(tapCount - inCount, 0)];
// int moveSize = std::min<int>(inCount, tapCount - inCount) * sizeof(complex_t);
// int inSize = inCount * sizeof(complex_t);
// int afterInterp = inCount * interp;
// int outIndex = 0;
// while (true) {
// if (_this->_input->read(inBuf, inCount) < 0) { break; };
// for (int i = 0; outIndex < outCount; i += decim) {
// outBuf[outIndex].i = 0;
// outBuf[outIndex].q = 0;
// for (int j = i % interp; j < tapCount; j += interp) {
// outBuf[outIndex].i += GET_FROM_RIGHT_BUF(inBuf, delayBuf, tapCount, (i - j) / interp).i * taps[j];
// outBuf[outIndex].q += GET_FROM_RIGHT_BUF(inBuf, delayBuf, tapCount, (i - j) / interp).q * taps[j];
// }
// outIndex++;
// }
// outIndex = 0;
// if (tapCount > inCount) {
// memmove(delayBuf, delayBufEnd, moveSize);
// memcpy(delayBufEnd, delayStart, inSize);
// }
// else {
// memcpy(delayBuf, delayStart, delaySize);
// }
// if (_this->output.write(outBuf, _this->outputBlockSize) < 0) { break; };
// }
// delete[] inBuf;
// delete[] outBuf;
// delete[] delayBuf;
// delete[] taps;
// }
// 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;
// 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);
// 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 inCount = _this->_blockSize;
// int outCount = _this->outputBlockSize;
// int interp = _this->_interp;
// int decim = _this->_decim;
// float correction = interp;//(float)sqrt((float)interp);
// int tapCount = _this->_taps.size();
// float* taps = new float[tapCount];
// for (int i = 0; i < tapCount; i++) {
// taps[i] = _this->_taps[i] * correction;
// }
// float* delayBuf = new float[tapCount];
// float* delayStart = &inBuf[std::max<int>(inCount - tapCount, 0)];
// int delaySize = tapCount * sizeof(float);
// float* delayBufEnd = &delayBuf[std::max<int>(tapCount - inCount, 0)];
// int moveSize = std::min<int>(inCount, tapCount - inCount) * sizeof(float);
// int inSize = inCount * sizeof(float);
// int afterInterp = inCount * interp;
// int outIndex = 0;
// while (true) {
// if (_this->_input->read(inBuf, inCount) < 0) { break; };
// for (int i = 0; outIndex < outCount; i += decim) {
// outBuf[outIndex] = 0;
// for (int j = (i % interp); j < tapCount; j += interp) {
// outBuf[outIndex] += GET_FROM_RIGHT_BUF(inBuf, delayBuf, tapCount, (i - j) / interp) * taps[j];
// }
// outIndex++;
// }
// outIndex = 0;
// if (tapCount > inCount) {
// memmove(delayBuf, delayBufEnd, moveSize);
// memcpy(delayBufEnd, delayStart, inSize);
// }
// else {
// 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;
// };
template <class T>
class FIRResampler {
public:
FIRResampler() {
}
void init(stream<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;
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);
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<T> output;
private:
// Float worker
static void _worker(FIRResampler<T>* _this) {
T* inBuf = new T[_this->_blockSize];
T* outBuf = new T[_this->outputBlockSize];
int inCount = _this->_blockSize;
int outCount = _this->outputBlockSize;
int interp = _this->_interp;
int decim = _this->_decim;
float correction = interp;
int tapCount = _this->_taps.size();
float* taps = new float[tapCount];
for (int i = 0; i < tapCount; i++) {
taps[i] = _this->_taps[i] * correction;
}
T* delayBuf = new T[tapCount];
T* delayStart = &inBuf[std::max<int>(inCount - tapCount, 0)];
int delaySize = tapCount * sizeof(T);
T* delayBufEnd = &delayBuf[std::max<int>(tapCount - inCount, 0)];
int moveSize = std::min<int>(inCount, tapCount - inCount) * sizeof(T);
int inSize = inCount * sizeof(T);
int afterInterp = inCount * interp;
int outIndex = 0;
while (true) {
if (_this->_input->read(inBuf, inCount) < 0) { break; };
if constexpr (std::is_same_v<T, float>) {
for (int i = 0; outIndex < outCount; i += decim) {
outBuf[outIndex] = 0;
for (int j = (i % interp); j < tapCount; j += interp) {
outBuf[outIndex] += GET_FROM_RIGHT_BUF(inBuf, delayBuf, tapCount, (i - j) / interp) * taps[j];
}
outIndex++;
}
}
if constexpr (std::is_same_v<T, complex_t>) {
for (int i = 0; outIndex < outCount; i += decim) {
outBuf[outIndex].i = 0;
outBuf[outIndex].q = 0;
for (int j = i % interp; j < tapCount; j += interp) {
outBuf[outIndex].i += GET_FROM_RIGHT_BUF(inBuf, delayBuf, tapCount, (i - j) / interp).i * taps[j];
outBuf[outIndex].q += GET_FROM_RIGHT_BUF(inBuf, delayBuf, tapCount, (i - j) / interp).q * taps[j];
}
outIndex++;
}
}
outIndex = 0;
if (tapCount > inCount) {
memmove(delayBuf, delayBufEnd, moveSize);
memcpy(delayBufEnd, delayStart, inSize);
}
else {
memcpy(delayBuf, delayStart, delaySize);
}
if (_this->output.write(outBuf, _this->outputBlockSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
delete[] delayBuf;
}
std::thread _workerThread;
stream<T>* _input;
std::vector<float> _taps;
int _interp;
int _decim;
int outputBlockSize;
float _outputSampleRate;
float _inputSampleRate;
int _blockSize;
bool running = false;
};
// class FloatPolyphaseFIRResampler {
// public:
// FloatPolyphaseFIRResampler() {
// }
// 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, _outputSampleRate, passBand, transWidth, _interp - 1);
// }
// else {
// dsp::BlackmanWindow(_taps, _outputSampleRate, cutoff, cutoff, _interp - 1);
// }
// }
// 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;
// float cutoff = std::min<float>(_outputSampleRate / 2.0f, _inputSampleRate / 2.0f);
// if (passBand > 0.0f && transWidth > 0.0f) {
// dsp::BlackmanWindow(_taps, _outputSampleRate, passBand, transWidth, _interp - 1);
// }
// else {
// dsp::BlackmanWindow(_taps,_outputSampleRate, cutoff, cutoff, _interp - 1);
// }
// 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);
// float cutoff = std::min<float>(_outputSampleRate / 2.0f, _inputSampleRate / 2.0f);
// if (passBand > 0.0f && transWidth > 0.0f) {
// dsp::BlackmanWindow(_taps, _outputSampleRate, passBand, transWidth, _interp - 1);
// }
// else {
// dsp::BlackmanWindow(_taps, _outputSampleRate, cutoff, cutoff, _interp - 1);
// }
// start();
// }
// void setFilterParams(float passBand, float transWidth) {
// stop();
// dsp::BlackmanWindow(_taps, _outputSampleRate, passBand, transWidth, _interp - 1);
// 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(FloatPolyphaseFIRResampler* _this) {
// float* inBuf = new float[_this->_blockSize];
// float* outBuf = new float[_this->outputBlockSize];
// int inCount = _this->_blockSize;
// int outCount = _this->outputBlockSize;
// int interp = _this->_interp;
// int decim = _this->_decim;
// float correction = interp;//(float)sqrt((float)interp);
// int tapCount = _this->_taps.size();
// float* taps = new float[tapCount];
// for (int i = 0; i < tapCount; i++) {
// taps[i] = _this->_taps[i] * correction;
// }
// float* delayBuf = new float[tapCount];
// float* delayStart = &inBuf[std::max<int>(inCount - tapCount, 0)];
// int delaySize = tapCount * sizeof(float);
// float* delayBufEnd = &delayBuf[std::max<int>(tapCount - inCount, 0)];
// int moveSize = std::min<int>(inCount, tapCount - inCount) * sizeof(float);
// int inSize = inCount * sizeof(float);
// int afterInterp = inCount * interp;
// int outIndex = 0;
// tapCount -= interp - 1;
// while (true) {
// if (_this->_input->read(inBuf, inCount) < 0) { break; };
// for (int i = 0; i < outCount; i++) {
// outBuf[i] = 0;
// int filterId = (i * decim) % interp;
// int inputId = (i * decim) / interp;
// for (int j = 0; j < tapCount; j++) {
// outBuf[i] += GET_FROM_RIGHT_BUF(inBuf, delayBuf, tapCount, inputId - j) * taps[j + filterId];
// }
// }
// if (tapCount > inCount) {
// memmove(delayBuf, delayBufEnd, moveSize);
// memcpy(delayBufEnd, delayStart, inSize);
// }
// else {
// 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;
// };
};

310
core/src/dsp/routing.h Normal file
View File

@ -0,0 +1,310 @@
#pragma once
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <vector>
#include <spdlog/spdlog.h>
namespace dsp {
class Splitter {
public:
Splitter() {
}
Splitter(stream<complex_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
output_a.init(bufferSize);
output_b.init(bufferSize);
}
void init(stream<complex_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
output_a.init(bufferSize);
output_b.init(bufferSize);
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
if (!running) {
return;
}
_in->stopReader();
output_a.stopWriter();
output_b.stopWriter();
_workerThread.join();
_in->clearReadStop();
output_a.clearWriteStop();
output_b.clearWriteStop();
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;
private:
static void _worker(Splitter* _this) {
complex_t* buf = new complex_t[_this->_bufferSize];
while (true) {
if (_this->_in->read(buf, _this->_bufferSize) < 0) { break; };
if (_this->output_a.write(buf, _this->_bufferSize) < 0) { break; };
if (_this->output_b.write(buf, _this->_bufferSize) < 0) { break; };
}
delete[] buf;
}
stream<complex_t>* _in;
int _bufferSize;
std::thread _workerThread;
bool running = false;
};
template <class T>
class DynamicSplitter {
public:
DynamicSplitter() {
}
DynamicSplitter(stream<T>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
}
void init(stream<T>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
if (!running) {
return;
}
_in->stopReader();
int outputCount = outputs.size();
for (int i = 0; i < outputCount; i++) {
outputs[i]->stopWriter();
}
_workerThread.join();
_in->clearReadStop();
for (int i = 0; i < outputCount; i++) {
outputs[i]->clearWriteStop();
}
running = false;
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_bufferSize = blockSize;
int outputCount = outputs.size();
for (int i = 0; i < outputCount; i++) {
outputs[i]->setMaxLatency(blockSize * 2);
}
}
void bind(stream<T>* stream) {
if (running) {
return;
}
outputs.push_back(stream);
}
void unbind(stream<T>* stream) {
if (running) {
return;
}
int outputCount = outputs.size();
for (int i = 0; i < outputCount; i++) {
if (outputs[i] == stream) {
outputs.erase(outputs.begin() + i);
return;
}
}
}
private:
static void _worker(DynamicSplitter* _this) {
T* buf = new T[_this->_bufferSize];
int outputCount = _this->outputs.size();
while (true) {
if (_this->_in->read(buf, _this->_bufferSize) < 0) { break; };
for (int i = 0; i < outputCount; i++) {
if (_this->outputs[i]->write(buf, _this->_bufferSize) < 0) { break; };
}
}
delete[] buf;
}
stream<T>* _in;
int _bufferSize;
std::thread _workerThread;
bool running = false;
std::vector<stream<T>*> outputs;
};
class MonoToStereo {
public:
MonoToStereo() {
}
MonoToStereo(stream<float>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
output.init(bufferSize * 2);
}
void init(stream<float>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
output.init(bufferSize * 2);
}
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<StereoFloat_t> output;
private:
static void _worker(MonoToStereo* _this) {
float* inBuf = new float[_this->_bufferSize];
StereoFloat_t* outBuf = new StereoFloat_t[_this->_bufferSize];
while (true) {
if (_this->_in->read(inBuf, _this->_bufferSize) < 0) { break; };
for (int i = 0; i < _this->_bufferSize; i++) {
outBuf[i].l = inBuf[i];
outBuf[i].r = inBuf[i];
}
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<float>* _in;
int _bufferSize;
std::thread _workerThread;
bool running = false;
};
class StereoToMono {
public:
StereoToMono() {
}
StereoToMono(stream<StereoFloat_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
}
void init(stream<StereoFloat_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
}
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<float> output;
private:
static void _worker(StereoToMono* _this) {
StereoFloat_t* inBuf = new StereoFloat_t[_this->_bufferSize];
float* outBuf = new float[_this->_bufferSize];
while (true) {
if (_this->_in->read(inBuf, _this->_bufferSize) < 0) { break; };
for (int i = 0; i < _this->_bufferSize; i++) {
outBuf[i] = (inBuf[i].l + inBuf[i].r) / 2.0f;
}
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<StereoFloat_t>* _in;
int _bufferSize;
std::thread _workerThread;
bool running = false;
};
};

133
core/src/dsp/sink.h Normal file
View File

@ -0,0 +1,133 @@
#pragma once
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <vector>
namespace dsp {
class HandlerSink {
public:
HandlerSink() {
}
HandlerSink(stream<complex_t>* input, complex_t* buffer, int bufferSize, void handler(complex_t*)) {
_in = input;
_bufferSize = bufferSize;
_buffer = buffer;
_handler = handler;
}
void init(stream<complex_t>* input, complex_t* buffer, int bufferSize, void handler(complex_t*)) {
_in = input;
_bufferSize = bufferSize;
_buffer = buffer;
_handler = handler;
}
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;
private:
static void _worker(HandlerSink* _this) {
while (true) {
if (_this->_in->read(_this->_buffer, _this->_bufferSize) < 0) { break; };
_this->_handler(_this->_buffer);
}
}
stream<complex_t>* _in;
int _bufferSize;
complex_t* _buffer;
std::thread _workerThread;
void (*_handler)(complex_t*);
bool running = false;
};
class NullSink {
public:
NullSink() {
}
NullSink(stream<complex_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
}
void init(stream<complex_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
}
void start() {
_workerThread = std::thread(_worker, this);
}
bool bypass;
private:
static void _worker(NullSink* _this) {
complex_t* buf = new complex_t[_this->_bufferSize];
while (true) {
_this->_in->read(buf, _this->_bufferSize);
}
}
stream<complex_t>* _in;
int _bufferSize;
std::thread _workerThread;
};
class FloatNullSink {
public:
FloatNullSink() {
}
FloatNullSink(stream<float>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
}
void init(stream<float>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
}
void start() {
_workerThread = std::thread(_worker, this);
}
bool bypass;
private:
static void _worker(FloatNullSink* _this) {
float* buf = new float[_this->_bufferSize];
while (true) {
_this->_in->read(buf, _this->_bufferSize);
}
}
stream<float>* _in;
int _bufferSize;
std::thread _workerThread;
};
};

92
core/src/dsp/source.h Normal file
View File

@ -0,0 +1,92 @@
#pragma once
#include <thread>
#include <dsp/stream.h>
#include <dsp/types.h>
#include <volk.h>
#include <spdlog/spdlog.h>
namespace dsp {
class SineSource {
public:
SineSource() {
}
SineSource(float frequency, long sampleRate, int blockSize) : output(blockSize * 2) {
_blockSize = blockSize;
_sampleRate = sampleRate;
_frequency = frequency;
_phasorSpeed = (2 * 3.1415926535 * frequency) / sampleRate;
_phase = 0;
}
void init(float frequency, long sampleRate, int blockSize) {
output.init(blockSize * 2);
_sampleRate = sampleRate;
_blockSize = blockSize;
_frequency = frequency;
_phasorSpeed = (2 * 3.1415926535 * frequency) / sampleRate;
_phase = 0;
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
if (!running) {
return;
}
output.stopWriter();
_workerThread.join();
output.clearWriteStop();
running = false;
}
void setFrequency(float frequency) {
_phasorSpeed = (2 * 3.1415926535) / (_sampleRate / frequency);
}
void setBlockSize(int blockSize) {
if (running) {
return;
}
_blockSize = blockSize;
output.setMaxLatency(blockSize * 2);
}
void setSampleRate(float sampleRate) {
_sampleRate = sampleRate;
_phasorSpeed = (2 * 3.1415926535 * _frequency) / sampleRate;
}
stream<complex_t> output;
private:
static void _worker(SineSource* _this) {
complex_t* outBuf = new complex_t[_this->_blockSize];
while (true) {
for (int i = 0; i < _this->_blockSize; i++) {
_this->_phase += _this->_phasorSpeed;
outBuf[i].i = sin(_this->_phase);
outBuf[i].q = cos(_this->_phase);
_this->_phase = fmodf(_this->_phase, 2.0f * 3.1415926535); // TODO: Get a more efficient generator
}
if (_this->output.write(outBuf, _this->_blockSize) < 0) { break; };
}
delete[] outBuf;
}
int _blockSize;
float _phasorSpeed;
float _phase;
long _sampleRate;
float _frequency;
std::thread _workerThread;
bool running = false;
};
};

228
core/src/dsp/stream.h Normal file
View File

@ -0,0 +1,228 @@
#pragma once
#include <condition_variable>
#include <algorithm>
#include <math.h>
#include <string.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 = 0;
readable = 0;
writable = size;
memset(_buffer, 0, size * sizeof(T));
}
void init(int maxLatency) {
size = STREAM_BUF_SZ;
_buffer = new T[size];
_stopReader = false;
_stopWriter = false;
this->maxLatency = maxLatency;
writec = 0;
readc = 0;
readable = 0;
writable = size;
memset(_buffer, 0, size * sizeof(T));
}
int read(T* data, int len) {
int dataRead = 0;
int toRead = 0;
while (dataRead < len) {
toRead = std::min<int>(waitUntilReadable(), len - dataRead);
if (toRead < 0) { return -1; };
if ((toRead + readc) > size) {
memcpy(&data[dataRead], &_buffer[readc], (size - readc) * sizeof(T));
memcpy(&data[dataRead + (size - readc)], &_buffer[0], (toRead - (size - readc)) * sizeof(T));
}
else {
memcpy(&data[dataRead], &_buffer[readc], toRead * sizeof(T));
}
dataRead += toRead;
_readable_mtx.lock();
readable -= toRead;
_readable_mtx.unlock();
_writable_mtx.lock();
writable += toRead;
_writable_mtx.unlock();
readc = (readc + toRead) % size;
canWriteVar.notify_one();
}
return len;
}
int readAndSkip(T* data, int len, int skip) {
int dataRead = 0;
int toRead = 0;
while (dataRead < len) {
toRead = std::min<int>(waitUntilReadable(), len - dataRead);
if (toRead < 0) { return -1; };
if ((toRead + readc) > size) {
memcpy(&data[dataRead], &_buffer[readc], (size - readc) * sizeof(T));
memcpy(&data[dataRead + (size - readc)], &_buffer[0], (toRead - (size - readc)) * sizeof(T));
}
else {
memcpy(&data[dataRead], &_buffer[readc], toRead * sizeof(T));
}
dataRead += toRead;
_readable_mtx.lock();
readable -= toRead;
_readable_mtx.unlock();
_writable_mtx.lock();
writable += toRead;
_writable_mtx.unlock();
readc = (readc + toRead) % size;
canWriteVar.notify_one();
}
dataRead = 0;
while (dataRead < skip) {
toRead = std::min<int>(waitUntilReadable(), skip - dataRead);
if (toRead < 0) { return -1; };
dataRead += toRead;
_readable_mtx.lock();
readable -= toRead;
_readable_mtx.unlock();
_writable_mtx.lock();
writable += toRead;
_writable_mtx.unlock();
readc = (readc + toRead) % size;
canWriteVar.notify_one();
}
return len;
}
int waitUntilReadable() {
if (_stopReader) { return -1; }
int _r = getReadable();
if (_r != 0) { return _r; }
std::unique_lock<std::mutex> lck(_readable_mtx);
canReadVar.wait(lck, [=](){ return ((this->getReadable(false) > 0) || this->getReadStop()); });
if (_stopReader) { return -1; }
return getReadable(false);
}
int getReadable(bool lock = true) {
if (lock) { _readable_mtx.lock(); };
int _r = readable;
if (lock) { _readable_mtx.unlock(); };
return _r;
}
int write(T* data, int len) {
int dataWritten = 0;
int toWrite = 0;
while (dataWritten < len) {
toWrite = std::min<int>(waitUntilwritable(), len - dataWritten);
if (toWrite < 0) { return -1; };
if ((toWrite + writec) > size) {
memcpy(&_buffer[writec], &data[dataWritten], (size - writec) * sizeof(T));
memcpy(&_buffer[0], &data[dataWritten + (size - writec)], (toWrite - (size - writec)) * sizeof(T));
}
else {
memcpy(&_buffer[writec], &data[dataWritten], toWrite * sizeof(T));
}
dataWritten += toWrite;
_readable_mtx.lock();
readable += toWrite;
_readable_mtx.unlock();
_writable_mtx.lock();
writable -= toWrite;
_writable_mtx.unlock();
writec = (writec + toWrite) % size;
canReadVar.notify_one();
}
return len;
}
int waitUntilwritable() {
if (_stopWriter) { return -1; }
int _w = getWritable();
if (_w != 0) { return _w; }
std::unique_lock<std::mutex> lck(_writable_mtx);
canWriteVar.wait(lck, [=](){ return ((this->getWritable(false) > 0) || this->getWriteStop()); });
if (_stopWriter) { return -1; }
return getWritable(false);
}
int getWritable(bool lock = true) {
if (lock) { _writable_mtx.lock(); };
int _w = writable;
if (lock) { _writable_mtx.unlock(); _readable_mtx.lock(); };
int _r = readable;
if (lock) { _readable_mtx.unlock(); };
return std::max<int>(std::min<int>(_w, maxLatency - _r), 0);
}
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 readable;
int writable;
int maxLatency;
bool _stopReader;
bool _stopWriter;
std::mutex _readable_mtx;
std::mutex _writable_mtx;
std::condition_variable canReadVar;
std::condition_variable canWriteVar;
};
};

55
core/src/dsp/types.h Normal file
View File

@ -0,0 +1,55 @@
#pragma once
namespace dsp {
struct complex_t {
float q;
float i;
complex_t operator+(complex_t& c) {
complex_t res;
res.i = c.i + i;
res.q = c.q + q;
return res;
}
complex_t operator-(complex_t& c) {
complex_t res;
res.i = i - c.i;
res.q = q - c.q;
return res;
}
complex_t operator*(float& f) {
complex_t res;
res.i = i * f;
res.q = q * f;
return res;
}
};
struct StereoFloat_t {
float l;
float r;
StereoFloat_t operator+(StereoFloat_t& s) {
StereoFloat_t res;
res.l = s.l + l;
res.r = s.r + r;
return res;
}
StereoFloat_t operator-(StereoFloat_t& s) {
StereoFloat_t res;
res.l = l - s.l;
res.r = r - s.r;
return res;
}
StereoFloat_t operator*(float& f) {
StereoFloat_t res;
res.l = l * f;
res.r = r * f;
return res;
}
};
};

103
core/src/dsp/vfo.h Normal file
View File

@ -0,0 +1,103 @@
#pragma once
#include <dsp/source.h>
#include <dsp/math.h>
#include <dsp/resampling.h>
#include <dsp/filter.h>
#include <spdlog/spdlog.h>
#include <dsp/block.h>
namespace dsp {
class VFO {
public:
VFO() {
}
void init(stream<complex_t>* in, float inputSampleRate, float outputSampleRate, float bandWidth, float offset, int blockSize) {
_input = in;
_outputSampleRate = outputSampleRate;
_inputSampleRate = inputSampleRate;
_bandWidth = bandWidth;
_blockSize = blockSize;
output = &resamp.output;
lo.init(offset, inputSampleRate, blockSize);
mixer.init(in, &lo.output, blockSize);
//resamp.init(&mixer.output, inputSampleRate, outputSampleRate, blockSize, _bandWidth * 0.8f, _bandWidth);
resamp.init(mixer.out[0], inputSampleRate, outputSampleRate, blockSize, _bandWidth * 0.8f, _bandWidth);
}
void start() {
lo.start();
mixer.start();
resamp.start();
}
void stop(bool resampler = true) {
lo.stop();
mixer.stop();
if (resampler) { resamp.stop(); };
}
void setInputSampleRate(float inputSampleRate, int blockSize = -1) {
lo.stop();
lo.setSampleRate(inputSampleRate);
_inputSampleRate = inputSampleRate;
if (blockSize > 0) {
_blockSize = blockSize;
mixer.stop();
lo.setBlockSize(_blockSize);
mixer.setBlockSize(_blockSize);
mixer.start();
}
resamp.setInputSampleRate(inputSampleRate, _blockSize, _bandWidth * 0.8f, _bandWidth);
lo.start();
}
void setOutputSampleRate(float outputSampleRate, float bandWidth = -1) {
if (bandWidth > 0) {
_bandWidth = bandWidth;
}
resamp.setOutputSampleRate(outputSampleRate, _bandWidth * 0.8f, _bandWidth);
}
void setBandwidth(float bandWidth) {
_bandWidth = bandWidth;
resamp.setFilterParams(_bandWidth * 0.8f, _bandWidth);
}
void setOffset(float offset) {
lo.setFrequency(-offset);
}
void setBlockSize(int blockSize) {
stop(false);
_blockSize = blockSize;
lo.setBlockSize(_blockSize);
mixer.setBlockSize(_blockSize);
resamp.setBlockSize(_blockSize);
start();
}
int getOutputBlockSize() {
return resamp.getOutputBlockSize();
}
stream<complex_t>* output;
private:
SineSource lo;
//Multiplier mixer;
DemoMultiplier mixer;
FIRResampler<complex_t> resamp;
DecimatingFIRFilter filter;
stream<complex_t>* _input;
float _outputSampleRate;
float _inputSampleRate;
float _bandWidth;
float _blockSize;
};
};