diff --git a/core/src/dsp/audio.h b/core/src/dsp/audio.h new file mode 100644 index 0000000..7122875 --- /dev/null +++ b/core/src/dsp/audio.h @@ -0,0 +1,92 @@ +#pragma once +#include +#include + +namespace dsp { + class MonoToStereo : public generic_block { + public: + MonoToStereo() {} + + MonoToStereo(stream* in) { init(in); } + + ~MonoToStereo() { generic_block::stop(); } + + void init(stream* in) { + _in = in; + generic_block::registerInput(_in); + generic_block::registerOutput(&out); + } + + void setInput(stream* in) { + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + _in = in; + generic_block::tempStart(); + } + + int run() { + count = _in->read(); + if (count < 0) { return -1; } + + if (out.aquire() < 0) { return -1; } + for (int i = 0; i < count; i++) { + out.data[i].l = _in->data[i]; + out.data[i].r = _in->data[i]; + } + + _in->flush(); + out.write(count); + return count; + } + + stream out; + + private: + int count; + stream* _in; + + }; + + class StereoToMono : public generic_block { + public: + StereoToMono() {} + + StereoToMono(stream* in) { init(in); } + + ~StereoToMono() { generic_block::stop(); } + + void init(stream* in) { + _in = in; + generic_block::registerInput(_in); + generic_block::registerOutput(&out); + } + + void setInput(stream* in) { + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + _in = in; + generic_block::tempStart(); + } + + int run() { + count = _in->read(); + if (count < 0) { return -1; } + + if (out.aquire() < 0) { return -1; } + for (int i = 0; i < count; i++) { + out.data[i] = (_in->data[i].l + _in->data[i].r) / 2.0f; + } + + _in->flush(); + out.write(count); + return count; + } + + stream out; + + private: + int count; + stream* _in; + + }; +} \ No newline at end of file diff --git a/core/src/dsp/block.h b/core/src/dsp/block.h index 111636c..8a5a37d 100644 --- a/core/src/dsp/block.h +++ b/core/src/dsp/block.h @@ -1,251 +1,121 @@ #pragma once -#include +#include #include -#include +#include +#include +#include +#include + +#define FL_M_PI 3.1415926535f namespace dsp { - template - class Block { + + template + class generic_block { public: - Block(std::vector inBs, std::vector 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(outBs[i] * 2)); - } - } + virtual void init() {} - void start() { + virtual void start() { + std::lock_guard lck(ctrlMtx); if (running) { return; } + doStart(); + } + + virtual void stop() { + std::lock_guard lck(ctrlMtx); + if (!running && !tempStopped) { + return; + } + doStop(); + } + + virtual int calcOutSize(int inSize) { return inSize; } + + virtual int run() = 0; + + friend BLOCK; + + private: + void workerLoop() { + while (run() >= 0); + } + + void aquire() { + ctrlMtx.lock(); + } + + void release() { + ctrlMtx.unlock(); + } + + void registerInput(untyped_steam* inStream) { + inputs.push_back(inStream); + } + + void unregisterInput(untyped_steam* inStream) { + inputs.erase(std::remove(inputs.begin(), inputs.end(), inStream), inputs.end()); + } + + void registerOutput(untyped_steam* outStream) { + outputs.push_back(outStream); + } + + void unregisterOutput(untyped_steam* outStream) { + outputs.erase(std::remove(outputs.begin(), outputs.end(), outStream), outputs.end()); + } + + virtual void doStart() { running = true; - startHandler(); - workerThread = std::thread(worker, derived); + workerThread = std::thread(&generic_block::workerLoop, this); } - void stop() { - if (!running) { - return; + virtual void doStop() { + for (auto const& in : inputs) { + in->stopReader(); } - stopHandler(); - for (auto is : in) { - is->stopReader(); + for (auto const& out : outputs) { + out->stopWriter(); } - for (auto os : out) { - os->stopWriter(); + + // TODO: Make sure this isn't needed, I don't know why it stops + if (workerThread.joinable()) { + workerThread.join(); } - workerThread.join(); - for (auto is : in) { - is->clearReadStop(); + for (auto const& in : inputs) { + in->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; + for (auto const& out : outputs) { + out->clearWriteStop(); } } - std::vector*> out; + void tempStart() { + if (tempStopped) { + doStart(); + tempStopped = false; + } + } + + void tempStop() { + if (running && !tempStopped) { + doStop(); + tempStopped = true; + } + } + + std::vector inputs; + std::vector outputs; + + bool running = false; + bool tempStopped = false; + + std::thread workerThread; protected: - virtual void startHandler() {} - virtual void stopHandler() {} - std::vector*> in; - std::vector inputBlockSize; - std::vector outputBlockSize; - bool running = false; - - private: - void (*worker)(D* _this); - std::thread workerThread; - D* derived; + std::mutex ctrlMtx; }; - - - class DemoMultiplier : public Block { - public: - DemoMultiplier() : Block({2}, {1}, this, worker) {} - - void init(stream* a, stream* 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* inA = _this->in[0]; - stream* inB = _this->in[1]; - stream* 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); - } - - }; - - class Squelch : public Block { - public: - Squelch() : Block({1}, {1}, this, worker) {} - - void init(stream* input, int blockSize) { - in[0] = input; - inputBlockSize[0] = blockSize; - out[0]->setMaxLatency(blockSize * 2); - outputBlockSize[0] = blockSize; - level = -50.0f; - } - - float level; - int onCount; - int offCount; - - private: - static void worker(Squelch* _this) { - int blockSize = _this->inputBlockSize[0]; - stream* in = _this->in[0]; - stream* out = _this->out[0]; - complex_t* buf = new complex_t[blockSize]; - - int _on = 0, _off = 0; - bool active = false; - - while (true) { - if (in->read(buf, blockSize) < 0) { break; }; - for (int i = 0; i < blockSize; i++) { - if (log10(sqrt((buf[i].i*buf[i].i) + (buf[i].q*buf[i].q))) * 10.0f > _this->level) { - _on++; - _off = 0; - } - else { - _on = 0; - _off++; - } - if (_on >= _this->onCount && !active) { - _on = _this->onCount; - active = true; - } - if (_off >= _this->offCount && active) { - _off = _this->offCount; - active = false; - } - if (!active) { - buf[i].i = 0.0f; - buf[i].q = 0.0f; - } - } - if (out->write(buf, blockSize) < 0) { break; }; - } - delete[] buf; - } - - }; - - - - - - template - class Reshaper { - public: - Reshaper() { - - } - - void init(int outBlockSize, dsp::stream* input) { - outputBlockSize = outBlockSize; - in = input; - out.init(outputBlockSize * 2); - } - - void setOutputBlockSize(int blockSize) { - if (running) { - return; - } - outputBlockSize = blockSize; - out.setMaxLatency(outputBlockSize * 2); - } - - void setInput(dsp::stream* input) { - if (running) { - return; - } - in = input; - } - - void start() { - if (running) { - return; - } - workerThread = std::thread(_worker, this); - running = true; - } - - void stop() { - if (!running) { - return; - } - in->stopReader(); - out.stopWriter(); - workerThread.join(); - in->clearReadStop(); - out.clearWriteStop(); - running = false; - } - - dsp::stream out; - - private: - static void _worker(Reshaper* _this) { - T* buf = new T[_this->outputBlockSize]; - while (true) { - if (_this->in->read(buf, _this->outputBlockSize) < 0) { break; } - if (_this->out.write(buf, _this->outputBlockSize) < 0) { break; } - } - delete[] buf; - } - - int outputBlockSize; - bool running = false; - std::thread workerThread; - - dsp::stream* in; - - }; -}; \ No newline at end of file +} \ No newline at end of file diff --git a/core/src/dsp/buffer.h b/core/src/dsp/buffer.h new file mode 100644 index 0000000..eb37677 --- /dev/null +++ b/core/src/dsp/buffer.h @@ -0,0 +1,216 @@ +#pragma once +#include + +#define RING_BUF_SZ + +namespace dsp { + template + class RingBuffer { + public: + RingBuffer() { + + } + + RingBuffer(int maxLatency) { init(maxLatency); } + + ~RingBuffer() { delete _buffer; } + + void init(int maxLatency) { + size = RING_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(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(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(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 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(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 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(std::min(_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; + }; +}; \ No newline at end of file diff --git a/core/src/dsp/correction.h b/core/src/dsp/correction.h deleted file mode 100644 index dc6db7b..0000000 --- a/core/src/dsp/correction.h +++ /dev/null @@ -1,234 +0,0 @@ -#pragma once -#include -#include -#include -#include - -namespace dsp { - class DCBiasRemover { - public: - DCBiasRemover() { - - } - - DCBiasRemover(stream* input, int bufferSize) : output(bufferSize * 2) { - _in = input; - _bufferSize = bufferSize; - bypass = false; - } - - void init(stream* 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); - } - - void setInput(stream* input) { - if (running) { - return; - } - _in = input; - } - - stream 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; - // } - - static void _worker(DCBiasRemover* _this) { - complex_t* buf = new complex_t[_this->_bufferSize]; - complex_t* mixBuf = new complex_t[_this->_bufferSize]; - - float currentPhase = 0.0f; - float lastPhase = 0.0f; - double phase = 0.0f; - - while (true) { - float ibias = 0.0f; - float qbias = 0.0f; - if (_this->_in->read(buf, _this->_bufferSize) < 0) { break; }; - if (_this->bypass) { - if (_this->output.write(buf, _this->_bufferSize) < 0) { break; }; - continue; - } - - // Detect the frequency of the signal - double avgDiff = 0.0f; - // for (int i = 0; i < _this->_bufferSize; i++) { - // currentPhase = fast_arctan2(buf[i].i, buf[i].q); - // float diff = currentPhase - lastPhase; - // if (diff > 3.1415926535f) { diff -= 2 * 3.1415926535f; } - // else if (diff <= -3.1415926535f) { diff += 2 * 3.1415926535f; } - // avgDiff += diff; - // lastPhase = currentPhase; - // } - // avgDiff /= (double)_this->_bufferSize; - // avgDiff /= (double)_this->_bufferSize; - - // Average the samples to "filter" the signal to the block frequency - for (int i = 0; i < _this->_bufferSize; i++) { - ibias += buf[i].i; - qbias += buf[i].q; - } - ibias /= _this->_bufferSize; - qbias /= _this->_bufferSize; - - // Get the phase difference from the last block - currentPhase = fast_arctan2(ibias, qbias); - float diff = currentPhase - lastPhase; - if (diff > 3.1415926535f) { diff -= 2 * 3.1415926535f; } - else if (diff <= -3.1415926535f) { diff += 2 * 3.1415926535f; } - avgDiff += diff; - lastPhase = currentPhase; - avgDiff /= (double)_this->_bufferSize; - - // Generate a correction signal using the phase difference - for (int i = 0; i < _this->_bufferSize; i++) { - mixBuf[i].i = sin(phase); - mixBuf[i].q = cos(phase); - phase -= avgDiff; - phase = fmodl(phase, 2.0 * 3.1415926535); - } - - // Mix the correction signal with the original signal to shift the unwanted signal - // to the center. Also, null out the real component so that symetric - // frequencies are removed (at least I hope...) - float tq; - for (int i = 0; i < _this->_bufferSize; i++) { - buf[i].i = ((mixBuf[i].i * buf[i].q) + (mixBuf[i].q * buf[i].i)) * 1.4142; - buf[i].q = 0; - } - - if (_this->output.write(buf, _this->_bufferSize) < 0) { break; }; - } - delete[] buf; - } - - stream* _in; - int _bufferSize; - std::thread _workerThread; - bool running = false; - }; - - class ComplexToStereo { - public: - ComplexToStereo() { - - } - - ComplexToStereo(stream* input, int bufferSize) : output(bufferSize * 2) { - _in = input; - _bufferSize = bufferSize; - } - - void init(stream* 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 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* _in; - int _bufferSize; - std::thread _workerThread; - bool running = false; - }; -}; \ No newline at end of file diff --git a/core/src/dsp/demodulator.h b/core/src/dsp/demodulator.h index 5c86559..2111657 100644 --- a/core/src/dsp/demodulator.h +++ b/core/src/dsp/demodulator.h @@ -1,424 +1,260 @@ #pragma once -#include -#include -#include -#include -#include +#include +#include -/* - 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 +#define FAST_ATAN2_COEF1 FL_M_PI / 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; - } + float abs_y = fabsf(y); + float r, angle; + if (x == 0.0f && y == 0.0f) { return 0.0f; } + if (x>=0.0f) { + 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.0f) { + return -angle; + } return angle; } namespace dsp { - class FMDemodulator { + class FMDemod : public generic_block { public: - FMDemodulator() { - - } + FMDemod() {} - FMDemodulator(stream* in, float deviation, long sampleRate, int blockSize) : output(blockSize * 2) { - running = false; - _input = in; - _blockSize = blockSize; - _phase = 0.0f; - _deviation = deviation; + FMDemod(stream* in, float sampleRate, float deviation) { init(in, sampleRate, deviation); } + + ~FMDemod() { generic_block::stop(); } + + void init(stream* in, float sampleRate, float deviation) { + _in = in; _sampleRate = sampleRate; - _phasorSpeed = (2 * 3.1415926535) / (sampleRate / deviation); + _deviation = deviation; + phasorSpeed = (_sampleRate / _deviation) / (2 * FL_M_PI); + generic_block::registerInput(_in); + generic_block::registerOutput(&out); } - void init(stream* 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 setInput(stream* in) { + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + _in = in; + generic_block::tempStart(); } void setSampleRate(float sampleRate) { + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); _sampleRate = sampleRate; - _phasorSpeed = (2 * 3.1415926535) / (sampleRate / _deviation); + phasorSpeed = (_sampleRate / _deviation) / (2 * FL_M_PI); + generic_block::tempStart(); + } + + float getSampleRate() { + return _sampleRate; } void setDeviation(float deviation) { + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); _deviation = deviation; - _phasorSpeed = (2 * 3.1415926535) / (_sampleRate / _deviation); + phasorSpeed = (_sampleRate / _deviation) / (2 * FL_M_PI); + generic_block::tempStart(); } - stream output; + float getDeviation() { + return _deviation; + } + + int run() { + count = _in->read(); + if (count < 0) { return -1; } + + // This is somehow faster than volk... + + float diff, currentPhase; + + if (out.aquire() < 0) { return -1; } + for (int i = 0; i < count; i++) { + currentPhase = fast_arctan2(_in->data[i].i, _in->data[i].q); + diff = currentPhase - phase; + if (diff > FL_M_PI) { out.data[i] = (diff - 2 * FL_M_PI) * phasorSpeed; } + else if (diff <= -FL_M_PI) { out.data[i] = (diff + 2 * FL_M_PI) * phasorSpeed; } + phase = currentPhase; + } + + _in->flush(); + out.write(count); + return count; + } + + stream out; 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; }; - } - } + int count; + float phase, phasorSpeed, _sampleRate, _deviation; + stream* _in; - stream* _input; - bool running; - int _blockSize; - float _phase; - float _phasorSpeed; - float _deviation; - float _sampleRate; - std::thread _workerThread; }; - - class AMDemodulator { + class AMDemod : public generic_block { public: - AMDemodulator() { - + AMDemod() {} + + AMDemod(stream* in) { init(in); } + + ~AMDemod() { generic_block::stop(); } + + void init(stream* in) { + _in = in; + generic_block::registerInput(_in); + generic_block::registerOutput(&out); } - AMDemodulator(stream* in, int blockSize) : output(blockSize * 2) { - running = false; - _input = in; - _blockSize = blockSize; + void setInput(stream* in) { + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + _in = in; + generic_block::tempStart(); } - void init(stream* in, int blockSize) { - output.init(blockSize * 2); - running = false; - _input = in; - _blockSize = blockSize; + int run() { + count = _in->read(); + if (count < 0) { return -1; } + + if (out.aquire() < 0) { return -1; } + volk_32fc_magnitude_32f(out.data, (lv_32fc_t*)_in->data, count); + + _in->flush(); + out.write(count); + return count; } - 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 output; + stream out; 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; - } + int count; + stream* _in; - stream* _input; - bool running; - int _blockSize; - std::thread _workerThread; }; - class SSBDemod { + class SSBDemod : public generic_block { public: - SSBDemod() { + SSBDemod() {} - } + SSBDemod(stream* in, float sampleRate, float bandWidth, int mode) { init(in, sampleRate, bandWidth, mode); } - void init(stream* 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 output; + ~SSBDemod() { generic_block::stop(); } enum { MODE_USB, MODE_LSB, - MODE_DSB, - _MODE_COUNT + MODE_DSB }; - 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; }; + void init(stream* in, float sampleRate, float bandWidth, int mode) { + _in = in; + _sampleRate = sampleRate; + _bandWidth = bandWidth; + _mode = mode; + phase = lv_cmake(1.0f, 0.0f); + switch (_mode) { + case MODE_USB: + phaseDelta = lv_cmake(std::cos((_bandWidth / _sampleRate) * FL_M_PI), std::sin((_bandWidth / _sampleRate) * FL_M_PI)); + break; + case MODE_LSB: + phaseDelta = lv_cmake(std::cos(-(_bandWidth / _sampleRate) * FL_M_PI), std::sin(-(_bandWidth / _sampleRate) * FL_M_PI)); + break; + case MODE_DSB: + phaseDelta = lv_cmake(1.0f, 0.0f); + break; } - - delete[] inBuf; - delete[] outBuf; + generic_block::registerInput(_in); + generic_block::registerOutput(&out); } - std::thread _workerThread; - SineSource lo; - Multiplier mixer; - int _blockSize; - float _bandWidth; + void setInput(stream* in) { + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + _in = in; + generic_block::tempStart(); + } + + void setSampleRate(float sampleRate) { + // No need to restart + _sampleRate = sampleRate; + switch (_mode) { + case MODE_USB: + phaseDelta = lv_cmake(std::cos((_bandWidth / _sampleRate) * FL_M_PI), std::sin((_bandWidth / _sampleRate) * FL_M_PI)); + break; + case MODE_LSB: + phaseDelta = lv_cmake(std::cos(-(_bandWidth / _sampleRate) * FL_M_PI), std::sin(-(_bandWidth / _sampleRate) * FL_M_PI)); + break; + case MODE_DSB: + phaseDelta = lv_cmake(1.0f, 0.0f); + break; + } + } + + void setBandWidth(float bandWidth) { + // No need to restart + _bandWidth = bandWidth; + switch (_mode) { + case MODE_USB: + phaseDelta = lv_cmake(std::cos((_bandWidth / _sampleRate) * FL_M_PI), std::sin((_bandWidth / _sampleRate) * FL_M_PI)); + break; + case MODE_LSB: + phaseDelta = lv_cmake(std::cos(-(_bandWidth / _sampleRate) * FL_M_PI), std::sin(-(_bandWidth / _sampleRate) * FL_M_PI)); + break; + case MODE_DSB: + phaseDelta = lv_cmake(1.0f, 0.0f); + break; + } + } + + void setMode(int mode) { + _mode = mode; + switch (_mode) { + case MODE_USB: + phaseDelta = lv_cmake(std::cos((_bandWidth / _sampleRate) * FL_M_PI), std::sin((_bandWidth / _sampleRate) * FL_M_PI)); + break; + case MODE_LSB: + phaseDelta = lv_cmake(std::cos(-(_bandWidth / _sampleRate) * FL_M_PI), std::sin(-(_bandWidth / _sampleRate) * FL_M_PI)); + break; + case MODE_DSB: + phaseDelta = lv_cmake(1.0f, 0.0f); + break; + } + } + + int run() { + count = _in->read(); + if (count < 0) { return -1; } + + if (out.aquire() < 0) { return -1; } + volk_32fc_s32fc_x2_rotator_32fc((lv_32fc_t*)out.data, (lv_32fc_t*)_in->data, phaseDelta, &phase, count); + volk_32fc_deinterleave_real_32f(out.data, (lv_32fc_t*)_in->data, count); + + _in->flush(); + out.write(count); + return count; + } + + stream out; + + private: + int count; int _mode; - bool running = false; + float _sampleRate, _bandWidth; + stream* _in; + lv_32fc_t phase; + lv_32fc_t phaseDelta; + }; - - - - - - // class CWDemod { - // public: - // CWDemod() { - - // } - - // void init(stream* 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 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; - // }; -}; \ No newline at end of file +} \ No newline at end of file diff --git a/core/src/dsp/filter.h b/core/src/dsp/filter.h index 7ccb022..35ac6cb 100644 --- a/core/src/dsp/filter.h +++ b/core/src/dsp/filter.h @@ -1,489 +1,167 @@ #pragma once -#include -#include -#include -#include -#include -#include - -#define GET_FROM_RIGHT_BUF(buffer, delayLine, delayLineSz, n) (((n) < 0) ? delayLine[(delayLineSz) + (n)] : buffer[(n)]) +#include +#include +#include & 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 { + template + class FIR : public generic_block> { public: - DecimatingFIRFilter() { - + FIR() {} + + FIR(stream* in, dsp::filter_window::generic_window* window) { init(in, window); } + + ~FIR() { + generic_block>::stop(); + volk_free(buffer); + volk_free(taps); } - DecimatingFIRFilter(stream* input, std::vector taps, int blockSize, float decim) { - output.init((blockSize * 2) / decim); - _in = input; - _blockSize = blockSize; - _tapCount = taps.size(); - delayBuf = new complex_t[_tapCount]; + void init(stream* in, dsp::filter_window::generic_window* window) { + _in = in; - _taps = new float[_tapCount]; - for (int i = 0; i < _tapCount; i++) { - _taps[i] = taps[i]; - } + tapCount = window->getTapCount(); + taps = (float*)volk_malloc(tapCount * sizeof(float), volk_get_alignment()); + window->createTaps(taps, tapCount); - _decim = decim; - - for (int i = 0; i < _tapCount; i++) { - delayBuf[i].i = 0.0f; - delayBuf[i].q = 0.0f; - } - - running = false; + buffer = (T*)volk_malloc(STREAM_BUFFER_SIZE * sizeof(T) * 2, volk_get_alignment()); + bufStart = &buffer[tapCount]; + generic_block>::registerInput(_in); + generic_block>::registerOutput(&out); } - void init(stream* input, std::vector& 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 setInput(stream* in) { + std::lock_guard lck(generic_block>::ctrlMtx); + generic_block>::tempStop(); + _in = in; + generic_block>::tempStart(); } - 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 updateWindow(dsp::filter_window::generic_window* window) { + _window = window; + volk_free(taps); + tapCount = window->getTapCount(); + taps = (float*)volk_malloc(tapCount * sizeof(float), volk_get_alignment()); + window->createTaps(taps, tapCount); } - void setTaps(std::vector& taps) { - if (running) { - return; + int run() { + count = _in->read(); + if (count < 0) { return -1; } + + memcpy(bufStart, _in->data, count * sizeof(T)); + _in->flush(); + + // Write to output + if (out.aquire() < 0) { return -1; } + + if constexpr (std::is_same_v) { + for (int i = 0; i < count; i++) { + volk_32f_x2_dot_prod_32f((float*)&out.data[i], (float*)&buffer[i+1], taps, tapCount); + } } - _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; + if constexpr (std::is_same_v) { + for (int i = 0; i < count; i++) { + volk_32fc_32f_dot_prod_32fc((lv_32fc_t*)&out.data[i], (lv_32fc_t*)&buffer[i+1], taps, tapCount); + } } + + out.write(count); + + memmove(buffer, &buffer[count], tapCount * sizeof(T)); + + return count; } - void setInput(stream* 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 output; + stream out; 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 count; + stream* _in; - 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; + dsp::filter_window::generic_window* _window; - 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; - } + T* bufStart; + T* buffer; + int tapCount; + float* taps; - 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* _in; - complex_t* delayBuf; - int _blockSize; - int _tapCount = 0; - float _decim; - std::thread _workerThread; - float* _taps; - bool running; }; - - class FloatDecimatingFIRFilter { + class BFMDeemp : public generic_block { public: - FloatDecimatingFIRFilter() { - - } + BFMDeemp() {} - FloatDecimatingFIRFilter(stream* input, std::vector taps, int blockSize, float decim) { - output.init((blockSize * 2) / decim); - _in = input; - _blockSize = blockSize; - _tapCount = taps.size(); - delayBuf = new float[_tapCount]; + BFMDeemp(stream* in, float sampleRate, float tau) { init(in, sampleRate, tau); } - _taps = new float[_tapCount]; - for (int i = 0; i < _tapCount; i++) { - _taps[i] = taps[i]; - } + ~BFMDeemp() { generic_block::stop(); } - _decim = decim; - - for (int i = 0; i < _tapCount; i++) { - delayBuf[i] = 0.0f; - } - - running = false; - } - - void init(stream* input, std::vector& 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& 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* 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 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* _in; - float* delayBuf; - int _blockSize; - int _tapCount = 0; - float _decim; - std::thread _workerThread; - float* _taps; - bool running; - }; - - class FMDeemphasis { - public: - FMDeemphasis() { - - } - - FMDeemphasis(stream* input, int bufferSize, float tau, float sampleRate) : output(bufferSize * 2) { - _in = input; - _bufferSize = bufferSize; - bypass = false; + void init(stream* in, float sampleRate, float tau) { + _in = in; + _sampleRate = sampleRate; _tau = tau; + float dt = 1.0f / _sampleRate; + alpha = dt / (_tau + dt); + generic_block::registerInput(_in); + generic_block::registerOutput(&out); + } + + void setInput(stream* in) { + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + _in = in; + generic_block::tempStart(); + } + + void setSampleRate(float sampleRate) { _sampleRate = sampleRate; - } - - void init(stream* 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; + float dt = 1.0f / _sampleRate; + alpha = dt / (_tau + dt); } void setTau(float tau) { - if (running) { - return; - } _tau = tau; + float dt = 1.0f / _sampleRate; + alpha = dt / (_tau + dt); } - stream output; - bool bypass; + int run() { + count = _in->read(); + if (count < 0) { return -1; } + + if (bypass) { + if (out.aquire() < 0) { return -1; } + _in->flush(); + out.write(count); + } + + if (isnan(lastOut)) { + lastOut = 0.0f; + } + if (out.aquire() < 0) { return -1; } + out.data[0] = (alpha * _in->data[0]) + ((1-alpha) * lastOut); + for (int i = 1; i < count; i++) { + out.data[i] = (alpha * _in->data[i]) + ((1 - alpha) * out.data[i - 1]); + } + lastOut = out.data[count - 1]; + + _in->flush(); + out.write(count); + return count; + } + + bool bypass = false; + + stream out; 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* _in; - int _bufferSize; - std::thread _workerThread; - bool running = false; - float _sampleRate; + int count; + float lastOut = 0.0f; + float alpha; float _tau; + float _sampleRate; + stream* _in; + }; -}; \ No newline at end of file +} \ No newline at end of file diff --git a/core/src/dsp/math.h b/core/src/dsp/math.h index 0347378..1d92553 100644 --- a/core/src/dsp/math.h +++ b/core/src/dsp/math.h @@ -1,85 +1,107 @@ #pragma once -#include -#include -#include +#include #include -#ifndef M_PI -#define M_PI 3.1415926535f -#endif - namespace dsp { - class Multiplier { + template + class Add : public generic_block> { public: - Multiplier() { - - } + Add() {} - Multiplier(stream* a, stream* b, int blockSize) : output(blockSize * 2) { + Add(stream* a, stream* b) { init(a, b); } + + ~Add() { generic_block::stop(); } + + void init(stream* a, stream* b) { _a = a; _b = b; - _blockSize = blockSize; + generic_block::registerInput(a); + generic_block::registerInput(b); + generic_block::registerOutput(&out); } - void init(stream* a, stream* b, int blockSize) { - output.init(blockSize * 2); - _a = a; - _b = b; - _blockSize = blockSize; - } - - void start() { - if (running) { - return; + int run() { + a_count = _a->read(); + if (a_count < 0) { return -1; } + b_count = _b->read(); + if (b_count < 0) { return -1; } + if (a_count != b_count) { + _a->flush(); + _b->flush(); + return 0; } - running = true; - _workerThread = std::thread(_worker, this); - } - void stop() { - if (!running) { - return; + if (out.aquire() < 0) { return -1; } + if constexpr (std::is_same_v || std::is_same_v) { + volk_32fc_x2_add_32fc(out.data, _a->data, _b->data, a_count); } - _a->stopReader(); - _b->stopReader(); - output.stopWriter(); - _workerThread.join(); - running = false; - _a->clearReadStop(); - _b->clearReadStop(); - output.clearWriteStop(); - } - - void setBlockSize(int blockSize) { - if (running) { - return; + else { + volk_32f_x2_add_32f(out.data, _a->data, _b->data, a_count); } - _blockSize = blockSize; - output.setMaxLatency(blockSize * 2); + + _a->flush(); + _b->flush(); + out.write(a_count); + return a_count; } - stream output; + stream out; 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); + int a_count, b_count; + stream* _a; + stream* _b; + + }; + + template + class Multiply : public generic_block> { + public: + Multiply() {} + + Multiply(stream* a, stream* b) { init(a, b); } + + ~Multiply() { generic_block::stop(); } + + void init(stream* a, stream* b) { + _a = a; + _b = b; + generic_block::registerInput(a); + generic_block::registerInput(b); + generic_block::registerOutput(&out); } - stream* _a; - stream* _b; - int _blockSize; - bool running = false; - std::thread _workerThread; + int run() { + a_count = _a->read(); + if (a_count < 0) { return -1; } + b_count = _b->read(); + if (b_count < 0) { return -1; } + if (a_count != b_count) { + _a->flush(); + _b->flush(); + return 0; + } + + if (out.aquire() < 0) { return -1; } + if constexpr (std::is_same_v) { + volk_32fc_x2_multiply_32fc(out.data, _a->data, _b->data, a_count); + } + else { + volk_32f_x2_multiply_32f(out.data, _a->data, _b->data, a_count); + } + + _a->flush(); + _b->flush(); + out.write(a_count); + return a_count; + } + + stream out; + + private: + int a_count, b_count; + stream* _a; + stream* _b; + }; -}; \ No newline at end of file +} \ No newline at end of file diff --git a/core/src/dsp/processing.h b/core/src/dsp/processing.h new file mode 100644 index 0000000..f851a56 --- /dev/null +++ b/core/src/dsp/processing.h @@ -0,0 +1,73 @@ +#pragma once +#include + +namespace dsp { + class FrequencyXlator : public generic_block { + public: + FrequencyXlator() {} + + FrequencyXlator(stream* in, float sampleRate, float freq) { init(in, sampleRate, freq); } + + ~FrequencyXlator() { generic_block::stop(); } + + void init(stream* in, float sampleRate, float freq) { + _in = in; + _sampleRate = sampleRate; + _freq = freq; + phase = lv_cmake(1.0f, 0.0f); + phaseDelta = lv_cmake(std::cos((_freq / _sampleRate) * 2.0f * FL_M_PI), std::sin((_freq / _sampleRate) * 2.0f * FL_M_PI)); + generic_block::registerOutput(&out); + } + + void setInputSize(stream* in) { + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + _in = in; + generic_block::tempStart(); + } + + void setSampleRate(float sampleRate) { + // No need to restart + _sampleRate = sampleRate; + phaseDelta = lv_cmake(std::cos((_freq / _sampleRate) * 2.0f * FL_M_PI), std::sin((_freq / _sampleRate) * 2.0f * FL_M_PI)); + } + + float getSampleRate() { + return _sampleRate; + } + + void setFrequency(float freq) { + // No need to restart + _freq = freq; + phaseDelta = lv_cmake(std::cos((_freq / _sampleRate) * 2.0f * FL_M_PI), std::sin((_freq / _sampleRate) * 2.0f * FL_M_PI)); + } + + float getFrequency() { + return _freq; + } + + int run() { + count = _in->read(); + if (count < 0) { return -1; } + + if (out.aquire() < 0) { return -1; } + + volk_32fc_s32fc_x2_rotator_32fc((lv_32fc_t*)out.data, (lv_32fc_t*)_in->data, phaseDelta, &phase, count); + + _in->flush(); + out.write(count); + return count; + } + + stream out; + + private: + int count; + float _sampleRate; + float _freq; + lv_32fc_t phaseDelta; + lv_32fc_t phase; + stream* _in; + + }; +} \ No newline at end of file diff --git a/core/src/dsp/resampling.h b/core/src/dsp/resampling.h index ad8f5b5..f74e756 100644 --- a/core/src/dsp/resampling.h +++ b/core/src/dsp/resampling.h @@ -1,958 +1,153 @@ #pragma once -#include -#include -#include -#include -#include -#include +#include +#include + +#include namespace dsp { template - class Interpolator { + class PolyphaseResampler : public generic_block> { public: - Interpolator() { - + PolyphaseResampler() {} + + PolyphaseResampler(stream* in, dsp::filter_window::generic_window* window, float inSampleRate, float outSampleRate) { init(in, window, inSampleRate, outSampleRate); } + + ~PolyphaseResampler() { + generic_block>::stop(); + volk_free(buffer); + volk_free(taps); } - Interpolator(stream* in, float interpolation, int blockSize) : output(blockSize * interpolation * 2) { - _input = in; - _interpolation = interpolation; - _blockSize = blockSize; - } + void init(stream* in, dsp::filter_window::generic_window* window, float inSampleRate, float outSampleRate) { + _in = in; + _window = window; + _inSampleRate = inSampleRate; + _outSampleRate = outSampleRate; - void init(stream* in, float interpolation, int blockSize) { - output.init(blockSize * 2 * interpolation); - _input = in; - _interpolation = interpolation; - _blockSize = blockSize; - } - - void start() { - if (running) { - return; + tapCount = _window->getTapCount(); + taps = (float*)volk_malloc(tapCount * sizeof(float), volk_get_alignment()); + _window->createTaps(taps, tapCount); + for (int i = 0; i < tapCount / 2; i++) { + float tap = taps[tapCount - i - 1]; + taps[tapCount - i - 1] = taps[i]; + taps[i] = tap; } - _workerThread = std::thread(_worker, this); - running = true; + + int _gcd = std::gcd((int)_inSampleRate, (int)_outSampleRate); + _interp = _outSampleRate / _gcd; + _decim = _inSampleRate / _gcd; + + buffer = (T*)volk_malloc(STREAM_BUFFER_SIZE * sizeof(T) * 2, volk_get_alignment()); + bufStart = &buffer[tapCount]; + generic_block>::registerInput(_in); + generic_block>::registerOutput(&out); } - void stop() { - if (!running) { - return; - } - _input->stopReader(); - output.stopWriter(); - _workerThread.join(); - _input->clearReadStop(); - output.clearWriteStop(); - running = false; + void setInput(stream* in) { + std::lock_guard lck(generic_block>::ctrlMtx); + generic_block>::tempStop(); + _in = in; + generic_block>::tempStart(); } - void setInterpolation(float interpolation) { - if (running) { - return; - } - _interpolation = interpolation; - output.setMaxLatency(_blockSize * _interpolation * 2); + void setInSampleRate(float inSampleRate) { + std::lock_guard lck(generic_block>::ctrlMtx); + generic_block>::tempStop(); + _inSampleRate = inSampleRate; + int _gcd = std::gcd((int)_inSampleRate, (int)_outSampleRate); + _interp = _outSampleRate / _gcd; + _decim = _inSampleRate / _gcd; + generic_block>::tempStart(); } - void setBlockSize(int blockSize) { - if (running) { - return; - } - _blockSize = blockSize; - output.setMaxLatency(_blockSize * _interpolation * 2); + void setOutSampleRate(float outSampleRate) { + std::lock_guard lck(generic_block>::ctrlMtx); + generic_block>::tempStop(); + _outSampleRate = outSampleRate; + int _gcd = std::gcd((int)_inSampleRate, (int)_outSampleRate); + _interp = _outSampleRate / _gcd; + _decim = _inSampleRate / _gcd; + generic_block>::tempStart(); } - void setInput(stream* input) { - if (running) { - return; - } - _input = input; + int getInterpolation() { + return _interp; } - stream output; - - private: - static void _worker(Interpolator* _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; + int getDecimation() { + return _decim; } - stream* _input; - int _blockSize; - float _interpolation; - std::thread _workerThread; - bool running = false; - }; - - class BlockDecimator { - public: - BlockDecimator() { - - } - - BlockDecimator(stream* in, int skip, int blockSize) : output(blockSize * 2) { - _input = in; - _skip = skip; - _blockSize = blockSize; - } - - void init(stream* 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 output; - - private: - static void _worker(BlockDecimator* _this) { - complex_t* buf = new complex_t[_this->_blockSize]; - bool delay = _this->_skip < 0; - - int readCount = std::min(_this->_blockSize + _this->_skip, _this->_blockSize); - int skip = std::max(_this->_skip, 0); - int delaySize = (-_this->_skip) * sizeof(complex_t); - - complex_t* start = &buf[std::max(-_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* _input; - int _blockSize; - int _skip; - std::thread _workerThread; - bool running = false; - }; - - // class FIRResampler { - // public: - // FIRResampler() { - - // } - - // void init(stream* 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(_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(_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(_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* input) { - // if (running) { - // return; - // } - // _input = input; - // } - - // int getOutputBlockSize() { - // return outputBlockSize; - // } - - // stream 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(inCount - tapCount, 0)]; - // int delaySize = tapCount * sizeof(complex_t); - // complex_t* delayBufEnd = &delayBuf[std::max(tapCount - inCount, 0)]; - // int moveSize = std::min(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* _input; - // std::vector _taps; - // int _interp; - // int _decim; - // int outputBlockSize; - // float _outputSampleRate; - // float _inputSampleRate; - // int _blockSize; - // bool running = false; - // }; - - // class FloatFIRResampler { - // public: - // FloatFIRResampler() { - - // } - - // void init(stream* 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(_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(_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(_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* input) { - // if (running) { - // return; - // } - // _input = input; - // } - - // int getOutputBlockSize() { - // return outputBlockSize; - // } - - // stream 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(inCount - tapCount, 0)]; - // int delaySize = tapCount * sizeof(float); - // float* delayBufEnd = &delayBuf[std::max(tapCount - inCount, 0)]; - // int moveSize = std::min(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* _input; - // std::vector _taps; - // int _interp; - // int _decim; - // int outputBlockSize; - // float _outputSampleRate; - // float _inputSampleRate; - // int _blockSize; - // bool running = false; - // }; - - - - - template - class FIRResampler { - public: - FIRResampler() { - - } - - void init(stream* 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(_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 updateWindow(dsp::filter_window::generic_window* window) { + _window = window; + volk_free(taps); + tapCount = window->getTapCount(); + taps = (float*)volk_malloc(tapCount * sizeof(float), volk_get_alignment()); + window->createTaps(taps, tapCount); + for (int i = 0; i < tapCount / 2; i++) { + float tap = taps[tapCount - i - 1]; + taps[tapCount - i - 1] = taps[i]; + taps[i] = tap; } } - void start() { - if (running) { - return; - } - _workerThread = std::thread(_worker, this); - running = true; + int calcOutSize(int in) { + return (in * _interp) / _decim; } - void stop() { - if (!running) { - return; - } - _input->stopReader(); - output.stopWriter(); - _workerThread.join(); - _input->clearReadStop(); - output.clearWriteStop(); - running = false; - } + int run() { + count = _in->read(); + if (count < 0) { return -1; } - 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; + int outCount = calcOutSize(count); - float cutoff = std::min(_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); - } + memcpy(bufStart, _in->data, count * sizeof(T)); + _in->flush(); - 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(_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* input) { - if (running) { - return; - } - _input = input; - } - - int getOutputBlockSize() { - return outputBlockSize; - } - - stream output; - - private: - - // Float worker - static void _worker(FIRResampler* _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(inCount - tapCount, 0)]; - int delaySize = tapCount * sizeof(T); - T* delayBufEnd = &delayBuf[std::max(tapCount - inCount, 0)]; - int moveSize = std::min(inCount, tapCount - inCount) * sizeof(T); - int inSize = inCount * sizeof(T); - - int afterInterp = inCount * interp; + // Write to output + if (out.aquire() < 0) { return -1; } int outIndex = 0; - while (true) { - if (_this->_input->read(inBuf, inCount) < 0) { break; }; - - if constexpr (std::is_same_v) { - 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) { + for (int i = 0; outIndex < outCount; i += _decim) { + out.data[outIndex] = 0; + for (int j = i % _interp; j < tapCount; j += _interp) { + out.data[outIndex] += buffer[((i - j) / _interp) + tapCount] * taps[j]; } + outIndex++; } - if constexpr (std::is_same_v) { - 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; + if constexpr (std::is_same_v) { + for (int i = 0; outIndex < outCount; i += _decim) { + out.data[outIndex].i = 0; + out.data[outIndex].q = 0; + for (int j = i % _interp; j < tapCount; j += _interp) { + out.data[outIndex].i += buffer[((i - j) / _interp) + tapCount].i * taps[j]; + out.data[outIndex].q += buffer[((i - j) / _interp) + tapCount].q * taps[j]; + } + outIndex++; + } + } + out.write(count); + + memmove(buffer, &buffer[count], tapCount * sizeof(T)); + + return count; } - std::thread _workerThread; - stream* _input; - std::vector _taps; - int _interp; - int _decim; - int outputBlockSize; - float _outputSampleRate; - float _inputSampleRate; - int _blockSize; - bool running = false; + stream out; + + private: + int count; + stream* _in; + + dsp::filter_window::generic_window* _window; + + T* bufStart; + T* buffer; + int tapCount; + int _interp, _decim; + float _inSampleRate, _outSampleRate; + float* taps; + }; - - // class FloatPolyphaseFIRResampler { - // public: - // FloatPolyphaseFIRResampler() { - - // } - - // void init(stream* 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(_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(_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(_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* input) { - // if (running) { - // return; - // } - // _input = input; - // } - - // int getOutputBlockSize() { - // return outputBlockSize; - // } - - // stream 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(inCount - tapCount, 0)]; - // int delaySize = tapCount * sizeof(float); - // float* delayBufEnd = &delayBuf[std::max(tapCount - inCount, 0)]; - // int moveSize = std::min(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* _input; - // std::vector _taps; - // int _interp; - // int _decim; - // int outputBlockSize; - // float _outputSampleRate; - // float _inputSampleRate; - // int _blockSize; - // bool running = false; - // }; -}; \ No newline at end of file +} \ No newline at end of file diff --git a/core/src/dsp/routing.h b/core/src/dsp/routing.h index 5c4a955..d00fb6b 100644 --- a/core/src/dsp/routing.h +++ b/core/src/dsp/routing.h @@ -1,310 +1,102 @@ #pragma once -#include -#include -#include -#include -#include +#include +#include namespace dsp { - class Splitter { - public: - Splitter() { - - } - - Splitter(stream* input, int bufferSize) { - _in = input; - _bufferSize = bufferSize; - output_a.init(bufferSize); - output_b.init(bufferSize); - } - - void init(stream* 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 output_a; - stream 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* _in; - int _bufferSize; - std::thread _workerThread; - bool running = false; - }; - template - class DynamicSplitter { + class Splitter : public generic_block> { public: - DynamicSplitter() { - + Splitter() {} + + Splitter(stream* in) { init(in); } + + ~Splitter() { generic_block::stop(); } + + void init(stream* in) { + _in = in; + generic_block::registerInput(_in); } - DynamicSplitter(stream* input, int bufferSize) { - _in = input; - _bufferSize = bufferSize; + void setInput(stream* in) { + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + generic_block::unregisterInput(_in); + _in = in; + generic_block::registerInput(_in); + generic_block::tempStart(); } - void init(stream* input, int bufferSize) { - _in = input; - _bufferSize = bufferSize; + void bindStream(stream* stream) { + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + out.push_back(stream); + generic_block::registerOutput(stream); + generic_block::tempStart(); } - 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* stream) { - if (running) { - return; - } - outputs.push_back(stream); - } - - void unbind(stream* 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; - } - } + void unbindStream(stream* stream) { + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + generic_block::unregisterOutput(stream); + out.erase(std::remove(out.begin(), out.end(), stream), out.end()); + generic_block::tempStart(); } 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; }; - } + int run() { + // TODO: If too slow, buffering might be necessary + int count = _in->read(); + if (count < 0) { return -1; } + for (const auto& stream : out) { + if (stream->aquire() < 0) { return -1; } + memcpy(stream->data, _in->data, count * sizeof(T)); + stream->write(count); } - delete[] buf; + _in->flush(); + return count; } stream* _in; - int _bufferSize; - std::thread _workerThread; - bool running = false; - std::vector*> outputs; + std::vector*> out; + }; - - class MonoToStereo { + template + class Reshaper : public generic_block> { public: - MonoToStereo() { - + Reshaper() {} + + Reshaper(stream* in) { init(in); } + + ~Reshaper() { generic_block>::stop(); } + + void init(stream* in) { + _in = in; + buffer = (T*)volk_malloc(STREAM_BUFFER_SIZE * sizeof(T), volk_get_alignment()); + generic_block>::registerInput(_in); + generic_block>::registerOutput(&out); } - MonoToStereo(stream* input, int bufferSize) { - _in = input; - _bufferSize = bufferSize; - output.init(bufferSize * 2); + void setInput(stream* in) { + std::lock_guard lck(generic_block>::ctrlMtx); + generic_block>::tempStop(); + _in = in; + generic_block>::tempStart(); } - void init(stream* input, int bufferSize) { - _in = input; - _bufferSize = bufferSize; - output.init(bufferSize * 2); + int run() { + int count = _in->read(); + _in->flush(); + return count; } - 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 output; + stream out; 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* _in; + T* buffer; + int _outBlockSize; + int readCount; - stream* _in; - int _bufferSize; - std::thread _workerThread; - bool running = false; }; - - class StereoToMono { - public: - StereoToMono() { - - } - - StereoToMono(stream* input, int bufferSize) { - _in = input; - _bufferSize = bufferSize; - } - - void init(stream* 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 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* _in; - int _bufferSize; - std::thread _workerThread; - bool running = false; - }; -}; \ No newline at end of file +} \ No newline at end of file diff --git a/core/src/dsp/sink.h b/core/src/dsp/sink.h index 55d8cd4..ce50bf0 100644 --- a/core/src/dsp/sink.h +++ b/core/src/dsp/sink.h @@ -1,140 +1,99 @@ #pragma once -#include -#include -#include -#include -#include - +#include +#include namespace dsp { - class HandlerSink { + template + class HandlerSink : public generic_block> { public: - HandlerSink() { - - } + HandlerSink() {} - HandlerSink(stream* input, complex_t* buffer, int bufferSize, void handler(complex_t*)) { - _in = input; - _bufferSize = bufferSize; - _buffer = buffer; + HandlerSink(stream* in, void (*handler)(T* data, int count, void* ctx), void* ctx) { init(in, handler, ctx); } + + ~HandlerSink() { generic_block>::stop(); } + + void init(stream* in, void (*handler)(T* data, int count, void* ctx), void* ctx) { + _in = in; _handler = handler; + _ctx = ctx; + generic_block>::registerInput(_in); } - void init(stream* input, complex_t* buffer, int bufferSize, void handler(complex_t*)) { - _in = input; - _bufferSize = bufferSize; - _buffer = buffer; + void setInput(stream* in) { + std::lock_guard lck(generic_block>::ctrlMtx); + generic_block>::tempStop(); + _in = in; + generic_block>::tempStart(); + } + + void setHandler(void (*handler)(T* data, int count, void* ctx), void* ctx) { + std::lock_guard lck(generic_block>::ctrlMtx); + generic_block>::tempStop(); _handler = handler; + _ctx = ctx; + generic_block>::tempStart(); } - void start() { - if (running) { - return; - } - _workerThread = std::thread(_worker, this); - running = true; + int run() { + count = _in->read(); + if (count < 0) { return -1; } + _handler(_in->data, count, _ctx); + _in->flush(); + return count; } - 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); - } - } + int count; + stream* _in; + void (*_handler)(T* data, int count, void* ctx); + void* _ctx; - stream* _in; - int _bufferSize; - complex_t* _buffer; - std::thread _workerThread; - void (*_handler)(complex_t*); - bool running = false; }; template - class NullSink { + class RingBufferSink : public generic_block> { public: - NullSink() { - + RingBufferSink() {} + + RingBufferSink(stream* in) { init(in); } + + ~RingBufferSink() { generic_block>::stop(); } + + void init(stream* in) { + _in = in; + generic_block>::registerInput(_in); } - NullSink(stream* input, int bufferSize) { - _in = input; - _bufferSize = bufferSize; + void setInput(stream* in) { + std::lock_guard lck(generic_block>::ctrlMtx); + generic_block>::tempStop(); + _in = in; + generic_block>::tempStart(); } - void init(stream* input, int bufferSize) { - _in = input; - _bufferSize = bufferSize; + int run() { + count = _in->read(); + if (count < 0) { return -1; } + if (data.write(_in->data, count) < 0) { return -1; } + _in->flush(); + return count; } - void start() { - _workerThread = std::thread(_worker, this); - } - - bool bypass; + RingBuffer data; private: - static void _worker(NullSink* _this) { - T* buf = new T[_this->_bufferSize]; - while (true) { - //spdlog::info("NS: Reading..."); - _this->_in->read(buf, _this->_bufferSize); + void doStop() { + _in->stopReader(); + data.stopWriter(); + if (generic_block>::workerThread.joinable()) { + generic_block>::workerThread.join(); } + _in->clearReadStop(); + data.clearWriteStop(); } + int count; stream* _in; - int _bufferSize; - std::thread _workerThread; + }; - - class FloatNullSink { - public: - FloatNullSink() { - - } - - FloatNullSink(stream* input, int bufferSize) { - _in = input; - _bufferSize = bufferSize; - } - - void init(stream* input, int bufferSize) { - _in = input; - _bufferSize = bufferSize; - } - - void start() { - spdlog::info("NS: Starting..."); - _workerThread = std::thread(_worker, this); - } - - bool bypass; - - private: - static void _worker(FloatNullSink* _this) { - spdlog::info("NS: Started!"); - float* buf = new float[_this->_bufferSize]; - while (true) { - spdlog::info("NS: Reading..."); - _this->_in->read(buf, _this->_bufferSize); - } - } - - stream* _in; - int _bufferSize; - std::thread _workerThread; - }; -}; \ No newline at end of file +} \ No newline at end of file diff --git a/core/src/dsp/source.h b/core/src/dsp/source.h index 1508372..75dd38a 100644 --- a/core/src/dsp/source.h +++ b/core/src/dsp/source.h @@ -1,92 +1,75 @@ #pragma once -#include -#include -#include -#include +#include namespace dsp { - class SineSource { + class SineSource : public generic_block { public: - SineSource() { - - } + SineSource() {} - SineSource(float frequency, long sampleRate, int blockSize) : output(blockSize * 2) { + SineSource(int blockSize, float sampleRate, float freq) { init(blockSize, sampleRate, freq); } + + ~SineSource() { generic_block::stop(); } + + void init(int blockSize, float sampleRate, float freq) { _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; + _freq = freq; + zeroPhase = (lv_32fc_t*)volk_malloc(STREAM_BUFFER_SIZE * sizeof(lv_32fc_t), volk_get_alignment()); + for (int i = 0; i < STREAM_BUFFER_SIZE; i++) { + zeroPhase[i] = lv_cmake(1.0f, 0.0f); } - _workerThread = std::thread(_worker, this); - running = true; - } - - void stop() { - if (!running) { - return; - } - output.stopWriter(); - _workerThread.join(); - output.clearWriteStop(); - running = false; - } - - void setFrequency(float frequency) { - _frequency = frequency; - _phasorSpeed = (2 * 3.1415926535 * frequency) / _sampleRate; + phase = lv_cmake(1.0f, 0.0f); + phaseDelta = lv_cmake(std::cos((_freq / _sampleRate) * 2.0f * FL_M_PI), std::sin((_freq / _sampleRate) * 2.0f * FL_M_PI)); + generic_block::registerOutput(&out); } void setBlockSize(int blockSize) { - if (running) { - return; - } + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); _blockSize = blockSize; - output.setMaxLatency(blockSize * 2); + generic_block::tempStart(); + } + + int getBlockSize() { + return _blockSize; } void setSampleRate(float sampleRate) { + // No need to restart _sampleRate = sampleRate; - _phasorSpeed = (2 * 3.1415926535 * _frequency) / sampleRate; + phaseDelta = lv_cmake(std::cos((_freq / _sampleRate) * 2.0f * FL_M_PI), std::sin((_freq / _sampleRate) * 2.0f * FL_M_PI)); } - stream output; + float getSampleRate() { + return _sampleRate; + } + + void setFrequency(float freq) { + // No need to restart + _freq = freq; + phaseDelta = lv_cmake(std::cos((_freq / _sampleRate) * 2.0f * FL_M_PI), std::sin((_freq / _sampleRate) * 2.0f * FL_M_PI)); + } + + float getFrequency() { + return _freq; + } + + int run() { + if (out.aquire() < 0) { return -1; } + volk_32fc_s32fc_x2_rotator_32fc((lv_32fc_t*)out.data, zeroPhase, phaseDelta, &phase, _blockSize); + out.write(_blockSize); + return _blockSize; + } + + stream out; 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; + float _sampleRate; + float _freq; + lv_32fc_t phaseDelta; + lv_32fc_t phase; + lv_32fc_t* zeroPhase; + }; -}; \ No newline at end of file +} \ No newline at end of file diff --git a/core/src/dsp/stream.h b/core/src/dsp/stream.h index 93a546f..4dc3b36 100644 --- a/core/src/dsp/stream.h +++ b/core/src/dsp/stream.h @@ -1,228 +1,100 @@ #pragma once +#include #include -#include -#include -#include +#include -#define STREAM_BUF_SZ 1000000 +// 1MB buffer +#define STREAM_BUFFER_SIZE 1000000 namespace dsp { + class untyped_steam { + public: + virtual int aquire() { return -1; } + virtual void write(int size) {} + virtual int read() { return -1; } + virtual void flush() {} + virtual void stopReader() {} + virtual void clearReadStop() {} + virtual void stopWriter() {} + virtual void clearWriteStop() {} + }; + template - class stream { + class stream : public untyped_steam { public: stream() { - + data = (T*)volk_malloc(STREAM_BUFFER_SIZE * sizeof(T), volk_get_alignment()); } - 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(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(); + int aquire() { + waitReady(); + if (writerStop) { + return -1; } - return len; + return 0; } - int readAndSkip(T* data, int len, int skip) { - int dataRead = 0; - int toRead = 0; - while (dataRead < len) { - toRead = std::min(waitUntilReadable(), len - dataRead); - if (toRead < 0) { return -1; }; + void write(int size) { + std::lock_guard lck(sigMtx); + contentSize = size; + dataReady = true; + cv.notify_all(); + } - 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(); + int read() { + waitData(); + if (readerStop) { + return -1; } - dataRead = 0; - while (dataRead < skip) { - toRead = std::min(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; + return contentSize; } - int waitUntilReadable() { - if (_stopReader) { return -1; } - int _r = getReadable(); - if (_r != 0) { return _r; } - std::unique_lock 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(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 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(std::min(_w, maxLatency - _r), 0); + void flush() { + std::lock_guard lck(sigMtx); + dataReady = false; + cv.notify_all(); } void stopReader() { - _stopReader = true; - canReadVar.notify_one(); - } - - void stopWriter() { - _stopWriter = true; - canWriteVar.notify_one(); - } - - bool getReadStop() { - return _stopReader; - } - - bool getWriteStop() { - return _stopWriter; + std::lock_guard lck(sigMtx); + readerStop = true; + cv.notify_all(); } void clearReadStop() { - _stopReader = false; + readerStop = false; + } + + void stopWriter() { + std::lock_guard lck(sigMtx); + writerStop = true; + cv.notify_all(); } void clearWriteStop() { - _stopWriter = false; + writerStop = false; } - void setMaxLatency(int maxLatency) { - this->maxLatency = maxLatency; - } + T* data; 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; + void waitReady() { + std::unique_lock lck(sigMtx); + cv.wait(lck, [this]{ return !dataReady || writerStop; }); + } + + void waitData() { + std::unique_lock lck(sigMtx); + cv.wait(lck, [this]{ return dataReady || readerStop; }); + } + + std::mutex sigMtx; + std::condition_variable cv; + bool dataReady = false; + + bool readerStop = false; + bool writerStop = false; + + int contentSize = 0; }; -}; \ No newline at end of file +} \ No newline at end of file diff --git a/core/src/dsp/types.h b/core/src/dsp/types.h index 5de461b..a1730df 100644 --- a/core/src/dsp/types.h +++ b/core/src/dsp/types.h @@ -4,52 +4,10 @@ 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 { + struct stereo_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; - } }; -}; \ No newline at end of file +} \ No newline at end of file diff --git a/core/src/dsp/vfo.h b/core/src/dsp/vfo.h index 312c612..78b596d 100644 --- a/core/src/dsp/vfo.h +++ b/core/src/dsp/vfo.h @@ -1,103 +1,112 @@ #pragma once -#include -#include -#include -#include -#include #include +#include +#include +#include +#include namespace dsp { class VFO { public: - VFO() { + VFO() {} - } + ~VFO() { stop(); } - void init(stream* in, float inputSampleRate, float outputSampleRate, float bandWidth, float offset, int blockSize) { - _input = in; - _outputSampleRate = outputSampleRate; - _inputSampleRate = inputSampleRate; + VFO(stream* in, float offset, float inSampleRate, float outSampleRate, float bandWidth) { + init(in, offset, inSampleRate, outSampleRate, bandWidth); + }; + + void init(stream* in, float offset, float inSampleRate, float outSampleRate, float bandWidth) { + _in = in; + _offset = offset; + _inSampleRate = inSampleRate; + _outSampleRate = outSampleRate; _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); + float realCutoff = std::min(_bandWidth, std::min(_inSampleRate, _outSampleRate)) / 2.0f; + + xlator.init(_in, _inSampleRate, -_offset); + win.init(realCutoff, realCutoff, inSampleRate); + resamp.init(&xlator.out, &win, _inSampleRate, _outSampleRate); + + win.setSampleRate(_inSampleRate * resamp.getInterpolation()); + resamp.updateWindow(&win); + + out = &resamp.out; } void start() { - lo.start(); - mixer.start(); + if (running) { return; } + xlator.start(); resamp.start(); } - void stop(bool resampler = true) { - lo.stop(); - mixer.stop(); - if (resampler) { resamp.stop(); }; + void stop() { + if (!running) { return; } + xlator.stop(); + 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 setInSampleRate(float inSampleRate) { + _inSampleRate = inSampleRate; + if (running) { xlator.stop(); resamp.stop(); } + xlator.setSampleRate(_inSampleRate); + resamp.setInSampleRate(_inSampleRate); + float realCutoff = std::min(_bandWidth, std::min(_inSampleRate, _outSampleRate)) / 2.0f; + win.setSampleRate(_inSampleRate * resamp.getInterpolation()); + win.setCutoff(realCutoff); + win.setTransWidth(realCutoff); + resamp.updateWindow(&win); + if (running) { xlator.start(); resamp.start(); } } - void setOutputSampleRate(float outputSampleRate, float bandWidth = -1) { - if (bandWidth > 0) { - _bandWidth = bandWidth; - } - resamp.setOutputSampleRate(outputSampleRate, _bandWidth * 0.8f, _bandWidth); + void setOutSampleRate(float outSampleRate) { + _outSampleRate = outSampleRate; + if (running) { resamp.stop(); } + resamp.setOutSampleRate(_outSampleRate); + float realCutoff = std::min(_bandWidth, std::min(_inSampleRate, _outSampleRate)) / 2.0f; + win.setSampleRate(_inSampleRate * resamp.getInterpolation()); + win.setCutoff(realCutoff); + win.setTransWidth(realCutoff); + resamp.updateWindow(&win); + if (running) { resamp.start(); } + } + + void setOutSampleRate(float outSampleRate, float bandWidth) { + _outSampleRate = outSampleRate; + _bandWidth = bandWidth; + if (running) { resamp.stop(); } + resamp.setOutSampleRate(_outSampleRate); + float realCutoff = std::min(_bandWidth, std::min(_inSampleRate, _outSampleRate)) / 2.0f; + win.setSampleRate(_inSampleRate * resamp.getInterpolation()); + win.setCutoff(realCutoff); + win.setTransWidth(realCutoff); + resamp.updateWindow(&win); + if (running) { resamp.start(); } + } + + void setOffset(float offset) { + _offset = offset; + xlator.setFrequency(-_offset); } void setBandwidth(float bandWidth) { _bandWidth = bandWidth; - resamp.setFilterParams(_bandWidth * 0.8f, _bandWidth); + float realCutoff = std::min(_bandWidth, std::min(_inSampleRate, _outSampleRate)) / 2.0f; + win.setCutoff(realCutoff); + win.setTransWidth(realCutoff); + resamp.updateWindow(&win); } - 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* output; + stream* out; private: - SineSource lo; - //Multiplier mixer; - DemoMultiplier mixer; - FIRResampler resamp; - DecimatingFIRFilter filter; - stream* _input; + bool running = false; + float _offset, _inSampleRate, _outSampleRate, _bandWidth; + filter_window::BlackmanWindow win; + stream* _in; + FrequencyXlator xlator; + PolyphaseResampler resamp; - float _outputSampleRate; - float _inputSampleRate; - float _bandWidth; - float _blockSize; }; -}; \ No newline at end of file +} \ No newline at end of file diff --git a/core/src/dsp/window.h b/core/src/dsp/window.h new file mode 100644 index 0000000..b71070e --- /dev/null +++ b/core/src/dsp/window.h @@ -0,0 +1,75 @@ +#pragma once +#include + +namespace dsp { + namespace filter_window { + class generic_window { + public: + virtual int getTapCount() { return -1; } + virtual void createTaps(float* taps, int tapCount) {} + }; + + class BlackmanWindow : public filter_window::generic_window { + public: + BlackmanWindow() {} + BlackmanWindow(float cutoff, float transWidth, float sampleRate) { init(cutoff, transWidth, sampleRate); } + + void init(float cutoff, float transWidth, float sampleRate) { + _cutoff = cutoff; + _transWidth = transWidth; + _sampleRate = sampleRate; + } + + void setSampleRate(float sampleRate) { + _sampleRate = sampleRate; + } + + void setCutoff(float cutoff) { + _cutoff = cutoff; + } + + void setTransWidth(float transWidth) { + _transWidth = transWidth; + } + + int getTapCount() { + 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++; } + + return _M; + } + + void createTaps(float* taps, int tapCount) { + float fc = _cutoff / _sampleRate; + if (fc > 1.0f) { + fc = 1.0f; + } + float tc = tapCount; + float sum = 0.0f; + float val; + for (int i = 0; i < tapCount; i++) { + val = (sin(2.0f * FL_M_PI * fc * ((float)i - (tc / 2))) / ((float)i - (tc / 2))) * + (0.42f - (0.5f * cos(2.0f * FL_M_PI / tc)) + (0.8f * cos(4.0f * FL_M_PI / tc))); + taps[tapCount - i - 1] = val; // tapCount - i - 1 + sum += val; + } + for (int i = 0; i < tapCount; i++) { + taps[i] /= sum; + } + } + + private: + float _cutoff, _transWidth, _sampleRate; + + }; + } +} \ No newline at end of file diff --git a/core/src/gui/main_window.cpp b/core/src/gui/main_window.cpp index 057519b..fa16bf4 100644 --- a/core/src/gui/main_window.cpp +++ b/core/src/gui/main_window.cpp @@ -47,7 +47,7 @@ int fftSize = 8192 * 8; std::vector _data; std::vector fftTaps; -void fftHandler(dsp::complex_t* samples) { +void fftHandler(dsp::complex_t* samples, int count, void* ctx) { fftwf_execute(p); int half = fftSize / 2; @@ -414,7 +414,7 @@ void drawWindow() { ImGui::Text("Center Frequency: %.0f Hz", gui::waterfall.getCenterFrequency()); ImGui::Text("Source name: %s", sourceName.c_str()); if (ImGui::Checkbox("Test technique", &dcbias.val)) { - sigpath::signalPath.setDCBiasCorrection(dcbias.val); + //sigpath::signalPath.setDCBiasCorrection(dcbias.val); } ImGui::Spacing(); } diff --git a/core/src/io/audio.h b/core/src/io/audio.h index b78d7da..e6fd495 100644 --- a/core/src/io/audio.h +++ b/core/src/io/audio.h @@ -5,6 +5,7 @@ #include #include #include +#include namespace io { class AudioSink { @@ -30,7 +31,7 @@ namespace io { AudioSink(int bufferSize) { _bufferSize = bufferSize; monoBuffer = new float[_bufferSize]; - stereoBuffer = new dsp::StereoFloat_t[_bufferSize]; + stereoBuffer = new dsp::stereo_t[_bufferSize]; _volume = 1.0f; Pa_Initialize(); @@ -81,7 +82,7 @@ namespace io { void init(int bufferSize) { _bufferSize = bufferSize; monoBuffer = new float[_bufferSize]; - stereoBuffer = new dsp::StereoFloat_t[_bufferSize]; + stereoBuffer = new dsp::stereo_t[_bufferSize]; _volume = 1.0f; Pa_Initialize(); @@ -130,11 +131,11 @@ namespace io { } void setMonoInput(dsp::stream* input) { - _monoInput = input; + monoSink.setInput(input); } - void setStereoInput(dsp::stream* input) { - _stereoInput = input; + void setStereoInput(dsp::stream* input) { + stereoSink.setInput(input); } void setVolume(float volume) { @@ -158,10 +159,12 @@ namespace io { if (streamType == MONO) { err = Pa_OpenStream(&stream, NULL, &outputParams, _sampleRate, _bufferSize, 0, (dev.channels == 2) ? _mono_to_stereo_callback : _mono_to_mono_callback, this); + monoSink.start(); } else { err = Pa_OpenStream(&stream, NULL, &outputParams, _sampleRate, _bufferSize, 0, (dev.channels == 2) ? _stereo_to_stereo_callback : _stereo_to_mono_callback, this); + stereoSink.start(); } if (err != 0) { @@ -182,18 +185,20 @@ namespace io { return; } if (streamType == MONO) { - _monoInput->stopReader(); + monoSink.stop(); + monoSink.data.stopReader(); } else { - _stereoInput->stopReader(); + stereoSink.stop(); + stereoSink.data.stopReader(); } Pa_StopStream(stream); Pa_CloseStream(stream); if (streamType == MONO) { - _monoInput->clearReadStop(); + monoSink.data.clearReadStop(); } else { - _stereoInput->clearReadStop(); + stereoSink.data.clearReadStop(); } running = false; } @@ -206,7 +211,7 @@ namespace io { delete[] monoBuffer; delete[] stereoBuffer; monoBuffer = new float[_bufferSize]; - stereoBuffer = new dsp::StereoFloat_t[_bufferSize]; + stereoBuffer = new dsp::stereo_t[_bufferSize]; } void setSampleRate(float sampleRate) { @@ -248,7 +253,7 @@ namespace io { PaStreamCallbackFlags statusFlags, void *userData ) { AudioSink* _this = (AudioSink*)userData; float* outbuf = (float*)output; - if (_this->_monoInput->read(_this->monoBuffer, frameCount) < 0) { + if (_this->monoSink.data.read(_this->monoBuffer, frameCount) < 0) { memset(outbuf, 0, sizeof(float) * frameCount); return 0; } @@ -266,9 +271,9 @@ namespace io { const PaStreamCallbackTimeInfo* timeInfo, PaStreamCallbackFlags statusFlags, void *userData ) { AudioSink* _this = (AudioSink*)userData; - dsp::StereoFloat_t* outbuf = (dsp::StereoFloat_t*)output; - if (_this->_stereoInput->read(_this->stereoBuffer, frameCount) < 0) { - memset(outbuf, 0, sizeof(dsp::StereoFloat_t) * frameCount); + dsp::stereo_t* outbuf = (dsp::stereo_t*)output; + if (_this->stereoSink.data.read(_this->stereoBuffer, frameCount) < 0) { + memset(outbuf, 0, sizeof(dsp::stereo_t) * frameCount); return 0; } @@ -288,9 +293,9 @@ namespace io { const PaStreamCallbackTimeInfo* timeInfo, PaStreamCallbackFlags statusFlags, void *userData ) { AudioSink* _this = (AudioSink*)userData; - dsp::StereoFloat_t* outbuf = (dsp::StereoFloat_t*)output; - if (_this->_monoInput->read(_this->monoBuffer, frameCount) < 0) { - memset(outbuf, 0, sizeof(dsp::StereoFloat_t) * frameCount); + dsp::stereo_t* outbuf = (dsp::stereo_t*)output; + if (_this->monoSink.data.read(_this->monoBuffer, frameCount) < 0) { + memset(outbuf, 0, sizeof(dsp::stereo_t) * frameCount); return 0; } @@ -309,7 +314,7 @@ namespace io { PaStreamCallbackFlags statusFlags, void *userData ) { AudioSink* _this = (AudioSink*)userData; float* outbuf = (float*)output; - if (_this->_stereoInput->read(_this->stereoBuffer, frameCount) < 0) { + if (_this->stereoSink.data.read(_this->stereoBuffer, frameCount) < 0) { memset(outbuf, 0, sizeof(float) * frameCount); return 0; } @@ -338,10 +343,10 @@ namespace io { int defaultDev; double _sampleRate; int _bufferSize; - dsp::stream* _monoInput; - dsp::stream* _stereoInput; + dsp::RingBufferSink monoSink; + dsp::RingBufferSink stereoSink; float* monoBuffer; - dsp::StereoFloat_t* stereoBuffer; + dsp::stereo_t* stereoBuffer; float _volume = 1.0f; PaStream *stream; bool running = false; diff --git a/core/src/signal_path/audio.cpp b/core/src/signal_path/audio.cpp index d99fb7e..e00b06c 100644 --- a/core/src/signal_path/audio.cpp +++ b/core/src/signal_path/audio.cpp @@ -12,7 +12,7 @@ namespace audio { astr->deviceId = astr->audio->getDeviceId(); double sampleRate = astr->audio->devices[astr->deviceId].sampleRates[0]; int blockSize = sampleRate / 200.0; // default block size - astr->monoAudioStream = new dsp::stream(blockSize * 2); + astr->monoAudioStream = new dsp::stream; astr->audio->setBlockSize(blockSize); astr->audio->setStreamType(io::AudioSink::MONO); astr->audio->setMonoInput(astr->monoAudioStream); @@ -21,8 +21,8 @@ namespace audio { astr->sampleRate = sampleRate; astr->monoStream = stream; astr->sampleRateChangeHandler = sampleRateChangeHandler; - astr->monoDynSplit = new dsp::DynamicSplitter(stream, blockSize); - astr->monoDynSplit->bind(astr->monoAudioStream); + astr->monoDynSplit = new dsp::Splitter(stream); + astr->monoDynSplit->bindStream(astr->monoAudioStream); astr->running = false; astr->volume = 1.0f; astr->sampleRateId = 0; @@ -31,7 +31,7 @@ namespace audio { return sampleRate; } - double registerStereoStream(dsp::stream* stream, std::string name, std::string vfoName, int (*sampleRateChangeHandler)(void* ctx, double sampleRate), void* ctx) { + double registerStereoStream(dsp::stream* stream, std::string name, std::string vfoName, int (*sampleRateChangeHandler)(void* ctx, double sampleRate), void* ctx) { AudioStream_t* astr = new AudioStream_t; astr->type = STREAM_TYPE_STEREO; astr->ctx = ctx; @@ -39,7 +39,7 @@ namespace audio { astr->audio->init(1); double sampleRate = astr->audio->devices[astr->audio->getDeviceId()].sampleRates[0]; int blockSize = sampleRate / 200.0; // default block size - astr->stereoAudioStream = new dsp::stream(blockSize * 2); + astr->stereoAudioStream = new dsp::stream; astr->audio->setBlockSize(blockSize); astr->audio->setStreamType(io::AudioSink::STEREO); astr->audio->setStereoInput(astr->stereoAudioStream); @@ -48,8 +48,8 @@ namespace audio { astr->sampleRate = sampleRate; astr->stereoStream = stream; astr->sampleRateChangeHandler = sampleRateChangeHandler; - astr->stereoDynSplit = new dsp::DynamicSplitter(stream, blockSize); - astr->stereoDynSplit->bind(astr->stereoAudioStream); + astr->stereoDynSplit = new dsp::Splitter(stream); + astr->stereoDynSplit->bindStream(astr->stereoAudioStream); astr->running = false; streams[name] = astr; astr->vfoName = vfoName; @@ -103,20 +103,20 @@ namespace audio { bstr.streamRemovedHandler = streamRemovedHandler; bstr.sampleRateChangeHandler = sampleRateChangeHandler; if (astr->type == STREAM_TYPE_MONO) { - bstr.monoStream = new dsp::stream(astr->blockSize * 2); + bstr.monoStream = new dsp::stream; astr->monoDynSplit->stop(); - astr->monoDynSplit->bind(bstr.monoStream); + astr->monoDynSplit->bindStream(bstr.monoStream); if (astr->running) { astr->monoDynSplit->start(); } astr->boundStreams.push_back(bstr); return bstr.monoStream; } - bstr.stereoStream = new dsp::stream(astr->blockSize * 2); - bstr.s2m = new dsp::StereoToMono(bstr.stereoStream, astr->blockSize * 2); - bstr.monoStream = &bstr.s2m->output; + bstr.stereoStream = new dsp::stream; + bstr.s2m = new dsp::StereoToMono(bstr.stereoStream); + bstr.monoStream = &bstr.s2m->out; astr->stereoDynSplit->stop(); - astr->stereoDynSplit->bind(bstr.stereoStream); + astr->stereoDynSplit->bindStream(bstr.stereoStream); if (astr->running) { astr->stereoDynSplit->start(); } @@ -125,7 +125,7 @@ namespace audio { return bstr.monoStream; } - dsp::stream* bindToStreamStereo(std::string name, void (*streamRemovedHandler)(void* ctx), void (*sampleRateChangeHandler)(void* ctx, double sampleRate, int blockSize), void* ctx) { + dsp::stream* bindToStreamStereo(std::string name, void (*streamRemovedHandler)(void* ctx), void (*sampleRateChangeHandler)(void* ctx, double sampleRate, int blockSize), void* ctx) { AudioStream_t* astr = streams[name]; BoundStream_t bstr; bstr.type = STREAM_TYPE_STEREO; @@ -133,20 +133,20 @@ namespace audio { bstr.streamRemovedHandler = streamRemovedHandler; bstr.sampleRateChangeHandler = sampleRateChangeHandler; if (astr->type == STREAM_TYPE_STEREO) { - bstr.stereoStream = new dsp::stream(astr->blockSize * 2); + bstr.stereoStream = new dsp::stream; astr->stereoDynSplit->stop(); - astr->stereoDynSplit->bind(bstr.stereoStream); + astr->stereoDynSplit->bindStream(bstr.stereoStream); if (astr->running) { astr->stereoDynSplit->start(); } astr->boundStreams.push_back(bstr); return bstr.stereoStream; } - bstr.monoStream = new dsp::stream(astr->blockSize * 2); - bstr.m2s = new dsp::MonoToStereo(bstr.monoStream, astr->blockSize * 2); - bstr.stereoStream = &bstr.m2s->output; + bstr.monoStream = new dsp::stream; + bstr.m2s = new dsp::MonoToStereo(bstr.monoStream); + bstr.stereoStream = &bstr.m2s->out; astr->monoDynSplit->stop(); - astr->monoDynSplit->bind(bstr.monoStream); + astr->monoDynSplit->bindStream(bstr.monoStream); if (astr->running) { astr->monoDynSplit->start(); } @@ -156,35 +156,37 @@ namespace audio { } void setBlockSize(std::string name, int blockSize) { - AudioStream_t* astr = streams[name]; - if (astr->running) { - return; - } - if (astr->type == STREAM_TYPE_MONO) { - astr->monoDynSplit->setBlockSize(blockSize); - for (int i = 0; i < astr->boundStreams.size(); i++) { - BoundStream_t bstr = astr->boundStreams[i]; - bstr.monoStream->setMaxLatency(blockSize * 2); - if (bstr.type == STREAM_TYPE_STEREO) { - bstr.m2s->stop(); - bstr.m2s->setBlockSize(blockSize); - bstr.m2s->start(); - } - } - astr->blockSize = blockSize; - return; - } - astr->monoDynSplit->setBlockSize(blockSize); - for (int i = 0; i < astr->boundStreams.size(); i++) { - BoundStream_t bstr = astr->boundStreams[i]; - bstr.stereoStream->setMaxLatency(blockSize * 2); - if (bstr.type == STREAM_TYPE_MONO) { - bstr.s2m->stop(); - bstr.s2m->setBlockSize(blockSize); - bstr.s2m->start(); - } - } - astr->blockSize = blockSize; + // NOTE: THIS SHOULD NOT BE NEEDED ANYMORE + + // AudioStream_t* astr = streams[name]; + // if (astr->running) { + // return; + // } + // if (astr->type == STREAM_TYPE_MONO) { + // astr->monoDynSplit->setBlockSize(blockSize); + // for (int i = 0; i < astr->boundStreams.size(); i++) { + // BoundStream_t bstr = astr->boundStreams[i]; + // bstr.monoStream->setMaxLatency(blockSize * 2); + // if (bstr.type == STREAM_TYPE_STEREO) { + // bstr.m2s->stop(); + // bstr.m2s->setBlockSize(blockSize); + // bstr.m2s->start(); + // } + // } + // astr->blockSize = blockSize; + // return; + // } + // astr->monoDynSplit->setBlockSize(blockSize); + // for (int i = 0; i < astr->boundStreams.size(); i++) { + // BoundStream_t bstr = astr->boundStreams[i]; + // bstr.stereoStream->setMaxLatency(blockSize * 2); + // if (bstr.type == STREAM_TYPE_MONO) { + // bstr.s2m->stop(); + // bstr.s2m->setBlockSize(blockSize); + // bstr.s2m->start(); + // } + // } + // astr->blockSize = blockSize; } void unbindFromStreamMono(std::string name, dsp::stream* stream) { @@ -196,7 +198,7 @@ namespace audio { } if (astr->type == STREAM_TYPE_STEREO) { astr->stereoDynSplit->stop(); - astr->stereoDynSplit->unbind(bstr.stereoStream); + astr->stereoDynSplit->unbindStream(bstr.stereoStream); if (astr->running) { astr->stereoDynSplit->start(); } @@ -205,7 +207,7 @@ namespace audio { return; } astr->monoDynSplit->stop(); - astr->monoDynSplit->unbind(bstr.monoStream); + astr->monoDynSplit->unbindStream(bstr.monoStream); if (astr->running) { astr->monoDynSplit->start(); } @@ -214,7 +216,7 @@ namespace audio { } } - void unbindFromStreamStereo(std::string name, dsp::stream* stream) { + void unbindFromStreamStereo(std::string name, dsp::stream* stream) { AudioStream_t* astr = streams[name]; for (int i = 0; i < astr->boundStreams.size(); i++) { BoundStream_t bstr = astr->boundStreams[i]; @@ -223,7 +225,7 @@ namespace audio { } if (astr->type == STREAM_TYPE_MONO) { astr->monoDynSplit->stop(); - astr->monoDynSplit->unbind(bstr.monoStream); + astr->monoDynSplit->unbindStream(bstr.monoStream); if (astr->running) { astr->monoDynSplit->start(); } @@ -232,7 +234,7 @@ namespace audio { return; } astr->stereoDynSplit->stop(); - astr->stereoDynSplit->unbind(bstr.stereoStream); + astr->stereoDynSplit->unbindStream(bstr.stereoStream); if (astr->running) { astr->stereoDynSplit->start(); } @@ -255,16 +257,19 @@ namespace audio { if (astr->running) { return; } + + // NOTE: All the blocksize stuff needs removal + int blockSize = astr->sampleRateChangeHandler(astr->ctx, sampleRate); astr->audio->setSampleRate(sampleRate); - astr->audio->setBlockSize(blockSize); + //astr->audio->setBlockSize(blockSize); if (astr->type == STREAM_TYPE_MONO) { - astr->monoDynSplit->setBlockSize(blockSize); + //astr->monoDynSplit->setBlockSize(blockSize); for (int i = 0; i < astr->boundStreams.size(); i++) { BoundStream_t bstr = astr->boundStreams[i]; if (bstr.type == STREAM_TYPE_STEREO) { bstr.m2s->stop(); - bstr.m2s->setBlockSize(blockSize); + //bstr.m2s->setBlockSize(blockSize); bstr.sampleRateChangeHandler(bstr.ctx, sampleRate, blockSize); bstr.m2s->start(); continue; @@ -273,12 +278,12 @@ namespace audio { } } else { - astr->stereoDynSplit->setBlockSize(blockSize); + //astr->stereoDynSplit->setBlockSize(blockSize); for (int i = 0; i < astr->boundStreams.size(); i++) { BoundStream_t bstr = astr->boundStreams[i]; if (bstr.type == STREAM_TYPE_MONO) { bstr.s2m->stop(); - bstr.s2m->setBlockSize(blockSize); + //bstr.s2m->setBlockSize(blockSize); bstr.sampleRateChangeHandler(bstr.ctx, sampleRate, blockSize); bstr.s2m->start(); continue; diff --git a/core/src/signal_path/audio.h b/core/src/signal_path/audio.h index e5d0010..5f5d13c 100644 --- a/core/src/signal_path/audio.h +++ b/core/src/signal_path/audio.h @@ -1,6 +1,7 @@ #pragma once #include #include +#include #include #include @@ -15,7 +16,7 @@ namespace audio { struct BoundStream_t { dsp::stream* monoStream; - dsp::stream* stereoStream; + dsp::stream* stereoStream; dsp::StereoToMono* s2m; dsp::MonoToStereo* m2s; void (*streamRemovedHandler)(void* ctx); @@ -27,12 +28,12 @@ namespace audio { struct AudioStream_t { io::AudioSink* audio; dsp::stream* monoAudioStream; - dsp::stream* stereoAudioStream; + dsp::stream* stereoAudioStream; std::vector boundStreams; dsp::stream* monoStream; - dsp::DynamicSplitter* monoDynSplit; - dsp::stream* stereoStream; - dsp::DynamicSplitter* stereoDynSplit; + dsp::Splitter* monoDynSplit; + dsp::stream* stereoStream; + dsp::Splitter* stereoDynSplit; int (*sampleRateChangeHandler)(void* ctx, double sampleRate); double sampleRate; int blockSize; @@ -48,15 +49,15 @@ namespace audio { extern std::map streams; double registerMonoStream(dsp::stream* stream, std::string name, std::string vfoName, int (*sampleRateChangeHandler)(void* ctx, double sampleRate), void* ctx); - double registerStereoStream(dsp::stream* stream, std::string name, std::string vfoName, int (*sampleRateChangeHandler)(void* ctx, double sampleRate), void* ctx); + double registerStereoStream(dsp::stream* stream, std::string name, std::string vfoName, int (*sampleRateChangeHandler)(void* ctx, double sampleRate), void* ctx); void startStream(std::string name); void stopStream(std::string name); void removeStream(std::string name); dsp::stream* bindToStreamMono(std::string name, void (*streamRemovedHandler)(void* ctx), void (*sampleRateChangeHandler)(void* ctx, double sampleRate, int blockSize), void* ctx); - dsp::stream* bindToStreamStereo(std::string name, void (*streamRemovedHandler)(void* ctx), void (*sampleRateChangeHandler)(void* ctx, double sampleRate, int blockSize), void* ctx); + dsp::stream* bindToStreamStereo(std::string name, void (*streamRemovedHandler)(void* ctx), void (*sampleRateChangeHandler)(void* ctx, double sampleRate, int blockSize), void* ctx); void setBlockSize(std::string name, int blockSize); void unbindFromStreamMono(std::string name, dsp::stream* stream); - void unbindFromStreamStereo(std::string name, dsp::stream* stream); + void unbindFromStreamStereo(std::string name, dsp::stream* stream); std::string getNameFromVFO(std::string vfoName); void setSampleRate(std::string name, double sampleRate); void setAudioDevice(std::string name, int deviceId, double sampleRate); diff --git a/core/src/signal_path/dsp.cpp b/core/src/signal_path/dsp.cpp index 62a5c49..cc6f52f 100644 --- a/core/src/signal_path/dsp.cpp +++ b/core/src/signal_path/dsp.cpp @@ -4,54 +4,43 @@ SignalPath::SignalPath() { } -void SignalPath::init(uint64_t sampleRate, int fftRate, int fftSize, dsp::stream* input, dsp::complex_t* fftBuffer, void fftHandler(dsp::complex_t*)) { +void SignalPath::init(uint64_t sampleRate, int fftRate, int fftSize, dsp::stream* input, dsp::complex_t* fftBuffer, void fftHandler(dsp::complex_t*,int,void*)) { this->sampleRate = sampleRate; this->fftRate = fftRate; this->fftSize = fftSize; inputBlockSize = sampleRate / 200.0f; - dcBiasRemover.init(input, 32000); - dcBiasRemover.bypass = true; - split.init(&dcBiasRemover.output, 32000); + split.init(input); - fftBlockDec.init(&split.output_a, (sampleRate / fftRate) - fftSize, fftSize); - fftHandlerSink.init(&fftBlockDec.output, fftBuffer, fftSize, fftHandler); - - dynSplit.init(&split.output_b, 32000); + reshape.init(&fftStream); + fftHandlerSink.init(&reshape.out, fftHandler, NULL); } void SignalPath::setSampleRate(double sampleRate) { this->sampleRate = sampleRate; - inputBlockSize = sampleRate / 200.0f; - dcBiasRemover.stop(); split.stop(); - fftBlockDec.stop(); - fftHandlerSink.stop(); - dynSplit.stop(); + //fftBlockDec.stop(); + //fftHandlerSink.stop(); for (auto const& [name, vfo] : vfos) { vfo.vfo->stop(); } - dcBiasRemover.setBlockSize(inputBlockSize); - split.setBlockSize(inputBlockSize); - int skip = (sampleRate / fftRate) - fftSize; - fftBlockDec.setSkip(skip); - dynSplit.setBlockSize(inputBlockSize); + // Claculate skip to maintain a constant fft rate + //int skip = (sampleRate / fftRate) - fftSize; + //fftBlockDec.setSkip(skip); - // TODO: Tell modules that the block size has changed + // TODO: Tell modules that the block size has changed (maybe?) for (auto const& [name, vfo] : vfos) { - vfo.vfo->setInputSampleRate(sampleRate, inputBlockSize); + vfo.vfo->setInSampleRate(sampleRate); vfo.vfo->start(); } - fftHandlerSink.start(); - fftBlockDec.start(); + //fftHandlerSink.start(); + //fftBlockDec.start(); split.start(); - dcBiasRemover.start(); - dynSplit.start(); } double SignalPath::getSampleRate() { @@ -59,32 +48,23 @@ double SignalPath::getSampleRate() { } void SignalPath::start() { - dcBiasRemover.start(); split.start(); - - fftBlockDec.start(); + reshape.start(); fftHandlerSink.start(); - - dynSplit.start(); -} - -void SignalPath::setDCBiasCorrection(bool enabled) { - dcBiasRemover.bypass = !enabled; } dsp::VFO* SignalPath::addVFO(std::string name, double outSampleRate, double bandwidth, double offset) { if (vfos.find(name) != vfos.end()) { return NULL; } - dynSplit.stop(); + VFO_t vfo; - vfo.inputStream = new dsp::stream(inputBlockSize * 2); - dynSplit.bind(vfo.inputStream); + vfo.inputStream = new dsp::stream; + split.bindStream(vfo.inputStream); vfo.vfo = new dsp::VFO(); - vfo.vfo->init(vfo.inputStream, sampleRate, outSampleRate, bandwidth, offset, inputBlockSize); + vfo.vfo->init(vfo.inputStream, offset, sampleRate, outSampleRate, bandwidth); vfo.vfo->start(); vfos[name] = vfo; - dynSplit.start(); return vfo.vfo; } @@ -93,30 +73,22 @@ void SignalPath::removeVFO(std::string name) { return; } - dynSplit.stop(); VFO_t vfo = vfos[name]; vfo.vfo->stop(); - dynSplit.unbind(vfo.inputStream); + split.unbindStream(vfo.inputStream); delete vfo.vfo; delete vfo.inputStream; - dynSplit.start(); vfos.erase(name); } void SignalPath::setInput(dsp::stream* input) { - dcBiasRemover.stop(); - dcBiasRemover.setInput(input); - dcBiasRemover.start(); + split.setInput(input); } void SignalPath::bindIQStream(dsp::stream* stream) { - dynSplit.stop(); - dynSplit.bind(stream); - dynSplit.start(); + split.bindStream(stream); } void SignalPath::unbindIQStream(dsp::stream* stream) { - dynSplit.stop(); - dynSplit.unbind(stream); - dynSplit.start(); + split.unbindStream(stream); } \ No newline at end of file diff --git a/core/src/signal_path/dsp.h b/core/src/signal_path/dsp.h index 80c1473..e41860a 100644 --- a/core/src/signal_path/dsp.h +++ b/core/src/signal_path/dsp.h @@ -6,7 +6,6 @@ #include #include #include -#include #include #include #include @@ -14,10 +13,9 @@ class SignalPath { public: SignalPath(); - void init(uint64_t sampleRate, int fftRate, int fftSize, dsp::stream* input, dsp::complex_t* fftBuffer, void fftHandler(dsp::complex_t*)); + void init(uint64_t sampleRate, int fftRate, int fftSize, dsp::stream* input, dsp::complex_t* fftBuffer, void fftHandler(dsp::complex_t*,int,void*)); void start(); void setSampleRate(double sampleRate); - void setDCBiasCorrection(bool enabled); void setFFTRate(double rate); double getSampleRate(); dsp::VFO* addVFO(std::string name, double outSampleRate, double bandwidth, double offset); @@ -32,15 +30,14 @@ private: dsp::VFO* vfo; }; - dsp::DCBiasRemover dcBiasRemover; - dsp::Splitter split; + dsp::Splitter split; // FFT - dsp::BlockDecimator fftBlockDec; - dsp::HandlerSink fftHandlerSink; + dsp::stream fftStream; + dsp::Reshaper reshape; + dsp::HandlerSink fftHandlerSink; // VFO - dsp::DynamicSplitter dynSplit; std::map vfos; double sampleRate; diff --git a/core/src/signal_path/vfo_manager.cpp b/core/src/signal_path/vfo_manager.cpp index 8579ffd..019a309 100644 --- a/core/src/signal_path/vfo_manager.cpp +++ b/core/src/signal_path/vfo_manager.cpp @@ -8,7 +8,7 @@ VFOManager::VFO::VFO(std::string name, int reference, double offset, double band wtfVFO->setReference(reference); wtfVFO->setBandwidth(bandwidth); wtfVFO->setOffset(offset); - output = dspVFO->output; + output = dspVFO->out; gui::waterfall.vfos[name] = wtfVFO; } @@ -34,7 +34,7 @@ void VFOManager::VFO::setBandwidth(double bandwidth) { } void VFOManager::VFO::setSampleRate(double sampleRate, double bandwidth) { - dspVFO->setOutputSampleRate(sampleRate, bandwidth); + dspVFO->setOutSampleRate(sampleRate, bandwidth); wtfVFO->setBandwidth(bandwidth); } @@ -43,7 +43,8 @@ void VFOManager::VFO::setReference(int ref) { } int VFOManager::VFO::getOutputBlockSize() { - return dspVFO->getOutputBlockSize(); + // NOTE: This shouldn't be needed anymore + return 1; //dspVFO->getOutputBlockSize(); } void VFOManager::VFO::setSnapInterval(double interval) { diff --git a/file_source/src/main.cpp b/file_source/src/main.cpp index 7eafb9e..98c0382 100644 --- a/file_source/src/main.cpp +++ b/file_source/src/main.cpp @@ -20,8 +20,6 @@ public: FileSourceModule(std::string name) { this->name = name; - stream.init(100); - handler.ctx = this; handler.selectHandler = menuSelected; handler.deselectHandler = menuDeselected; @@ -84,22 +82,19 @@ private: double sampleRate = _this->reader->getSampleRate(); int blockSize = sampleRate / 200.0; int16_t* inBuf = new int16_t[blockSize * 2]; - dsp::complex_t* outBuf = new dsp::complex_t[blockSize]; - - _this->stream.setMaxLatency(blockSize * 2); while (true) { _this->reader->readSamples(inBuf, blockSize * 2 * sizeof(int16_t)); + if (_this->stream.aquire() < 0) { break; }; for (int i = 0; i < blockSize; i++) { - outBuf[i].q = (float)inBuf[i * 2] / (float)0x7FFF; - outBuf[i].i = (float)inBuf[(i * 2) + 1] / (float)0x7FFF; + _this->stream.data[i].q = (float)inBuf[i * 2] / (float)0x7FFF; + _this->stream.data[i].i = (float)inBuf[(i * 2) + 1] / (float)0x7FFF; } - if (_this->stream.write(outBuf, blockSize) < 0) { break; }; + _this->stream.write(blockSize); //std::this_thread::sleep_for(std::chrono::milliseconds(5)); } delete[] inBuf; - delete[] outBuf; } std::string name; diff --git a/radio/src/main.cpp b/radio/src/main.cpp index 0967c71..5dd02a6 100644 --- a/radio/src/main.cpp +++ b/radio/src/main.cpp @@ -122,11 +122,6 @@ private: _this->sigPath.setBandwidth(_this->bandWidth); } - ImGui::Text("Squelch"); - ImGui::SameLine(); - ImGui::SetNextItemWidth(menuColumnWidth - ImGui::GetCursorPosX()); - ImGui::SliderFloat(CONCAT("##_squelch_select_", _this->name), &_this->sigPath.squelch.level, -100, 0); - ImGui::PopItemWidth(); ImGui::Text("Snap Interval"); diff --git a/radio/src/path.cpp b/radio/src/path.cpp index cc49403..1faad6f 100644 --- a/radio/src/path.cpp +++ b/radio/src/path.cpp @@ -11,12 +11,17 @@ int SigPath::sampleRateChangeHandler(void* ctx, double sampleRate) { _this->audioResamp.stop(); _this->deemp.stop(); float bw = std::min(_this->bandwidth, sampleRate / 2.0f); - _this->audioResamp.setOutputSampleRate(sampleRate, bw, bw); - _this->deemp.setBlockSize(_this->audioResamp.getOutputBlockSize()); - _this->deemp.setSamplerate(sampleRate); + + + _this->audioResamp.setOutSampleRate(sampleRate); + _this->audioWin.setSampleRate(_this->sampleRate * _this->audioResamp.getInterpolation()); + _this->audioResamp.updateWindow(&_this->audioWin); + + _this->deemp.setSampleRate(sampleRate); _this->audioResamp.start(); _this->deemp.start(); - return _this->audioResamp.getOutputBlockSize(); + // Note: returning a block size should not be needed anymore + return 1; } void SigPath::init(std::string vfoName, uint64_t sampleRate, int blockSize) { @@ -35,21 +40,18 @@ void SigPath::init(std::string vfoName, uint64_t sampleRate, int blockSize) { // TODO: ajust deemphasis for different output sample rates // TODO: Add a mono to stereo for different modes - squelch.init(vfo->output, 800); - squelch.level = -100.0; - squelch.onCount = 1; - squelch.offCount = 2560; + demod.init(vfo->output, 200000, 100000); + amDemod.init(vfo->output); + ssbDemod.init(vfo->output, 6000, 3000, dsp::SSBDemod::MODE_USB); - demod.init(squelch.out[0], 100000, 200000, 800); - amDemod.init(squelch.out[0], 50); - ssbDemod.init(squelch.out[0], 6000, 3000, 22); - cpx2stereo.init(squelch.out[0], 22); + audioWin.init(24000, 24000, 200000); + audioResamp.init(&demod.out, &audioWin, 200000, 48000); + audioWin.setSampleRate(audioResamp.getInterpolation() * 200000); + audioResamp.updateWindow(&audioWin); + + deemp.init(&audioResamp.out, 48000, 50e-6); - audioResamp.init(&demod.output, 200000, 48000, 800); - deemp.init(&audioResamp.output, 800, 50e-6, 48000); - - outputSampleRate = audio::registerMonoStream(&deemp.output, vfoName, vfoName, sampleRateChangeHandler, this); - audio::setBlockSize(vfoName, audioResamp.getOutputBlockSize()); + outputSampleRate = audio::registerMonoStream(&deemp.out, vfoName, vfoName, sampleRateChangeHandler, this); setDemodulator(_demod, bandwidth); } @@ -90,27 +92,27 @@ void SigPath::setDemodulator(int demId, float bandWidth) { else if (_demod == DEMOD_DSB) { ssbDemod.stop(); } - else if (_demod == DEMOD_RAW) { - cpx2stereo.stop(); - } else { spdlog::error("UNIMPLEMENTED DEMODULATOR IN SigPath::setDemodulator (stop)"); } _demod = demId; - - squelch.stop(); // Set input of the audio resampler // TODO: Set bandwidth from argument if (demId == DEMOD_FM) { demodOutputSamplerate = 200000; vfo->setSampleRate(200000, bandwidth); - demod.setBlockSize(vfo->getOutputBlockSize()); demod.setSampleRate(200000); demod.setDeviation(bandwidth / 2.0f); - audioResamp.setInput(&demod.output); + audioResamp.setInput(&demod.out); audioBw = std::min(bandwidth, outputSampleRate / 2.0f); - audioResamp.setInputSampleRate(200000, vfo->getOutputBlockSize(), audioBw, audioBw); + + audioResamp.setInSampleRate(200000); + audioWin.setSampleRate(200000 * audioResamp.getInterpolation()); + audioWin.setCutoff(audioBw); + audioWin.setTransWidth(audioBw); + audioResamp.updateWindow(&audioWin); + deemp.bypass = (_deemp == DEEMP_NONE); vfo->setReference(ImGui::WaterfallVFO::REF_CENTER); demod.start(); @@ -118,12 +120,17 @@ void SigPath::setDemodulator(int demId, float bandWidth) { else if (demId == DEMOD_NFM) { demodOutputSamplerate = 16000; vfo->setSampleRate(16000, bandwidth); - demod.setBlockSize(vfo->getOutputBlockSize()); demod.setSampleRate(16000); demod.setDeviation(bandwidth / 2.0f); - audioResamp.setInput(&demod.output); + audioResamp.setInput(&demod.out); audioBw = std::min(bandwidth, outputSampleRate / 2.0f); - audioResamp.setInputSampleRate(16000, vfo->getOutputBlockSize(), audioBw, audioBw); + + audioResamp.setInSampleRate(16000); + audioWin.setSampleRate(16000 * audioResamp.getInterpolation()); + audioWin.setCutoff(audioBw); + audioWin.setTransWidth(audioBw); + audioResamp.updateWindow(&audioWin); + deemp.bypass = true; vfo->setReference(ImGui::WaterfallVFO::REF_CENTER); demod.start(); @@ -131,10 +138,15 @@ void SigPath::setDemodulator(int demId, float bandWidth) { else if (demId == DEMOD_AM) { demodOutputSamplerate = 125000; vfo->setSampleRate(12500, bandwidth); - amDemod.setBlockSize(vfo->getOutputBlockSize()); - audioResamp.setInput(&amDemod.output); + audioResamp.setInput(&amDemod.out); audioBw = std::min(bandwidth, outputSampleRate / 2.0f); - audioResamp.setInputSampleRate(12500, vfo->getOutputBlockSize(), audioBw, audioBw); + + audioResamp.setInSampleRate(12500); + audioWin.setSampleRate(12500 * audioResamp.getInterpolation()); + audioWin.setCutoff(audioBw); + audioWin.setTransWidth(audioBw); + audioResamp.updateWindow(&audioWin); + deemp.bypass = true; vfo->setReference(ImGui::WaterfallVFO::REF_CENTER); amDemod.start(); @@ -142,11 +154,16 @@ void SigPath::setDemodulator(int demId, float bandWidth) { else if (demId == DEMOD_USB) { demodOutputSamplerate = 6000; vfo->setSampleRate(6000, bandwidth); - ssbDemod.setBlockSize(vfo->getOutputBlockSize()); ssbDemod.setMode(dsp::SSBDemod::MODE_USB); - audioResamp.setInput(&ssbDemod.output); + audioResamp.setInput(&ssbDemod.out); audioBw = std::min(bandwidth, outputSampleRate / 2.0f); - audioResamp.setInputSampleRate(6000, vfo->getOutputBlockSize(), audioBw, audioBw); + + audioResamp.setInSampleRate(6000); + audioWin.setSampleRate(6000 * audioResamp.getInterpolation()); + audioWin.setCutoff(audioBw); + audioWin.setTransWidth(audioBw); + audioResamp.updateWindow(&audioWin); + deemp.bypass = true; vfo->setReference(ImGui::WaterfallVFO::REF_LOWER); ssbDemod.start(); @@ -154,11 +171,16 @@ void SigPath::setDemodulator(int demId, float bandWidth) { else if (demId == DEMOD_LSB) { demodOutputSamplerate = 6000; vfo->setSampleRate(6000, bandwidth); - ssbDemod.setBlockSize(vfo->getOutputBlockSize()); ssbDemod.setMode(dsp::SSBDemod::MODE_LSB); - audioResamp.setInput(&ssbDemod.output); + audioResamp.setInput(&ssbDemod.out); audioBw = std::min(bandwidth, outputSampleRate / 2.0f); - audioResamp.setInputSampleRate(6000, vfo->getOutputBlockSize(), audioBw, audioBw); + + audioResamp.setInSampleRate(6000); + audioWin.setSampleRate(6000 * audioResamp.getInterpolation()); + audioWin.setCutoff(audioBw); + audioWin.setTransWidth(audioBw); + audioResamp.updateWindow(&audioWin); + deemp.bypass = true; vfo->setReference(ImGui::WaterfallVFO::REF_UPPER); ssbDemod.start(); @@ -166,34 +188,24 @@ void SigPath::setDemodulator(int demId, float bandWidth) { else if (demId == DEMOD_DSB) { demodOutputSamplerate = 6000; vfo->setSampleRate(6000, bandwidth); - ssbDemod.setBlockSize(vfo->getOutputBlockSize()); ssbDemod.setMode(dsp::SSBDemod::MODE_DSB); - audioResamp.setInput(&ssbDemod.output); + audioResamp.setInput(&ssbDemod.out); audioBw = std::min(bandwidth, outputSampleRate / 2.0f); - audioResamp.setInputSampleRate(6000, vfo->getOutputBlockSize(), audioBw, audioBw); + + audioResamp.setInSampleRate(6000); + audioWin.setSampleRate(6000 * audioResamp.getInterpolation()); + audioWin.setCutoff(audioBw); + audioWin.setTransWidth(audioBw); + audioResamp.updateWindow(&audioWin); + deemp.bypass = true; vfo->setReference(ImGui::WaterfallVFO::REF_CENTER); ssbDemod.start(); } - else if (demId == DEMOD_RAW) { - demodOutputSamplerate = 10000; - vfo->setSampleRate(10000, bandwidth); - cpx2stereo.setBlockSize(vfo->getOutputBlockSize()); - //audioResamp.setInput(&cpx2stereo.output); - audioBw = std::min(bandwidth, outputSampleRate / 2.0f); - audioResamp.setInputSampleRate(10000, vfo->getOutputBlockSize(), audioBw, audioBw); - vfo->setReference(ImGui::WaterfallVFO::REF_LOWER); - cpx2stereo.start(); - } else { spdlog::error("UNIMPLEMENTED DEMODULATOR IN SigPath::setDemodulator (start): {0}", demId); } - squelch.setBlockSize(vfo->getOutputBlockSize()); - squelch.start(); - - deemp.setBlockSize(audioResamp.getOutputBlockSize()); - audioResamp.start(); deemp.start(); } @@ -237,17 +249,17 @@ void SigPath::setBandwidth(float bandWidth) { } else if (_demod == DEMOD_USB) { ssbDemod.stop(); - ssbDemod.setBandwidth(bandwidth); + ssbDemod.setBandWidth(bandwidth); ssbDemod.start(); } else if (_demod == DEMOD_LSB) { ssbDemod.stop(); - ssbDemod.setBandwidth(bandwidth); + ssbDemod.setBandWidth(bandwidth); ssbDemod.start(); } else if (_demod == DEMOD_DSB) { ssbDemod.stop(); - ssbDemod.setBandwidth(bandwidth); + ssbDemod.setBandWidth(bandwidth); ssbDemod.start(); } else if (_demod == DEMOD_RAW) { @@ -260,15 +272,16 @@ void SigPath::setBandwidth(float bandWidth) { if (audioBw != _audioBw) { audioBw = _audioBw; audioResamp.stop(); - audioResamp.setFilterParams(audioBw, audioBw); - audioResamp.setBlockSize(vfo->getOutputBlockSize()); - //audioResamp.setInputSampleRate(demodOutputSamplerate, vfo->getOutputBlockSize(), audioBw, audioBw); + + audioWin.setCutoff(audioBw); + audioWin.setTransWidth(audioBw); + audioResamp.updateWindow(&audioWin); + audioResamp.start(); } } void SigPath::start() { - squelch.start(); demod.start(); audioResamp.start(); deemp.start(); diff --git a/radio/src/path.h b/radio/src/path.h index 7d5318d..21e6a25 100644 --- a/radio/src/path.h +++ b/radio/src/path.h @@ -6,9 +6,10 @@ #include #include #include -#include #include #include +#include +#include #include #include #include @@ -44,28 +45,23 @@ public: }; - dsp::FMDeemphasis deemp; - dsp::Squelch squelch; + dsp::BFMDeemp deemp; VFOManager::VFO* vfo; private: static int sampleRateChangeHandler(void* ctx, double sampleRate); - - dsp::stream input; - - // Demodulators - dsp::FMDemodulator demod; - dsp::AMDemodulator amDemod; + dsp::FMDemod demod; + dsp::AMDemod amDemod; dsp::SSBDemod ssbDemod; - dsp::ComplexToStereo cpx2stereo; // Audio output dsp::MonoToStereo m2s; - dsp::FIRResampler audioResamp; + dsp::filter_window::BlackmanWindow audioWin; + dsp::PolyphaseResampler audioResamp; std::string vfoName; diff --git a/recorder/src/main.cpp b/recorder/src/main.cpp index 73553fc..5936db5 100644 --- a/recorder/src/main.cpp +++ b/recorder/src/main.cpp @@ -104,8 +104,7 @@ private: _this->samplesWritten = 0; _this->sampleRate = sigpath::signalPath.getSampleRate(); _this->writer = new WavWriter(ROOT_DIR "/recordings/" + genFileName("baseband_"), 16, 2, _this->sampleRate); - _this->iqStream = new dsp::stream(); - _this->iqStream->init(_this->sampleRate / 200.0); + _this->iqStream = new dsp::stream; sigpath::signalPath.bindIQStream(_this->iqStream); _this->workerThread = std::thread(_iqWriteWorker, _this); _this->recording = true; @@ -176,20 +175,18 @@ private: } static void _audioWriteWorker(RecorderModule* _this) { - dsp::StereoFloat_t* floatBuf = new dsp::StereoFloat_t[1024]; - int16_t* sampleBuf = new int16_t[2048]; + int16_t* sampleBuf = new int16_t[STREAM_BUFFER_SIZE * 2]; while (true) { - if (_this->audioStream->read(floatBuf, 1024) < 0) { - break; - } + int count = _this->audioStream->read(); + if (count < 0) { break; } for (int i = 0; i < 1024; i++) { - sampleBuf[(i * 2) + 0] = floatBuf[i].l * 0x7FFF; - sampleBuf[(i * 2) + 1] = floatBuf[i].r * 0x7FFF; + sampleBuf[(i * 2) + 0] = _this->audioStream->data[i].l * 0x7FFF; + sampleBuf[(i * 2) + 1] = _this->audioStream->data[i].r * 0x7FFF; } + _this->audioStream->flush(); _this->samplesWritten += 1024; _this->writer->writeSamples(sampleBuf, 2048 * sizeof(int16_t)); } - delete[] floatBuf; delete[] sampleBuf; } @@ -197,13 +194,13 @@ private: dsp::complex_t* iqBuf = new dsp::complex_t[1024]; int16_t* sampleBuf = new int16_t[2048]; while (true) { - if (_this->iqStream->read(iqBuf, 1024) < 0) { - break; - } + int count = _this->iqStream->read(); + if (count < 0) { break; } for (int i = 0; i < 1024; i++) { - sampleBuf[(i * 2) + 0] = iqBuf[i].q * 0x7FFF; - sampleBuf[(i * 2) + 1] = iqBuf[i].i * 0x7FFF; + sampleBuf[(i * 2) + 0] = _this->iqStream->data[i].q * 0x7FFF; + sampleBuf[(i * 2) + 1] = _this->iqStream->data[i].i * 0x7FFF; } + _this->iqStream->flush(); _this->samplesWritten += 1024; _this->writer->writeSamples(sampleBuf, 2048 * sizeof(int16_t)); } @@ -212,7 +209,7 @@ private: } std::string name; - dsp::stream* audioStream; + dsp::stream* audioStream; dsp::stream* iqStream; WavWriter* writer; std::thread workerThread; diff --git a/root_dev/config.json b/root_dev/config.json index 4e00233..703ab60 100644 --- a/root_dev/config.json +++ b/root_dev/config.json @@ -3,7 +3,7 @@ "Radio": { "device": "default", "sampleRate": 48000.0, - "volume": 0.4407407343387604 + "volume": 0.0 }, "Radio 1": { "device": "Speakers (Realtek High Definition Audio)", @@ -19,9 +19,9 @@ "bandPlan": "General", "bandPlanEnabled": true, "fftHeight": 298, - "frequency": 4620379, + "frequency": 99000000, "max": 0.0, - "maximized": true, + "maximized": false, "menuOrder": [ "Source", "Radio", diff --git a/root_dev/module_list.json b/root_dev/module_list.json index b93a9b4..3c037b7 100644 --- a/root_dev/module_list.json +++ b/root_dev/module_list.json @@ -1,6 +1,6 @@ { - "Radio": "./radio/Release/radio.dll", - "Recorder": "./recorder/Release/recorder.dll", - "Soapy": "./soapy/Release/soapy.dll", - "RTLTCPSource": "./rtl_tcp_source/Release/rtl_tcp_source.dll" + "Radio": "./radio/RelWithDebInfo/radio.dll", + "Recorder": "./recorder/RelWithDebInfo/recorder.dll", + "Soapy": "./soapy/RelWithDebInfo/soapy.dll", + "RTLTCPSource": "./rtl_tcp_source/RelWithDebInfo/rtl_tcp_source.dll" } diff --git a/root_dev/soapy_source_config.json b/root_dev/soapy_source_config.json index 757b6a3..0dde10e 100644 --- a/root_dev/soapy_source_config.json +++ b/root_dev/soapy_source_config.json @@ -1,29 +1,35 @@ -{ - "device": "HackRF One #0 901868dc282c8f8b", - "devices": { - "AirSpy HF+ [c852435de0224af7]": { - "gains": { - "LNA": 6.0, - "RF": 0.0 - }, - "sampleRate": 768000.0 - }, - "Generic RTL2832U OEM :: 00000001": { - "gains": { - "TUNER": 49.599998474121094 - }, - "sampleRate": 2560000.0 - }, - "HackRF One #0 901868dc282c8f8b": { - "gains": { - "AMP": 13.86299991607666, - "LNA": 24.711999893188477, - "VGA": 14.282999992370605 - }, - "sampleRate": 8000000.0 - }, - "PulseAudio": { - "sampleRate": 96000.0 - } - } +{ + "device": "Generic RTL2832U OEM :: 00000001", + "devices": { + "AirSpy HF+ [c852435de0224af7]": { + "gains": { + "LNA": 6.0, + "RF": 0.0 + }, + "sampleRate": 768000.0 + }, + "Default Device": { + "sampleRate": 32000.0 + }, + "Generic RTL2832U OEM :: 00000001": { + "gains": { + "TUNER": 49.599998474121094 + }, + "sampleRate": 2560000.0 + }, + "HackRF One #0 901868dc282c8f8b": { + "gains": { + "AMP": 13.86299991607666, + "LNA": 24.711999893188477, + "VGA": 14.282999992370605 + }, + "sampleRate": 8000000.0 + }, + "Microphone (Realtek High Definition Audio)": { + "sampleRate": 96000.0 + }, + "PulseAudio": { + "sampleRate": 96000.0 + } + } } \ No newline at end of file diff --git a/rtl_tcp_source/src/main.cpp b/rtl_tcp_source/src/main.cpp index e247afb..2bc0323 100644 --- a/rtl_tcp_source/src/main.cpp +++ b/rtl_tcp_source/src/main.cpp @@ -21,8 +21,6 @@ public: RTLTCPSourceModule(std::string name) { this->name = name; - stream.init(100); - sampleRate = 2560000.0; handler.ctx = this; @@ -143,22 +141,19 @@ private: RTLTCPSourceModule* _this = (RTLTCPSourceModule*)ctx; int blockSize = _this->sampleRate / 200.0; uint8_t* inBuf = new uint8_t[blockSize * 2]; - dsp::complex_t* outBuf = new dsp::complex_t[blockSize]; - - _this->stream.setMaxLatency(blockSize * 2); while (true) { // Read samples here _this->client.receiveData(inBuf, blockSize * 2); + if (_this->stream.aquire() < 0) { break; } for (int i = 0; i < blockSize; i++) { - outBuf[i].q = ((double)inBuf[i * 2] - 128.0) / 128.0; - outBuf[i].i = ((double)inBuf[(i * 2) + 1] - 128.0) / 128.0; + _this->stream.data[i].q = ((double)inBuf[i * 2] - 128.0) / 128.0; + _this->stream.data[i].i = ((double)inBuf[(i * 2) + 1] - 128.0) / 128.0; } - if (_this->stream.write(outBuf, blockSize) < 0) { break; }; + _this->stream.write(blockSize); } delete[] inBuf; - delete[] outBuf; } std::string name; diff --git a/soapy/src/main.cpp b/soapy/src/main.cpp index e461097..81ded3b 100644 --- a/soapy/src/main.cpp +++ b/soapy/src/main.cpp @@ -31,8 +31,6 @@ public: refresh(); - stream.init(100); - // Select default device config.aquire(); std::string devName = config.conf["device"]; @@ -208,7 +206,9 @@ private: _this->running = false; _this->dev->deactivateStream(_this->devStream); _this->dev->closeStream(_this->devStream); + _this->stream.stopWriter(); _this->workerThread.join(); + _this->stream.clearWriteStop(); SoapySDR::Device::unmake(_this->dev); spdlog::info("SoapyModule '{0}': Stop!", _this->name); @@ -289,18 +289,17 @@ private: static void _worker(SoapyModule* _this) { spdlog::info("SOAPY: WORKER STARTED {0}", _this->sampleRate); int blockSize = _this->sampleRate / 200.0f; - dsp::complex_t* buf = new dsp::complex_t[blockSize]; int flags = 0; long long timeMs = 0; while (_this->running) { - int res = _this->dev->readStream(_this->devStream, (void**)&buf, blockSize, flags, timeMs); + if (_this->stream.aquire() < 0) { break; } + int res = _this->dev->readStream(_this->devStream, (void**)&_this->stream.data, blockSize, flags, timeMs); if (res < 1) { continue; } - _this->stream.write(buf, res); + _this->stream.write(res); } - delete[] buf; } std::string name;