From 30f1b423a6b7f6ad1c4449bf5cd0ed4fe04508b8 Mon Sep 17 00:00:00 2001 From: Ryzerth Date: Thu, 9 Jul 2020 16:02:58 +0200 Subject: [PATCH] New DSP code --- CMakeLists.txt | 2 + src/dsp/filter.h | 15 +-- src/dsp/legacy_stream.h | 221 ++++++++++++++++++++++++++++++++++++++++ src/dsp/resampling.h | 19 +++- src/dsp/routing.h | 26 ++++- src/dsp/stream.h | 197 ++++++++++++++++++----------------- src/main.cpp | 2 +- src/main_window.cpp | 2 +- src/signal_path.cpp | 48 ++++----- 9 files changed, 399 insertions(+), 133 deletions(-) create mode 100644 src/dsp/legacy_stream.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 43ad028d..7cfd9d11 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.9) project(sdrpp) +set(CMAKE_BUILD_TYPE "RelWithDebInfo") + # Compiler config set(CMAKE_CXX_FLAGS "-O2 /std:c++17") diff --git a/src/dsp/filter.h b/src/dsp/filter.h index 575529a5..d8fb76d6 100644 --- a/src/dsp/filter.h +++ b/src/dsp/filter.h @@ -31,8 +31,8 @@ namespace dsp { } - DecimatingFIRFilter(stream* input, std::vector taps, int blockSize, float decim) : output(blockSize * 2) { - output.init(blockSize * 2); + DecimatingFIRFilter(stream* input, std::vector taps, int blockSize, float decim) { + output.init((blockSize * 2) / decim); _in = input; _blockSize = blockSize; _tapCount = taps.size(); @@ -54,7 +54,7 @@ namespace dsp { } void init(stream* input, std::vector& taps, int blockSize, float decim) { - output.init(blockSize * 2); + output.init((blockSize * 2) / decim); _in = input; _blockSize = blockSize; _tapCount = taps.size(); @@ -107,6 +107,8 @@ namespace dsp { delayBuf = new complex_t[_tapCount]; for (int i = 0; i < _tapCount; i++) { _taps[i] = taps[i]; + delayBuf[i].i = 0; + delayBuf[i].q = 0; } } @@ -201,8 +203,8 @@ namespace dsp { } - FloatDecimatingFIRFilter(stream* input, std::vector taps, int blockSize, float decim) : output(blockSize * 2) { - output.init(blockSize * 2); + FloatDecimatingFIRFilter(stream* input, std::vector taps, int blockSize, float decim) { + output.init((blockSize * 2) / decim); _in = input; _blockSize = blockSize; _tapCount = taps.size(); @@ -223,7 +225,7 @@ namespace dsp { } void init(stream* input, std::vector& taps, int blockSize, float decim) { - output.init(blockSize * 2); + output.init((blockSize * 2) / decim); _in = input; _blockSize = blockSize; _tapCount = taps.size(); @@ -274,6 +276,7 @@ namespace dsp { delayBuf = new float[_tapCount]; for (int i = 0; i < _tapCount; i++) { _taps[i] = taps[i]; + delayBuf[i] = 0; } } diff --git a/src/dsp/legacy_stream.h b/src/dsp/legacy_stream.h new file mode 100644 index 00000000..69d100bb --- /dev/null +++ b/src/dsp/legacy_stream.h @@ -0,0 +1,221 @@ +#pragma once +#include +#include +#include + +#define STREAM_BUF_SZ 1000000 + +namespace dsp { + template + class stream { + public: + stream() { + + } + + stream(int maxLatency) { + size = STREAM_BUF_SZ; + _buffer = new T[size]; + _stopReader = false; + _stopWriter = false; + this->maxLatency = maxLatency; + writec = 0; + readc = size - 1; + } + + void init(int maxLatency) { + size = STREAM_BUF_SZ; + _buffer = new T[size]; + _stopReader = false; + _stopWriter = false; + this->maxLatency = maxLatency; + writec = 0; + readc = size - 1; + } + + int read(T* data, int len) { + int dataRead = 0; + while (dataRead < len) { + int canRead = waitUntilReadable(); + if (canRead < 0) { + printf("Reader stopped\n"); + clearReadStop(); + return -1; + } + int toRead = std::min(canRead, len - dataRead); + + int len1 = (toRead >= (size - readc) ? (size - readc) : (toRead)); + + memcpy(&data[dataRead], &_buffer[readc], len1 * sizeof(T)); + if (len1 < toRead) { + memcpy(&data[dataRead + len1], _buffer, (toRead - len1) * sizeof(T)); + } + + dataRead += toRead; + readc_mtx.lock(); + readc = (readc + toRead) % size; + readc_mtx.unlock(); + canWriteVar.notify_one(); + } + return len; + } + + int readAndSkip(T* data, int len, int skip) { + int dataRead = 0; + while (dataRead < len) { + int canRead = waitUntilReadable(); + if (canRead < 0) { + printf("reader stopped (read and skip)\n"); + clearReadStop(); + return -1; + } + int toRead = std::min(canRead, len - dataRead); + + int len1 = (toRead >= (size - readc) ? (size - readc) : (toRead)); + memcpy(&data[dataRead], &_buffer[readc], len1 * sizeof(T)); + if (len1 < toRead) { + memcpy(&data[dataRead + len1], _buffer, (toRead - len1) * sizeof(T)); + } + + dataRead += toRead; + readc_mtx.lock(); + readc = (readc + toRead) % size; + readc_mtx.unlock(); + canWriteVar.notify_one(); + } + + // Skip + + dataRead = 0; + while (dataRead < skip) { + int canRead = waitUntilReadable(); + int toRead = std::min(canRead, skip - dataRead); + + dataRead += toRead; + readc_mtx.lock(); + readc = (readc + toRead) % size; + readc_mtx.unlock(); + canWriteVar.notify_one(); + } + return len; + } + + int waitUntilReadable() { + int canRead = readable(); + if (canRead > 0) { + return canRead; + } + std::unique_lock lck(writec_mtx); + canReadVar.wait(lck, [=](){ return ((this->readable(false) > 0) || this->getReadStop()); }); + if (this->getReadStop()) { + return -1; + } + return this->readable(false); + } + + int readable(bool lock = true) { + if (lock) { writec_mtx.lock(); } + int _wc = writec; + if (lock) { writec_mtx.unlock(); } + int readable = (_wc - readc) % this->size; + if (_wc < readc) { + readable = (this->size + readable); + } + return readable - 1; + } + + int write(T* data, int len) { + int dataWrite = 0; + while (dataWrite < len) { + int canWrite = waitUntilWriteable(); + if (canWrite < 0) { + printf("Writer stopped\n"); + clearWriteStop(); + return -1; + } + int toWrite = std::min(canWrite, len - dataWrite); + + int len1 = (toWrite >= (size - writec) ? (size - writec) : (toWrite)); + + memcpy(&_buffer[writec], &data[dataWrite], len1 * sizeof(T)); + if (len1 < toWrite) { + memcpy(_buffer, &data[dataWrite + len1], (toWrite - len1) * sizeof(T)); + } + + dataWrite += toWrite; + writec_mtx.lock(); + writec = (writec + toWrite) % size; + writec_mtx.unlock(); + canReadVar.notify_one(); + } + return len; + } + + int waitUntilWriteable() { + int canWrite = writeable(); + if (canWrite > 0) { + return canWrite; + } + std::unique_lock lck(readc_mtx); + canWriteVar.wait(lck, [=](){ return ((this->writeable(false) > 0) || this->getWriteStop()); }); + if (this->getWriteStop()) { + return -1; + } + return this->writeable(false); + } + + int writeable(bool lock = true) { + if (lock) { readc_mtx.lock(); } + int _rc = readc; + if (lock) { readc_mtx.unlock(); } + int writeable = (_rc - writec) % this->size; + if (_rc < writec) { + writeable = (this->size + writeable); + } + return std::min(writeable - 1, maxLatency - readable(false) - 1); + } + + void stopReader() { + _stopReader = true; + canReadVar.notify_one(); + } + + void stopWriter() { + _stopWriter = true; + canWriteVar.notify_one(); + } + + bool getReadStop() { + return _stopReader; + } + + bool getWriteStop() { + return _stopWriter; + } + + void clearReadStop() { + _stopReader = false; + } + + void clearWriteStop() { + _stopWriter = false; + } + + void setMaxLatency(int maxLatency) { + this->maxLatency = maxLatency; + } + + private: + T* _buffer; + int size; + int readc; + int writec; + int maxLatency; + bool _stopReader; + bool _stopWriter; + std::mutex readc_mtx; + std::mutex writec_mtx; + std::condition_variable canReadVar; + std::condition_variable canWriteVar; + }; +}; \ No newline at end of file diff --git a/src/dsp/resampling.h b/src/dsp/resampling.h index eb8e1b6f..eb53dffa 100644 --- a/src/dsp/resampling.h +++ b/src/dsp/resampling.h @@ -5,6 +5,8 @@ #include #include +#include + namespace dsp { template @@ -21,7 +23,7 @@ namespace dsp { } void init(stream* in, float interpolation, int blockSize) { - output.init(blockSize * 2); + output.init(blockSize * 2 * interpolation); _input = in; _interpolation = interpolation; _blockSize = blockSize; @@ -194,6 +196,9 @@ namespace dsp { } void start() { + if (running) { + return; + } if (_interp != 1) { interp.start(); } @@ -202,6 +207,9 @@ namespace dsp { } void stop() { + if (!running) { + return; + } interp.stop(); decim.stop(); running = false; @@ -323,6 +331,9 @@ namespace dsp { } void start() { + if (running) { + return; + } if (_interp != 1) { interp.start(); } @@ -331,8 +342,12 @@ namespace dsp { } void stop() { + if (!running) { + return; + } interp.stop(); - decim.stop(); + //decim.stop(); + Sleep(200); running = false; } diff --git a/src/dsp/routing.h b/src/dsp/routing.h index e1d61f29..907b5622 100644 --- a/src/dsp/routing.h +++ b/src/dsp/routing.h @@ -26,7 +26,25 @@ namespace dsp { } 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; } stream output_a; @@ -36,14 +54,16 @@ namespace dsp { static void _worker(Splitter* _this) { complex_t* buf = new complex_t[_this->_bufferSize]; while (true) { - _this->_in->read(buf, _this->_bufferSize); - _this->output_a.write(buf, _this->_bufferSize); - _this->output_b.write(buf, _this->_bufferSize); + 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; }; }; \ No newline at end of file diff --git a/src/dsp/stream.h b/src/dsp/stream.h index 21daf512..fb6d5670 100644 --- a/src/dsp/stream.h +++ b/src/dsp/stream.h @@ -20,7 +20,10 @@ namespace dsp { _stopWriter = false; this->maxLatency = maxLatency; writec = 0; - readc = size - 1; + readc = 0; + readable = 0; + writable = size; + memset(_buffer, 0, size * sizeof(T)); } void init(int maxLatency) { @@ -30,35 +33,36 @@ namespace dsp { _stopWriter = false; this->maxLatency = maxLatency; writec = 0; - readc = size - 1; + 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) { - int canRead = waitUntilReadable(); - if (canRead < 0) { - if (_stopReader) { - printf("Stop reader set"); - } - else { - printf("Stop not set"); - } - clearReadStop(); - return -1; - } - int toRead = std::min(canRead, len - dataRead); + toRead = std::min(waitUntilReadable(), len - dataRead); + if (toRead < 0) { return -1; }; - int len1 = (toRead >= (size - readc) ? (size - readc) : (toRead)); - memcpy(&data[dataRead], &_buffer[readc], len1 * sizeof(T)); - if (len1 < toRead) { - memcpy(&data[dataRead + len1], _buffer, (toRead - len1) * sizeof(T)); + 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; - readc_mtx.lock(); + + _readable_mtx.lock(); + readable -= toRead; + _readable_mtx.unlock(); + _writable_mtx.lock(); + writable += toRead; + _writable_mtx.unlock(); readc = (readc + toRead) % size; - readc_mtx.unlock(); canWriteVar.notify_one(); } return len; @@ -66,114 +70,113 @@ namespace dsp { int readAndSkip(T* data, int len, int skip) { int dataRead = 0; + int toRead = 0; while (dataRead < len) { - int canRead = waitUntilReadable(); - if (canRead < 0) { - clearReadStop(); - return -1; - } - int toRead = std::min(canRead, len - dataRead); + toRead = std::min(waitUntilReadable(), len - dataRead); + if (toRead < 0) { return -1; }; - int len1 = (toRead >= (size - readc) ? (size - readc) : (toRead)); - memcpy(&data[dataRead], &_buffer[readc], len1 * sizeof(T)); - if (len1 < toRead) { - memcpy(&data[dataRead + len1], _buffer, (toRead - len1) * sizeof(T)); + 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; - readc_mtx.lock(); + + _readable_mtx.lock(); + readable -= toRead; + _readable_mtx.unlock(); + _writable_mtx.lock(); + writable += toRead; + _writable_mtx.unlock(); readc = (readc + toRead) % size; - readc_mtx.unlock(); canWriteVar.notify_one(); } - - // Skip - dataRead = 0; while (dataRead < skip) { - int canRead = waitUntilReadable(); - int toRead = std::min(canRead, skip - dataRead); + toRead = std::min(waitUntilReadable(), skip - dataRead); + if (toRead < 0) { return -1; }; dataRead += toRead; - readc_mtx.lock(); + + _readable_mtx.lock(); + readable -= toRead; + _readable_mtx.unlock(); + _writable_mtx.lock(); + writable += toRead; + _writable_mtx.unlock(); readc = (readc + toRead) % size; - readc_mtx.unlock(); canWriteVar.notify_one(); } return len; } int waitUntilReadable() { - int canRead = readable(); - if (canRead > 0) { - return canRead; - } - std::unique_lock lck(writec_mtx); - canReadVar.wait(lck, [=](){ return ((this->readable(false) > 0) || this->getReadStop()); }); - if (this->getReadStop()) { - return -1; - } - return this->readable(false); + 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 readable(bool lock = true) { - if (lock) { writec_mtx.lock(); } - int _wc = writec; - if (lock) { writec_mtx.unlock(); } - int readable = (_wc - readc) % this->size; - if (_wc < readc) { - readable = (this->size + readable); - } - return readable - 1; + int 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 dataWrite = 0; - while (dataWrite < len) { - int canWrite = waitUntilWriteable(); - if (canWrite < 0) { - clearWriteStop(); - return -1; - } - int toWrite = std::min(canWrite, len - dataWrite); + int dataWritten = 0; + int toWrite = 0; + while (dataWritten < len) { + toWrite = std::min(waitUntilwritable(), len - dataWritten); + if (toWrite < 0) { return -1; }; - int len1 = (toWrite >= (size - writec) ? (size - writec) : (toWrite)); - memcpy(&_buffer[writec], &data[dataWrite], len1 * sizeof(T)); - if (len1 < toWrite) { - memcpy(_buffer, &data[dataWrite + len1], (toWrite - len1) * sizeof(T)); + 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)); } - dataWrite += toWrite; - writec_mtx.lock(); + dataWritten += toWrite; + + _readable_mtx.lock(); + readable += toWrite; + _readable_mtx.unlock(); + _writable_mtx.lock(); + writable -= toWrite; + _writable_mtx.unlock(); writec = (writec + toWrite) % size; - writec_mtx.unlock(); + canReadVar.notify_one(); } return len; } - int waitUntilWriteable() { - int canWrite = writeable(); - if (canWrite > 0) { - return canWrite; - } - std::unique_lock lck(readc_mtx); - canWriteVar.wait(lck, [=](){ return ((this->writeable(false) > 0) || this->getWriteStop()); }); - if (this->getWriteStop()) { - return -1; - } - return this->writeable(false); + 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 writeable(bool lock = true) { - if (lock) { readc_mtx.lock(); } - int _rc = readc; - if (lock) { readc_mtx.unlock(); } - int writeable = (_rc - writec) % this->size; - if (_rc < writec) { - writeable = (this->size + writeable); - } - return std::min(writeable - 1, maxLatency - readable(false) - 1); + 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() { @@ -211,11 +214,13 @@ namespace dsp { int size; int readc; int writec; + int readable; + int writable; int maxLatency; bool _stopReader; bool _stopWriter; - std::mutex readc_mtx; - std::mutex writec_mtx; + std::mutex _readable_mtx; + std::mutex _writable_mtx; std::condition_variable canReadVar; std::condition_variable canWriteVar; }; diff --git a/src/main.cpp b/src/main.cpp index cd113792..9dc1492c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -33,7 +33,7 @@ int main() { glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE); // Required on Mac // Create window with graphics context - GLFWwindow* window = glfwCreateWindow(1280, 720, "SDR++ v0.1.0", NULL, NULL); + GLFWwindow* window = glfwCreateWindow(1280, 720, "SDR++ v0.1.0 (Built at " __TIME__ ", " __DATE__ ")", NULL, NULL); if (window == NULL) return 1; glfwMakeContextCurrent(window); diff --git a/src/main_window.cpp b/src/main_window.cpp index 7e7f8c6b..03082265 100644 --- a/src/main_window.cpp +++ b/src/main_window.cpp @@ -49,7 +49,7 @@ void fftHandler(dsp::complex_t* samples) { } void windowInit() { - int sampleRate = 2000000; + int sampleRate = 8000000; wtf.bandWidth = sampleRate; wtf.range = 500000; wtf.centerFrequency = 90500000; diff --git a/src/signal_path.cpp b/src/signal_path.cpp index aa933d4e..5e8eed34 100644 --- a/src/signal_path.cpp +++ b/src/signal_path.cpp @@ -49,31 +49,31 @@ void SignalPath::setDemodulator(int demId) { audioResamp.stop(); // Stop current demodulator - if (_demod == DEMOD_FM) { - printf("Stopping FM demodulator\n"); - demod.stop(); - } - else if (_demod == DEMOD_AM) { - printf("Stopping AM demodulator\n"); - amDemod.stop(); - } - _demod = demId; + // if (_demod == DEMOD_FM) { + // printf("Stopping FM demodulator\n"); + // demod.stop(); + // } + // else if (_demod == DEMOD_AM) { + // printf("Stopping AM demodulator\n"); + // amDemod.stop(); + // } + // _demod = demId; - // Set input of the audio resampler - if (demId == DEMOD_FM) { - printf("Starting FM demodulator\n"); - mainVFO.setOutputSampleRate(200000, 200000); - audioResamp.setInput(&demod.output); - audioResamp.setInputSampleRate(200000, 800); - demod.start(); - } - else if (demId == DEMOD_AM) { - printf("Starting AM demodulator\n"); - mainVFO.setOutputSampleRate(12500, 12500); - audioResamp.setInput(&amDemod.output); - audioResamp.setInputSampleRate(12500, 50); - amDemod.start(); - } + // // Set input of the audio resampler + // if (demId == DEMOD_FM) { + // printf("Starting FM demodulator\n"); + // // mainVFO.setOutputSampleRate(200000, 200000); + // // audioResamp.setInput(&demod.output); + // // audioResamp.setInputSampleRate(200000, 800); + // demod.start(); + // } + // else if (demId == DEMOD_AM) { + // printf("Starting AM demodulator\n"); + // mainVFO.setOutputSampleRate(12500, 12500); + // audioResamp.setInput(&amDemod.output); + // audioResamp.setInputSampleRate(12500, 50); + // amDemod.start(); + // } audioResamp.start(); }