mirror of
https://github.com/AlexandreRouma/SDRPlusPlus.git
synced 2025-07-09 10:35:21 +02:00
new dsp
This commit is contained in:
Ryzerth
@ -1,251 +1,121 @@
|
||||
#pragma once
|
||||
#include <vector>
|
||||
#include <stdio.h>
|
||||
#include <dsp/stream.h>
|
||||
#include <volk/volk.h>
|
||||
#include <dsp/types.h>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
|
||||
#define FL_M_PI 3.1415926535f
|
||||
|
||||
namespace dsp {
|
||||
template <class D, class I, class O, int IC, int OC>
|
||||
class Block {
|
||||
|
||||
template <class BLOCK>
|
||||
class generic_block {
|
||||
public:
|
||||
Block(std::vector<int> inBs, std::vector<int> outBs, D* inst, void (*workerFunc)(D* _this)) {
|
||||
derived = inst;
|
||||
worker = workerFunc;
|
||||
inputBlockSize = inBs;
|
||||
outputBlockSize = outBs;
|
||||
in.reserve(IC);
|
||||
out.reserve(OC);
|
||||
for (int i = 0; i < IC; i++) {
|
||||
in.push_back(NULL);
|
||||
}
|
||||
for (int i = 0; i < OC; i++) {
|
||||
out.push_back(new stream<I>(outBs[i] * 2));
|
||||
}
|
||||
}
|
||||
virtual void init() {}
|
||||
|
||||
void start() {
|
||||
virtual void start() {
|
||||
std::lock_guard<std::mutex> lck(ctrlMtx);
|
||||
if (running) {
|
||||
return;
|
||||
}
|
||||
doStart();
|
||||
}
|
||||
|
||||
virtual void stop() {
|
||||
std::lock_guard<std::mutex> 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<stream<I>*> out;
|
||||
void tempStart() {
|
||||
if (tempStopped) {
|
||||
doStart();
|
||||
tempStopped = false;
|
||||
}
|
||||
}
|
||||
|
||||
void tempStop() {
|
||||
if (running && !tempStopped) {
|
||||
doStop();
|
||||
tempStopped = true;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<untyped_steam*> inputs;
|
||||
std::vector<untyped_steam*> outputs;
|
||||
|
||||
bool running = false;
|
||||
bool tempStopped = false;
|
||||
|
||||
std::thread workerThread;
|
||||
|
||||
protected:
|
||||
virtual void startHandler() {}
|
||||
virtual void stopHandler() {}
|
||||
std::vector<stream<I>*> in;
|
||||
std::vector<int> inputBlockSize;
|
||||
std::vector<int> outputBlockSize;
|
||||
bool running = false;
|
||||
|
||||
private:
|
||||
void (*worker)(D* _this);
|
||||
std::thread workerThread;
|
||||
D* derived;
|
||||
std::mutex ctrlMtx;
|
||||
|
||||
};
|
||||
|
||||
|
||||
class DemoMultiplier : public Block<DemoMultiplier, complex_t, complex_t, 2, 1> {
|
||||
public:
|
||||
DemoMultiplier() : Block({2}, {1}, this, worker) {}
|
||||
|
||||
void init(stream<complex_t>* a, stream<complex_t>* b, int blockSize) {
|
||||
in[0] = a;
|
||||
in[1] = b;
|
||||
inputBlockSize[0] = blockSize;
|
||||
inputBlockSize[1] = blockSize;
|
||||
out[0]->setMaxLatency(blockSize * 2);
|
||||
outputBlockSize[0] = blockSize;
|
||||
}
|
||||
|
||||
private:
|
||||
static void worker(DemoMultiplier* _this) {
|
||||
int blockSize = _this->inputBlockSize[0];
|
||||
stream<complex_t>* inA = _this->in[0];
|
||||
stream<complex_t>* inB = _this->in[1];
|
||||
stream<complex_t>* out = _this->out[0];
|
||||
complex_t* aBuf = (complex_t*)volk_malloc(sizeof(complex_t) * blockSize, volk_get_alignment());
|
||||
complex_t* bBuf = (complex_t*)volk_malloc(sizeof(complex_t) * blockSize, volk_get_alignment());
|
||||
complex_t* outBuf = (complex_t*)volk_malloc(sizeof(complex_t) * blockSize, volk_get_alignment());
|
||||
while (true) {
|
||||
if (inA->read(aBuf, blockSize) < 0) { break; };
|
||||
if (inB->read(bBuf, blockSize) < 0) { break; };
|
||||
volk_32fc_x2_multiply_32fc((lv_32fc_t*)outBuf, (lv_32fc_t*)aBuf, (lv_32fc_t*)bBuf, blockSize);
|
||||
if (out->write(outBuf, blockSize) < 0) { break; };
|
||||
}
|
||||
volk_free(aBuf);
|
||||
volk_free(bBuf);
|
||||
volk_free(outBuf);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class Squelch : public Block<Squelch, complex_t, complex_t, 1, 1> {
|
||||
public:
|
||||
Squelch() : Block({1}, {1}, this, worker) {}
|
||||
|
||||
void init(stream<complex_t>* 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<complex_t>* in = _this->in[0];
|
||||
stream<complex_t>* 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 T>
|
||||
class Reshaper {
|
||||
public:
|
||||
Reshaper() {
|
||||
|
||||
}
|
||||
|
||||
void init(int outBlockSize, dsp::stream<T>* 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<T>* 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<T> 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<T>* in;
|
||||
|
||||
};
|
||||
};
|
||||
}
|
||||
Reference in New Issue
Block a user