mirror of
https://github.com/AlexandreRouma/SDRPlusPlus.git
synced 2025-06-25 20:07:51 +02:00
New radio logic system
This commit is contained in:
AlexandreRouma
208
core/src/dsp/chain.h
Normal file
208
core/src/dsp/chain.h
Normal file
@ -0,0 +1,208 @@
|
||||
#pragma once
|
||||
#include <dsp/stream.h>
|
||||
#include <type_traits>
|
||||
#include <vector>
|
||||
#include <utils/event.h>
|
||||
|
||||
namespace dsp {
|
||||
template <class T>
|
||||
class ChainLinkAny {
|
||||
public:
|
||||
virtual ~ChainLinkAny() {}
|
||||
virtual void setInput(stream<T>* stream) = 0;
|
||||
virtual stream<T>* getOutput() = 0;
|
||||
virtual void start() = 0;
|
||||
virtual void stop() = 0;
|
||||
bool enabled = false;
|
||||
};
|
||||
|
||||
template <class BLOCK, class T>
|
||||
class ChainLink : public ChainLinkAny<T> {
|
||||
public:
|
||||
~ChainLink() {}
|
||||
|
||||
void setInput(stream<T>* stream) {
|
||||
block.setInput(stream);
|
||||
}
|
||||
|
||||
stream<T>* getOutput() {
|
||||
return &block.out;
|
||||
}
|
||||
|
||||
void start() {
|
||||
block.start();
|
||||
}
|
||||
|
||||
void stop() {
|
||||
block.stop();
|
||||
}
|
||||
|
||||
BLOCK block;
|
||||
};
|
||||
|
||||
template <class T>
|
||||
class Chain {
|
||||
public:
|
||||
Chain() {}
|
||||
|
||||
Chain(stream<T>* input, EventHandler<stream<T>*>* outputChangedHandler) {
|
||||
init(input, outputChangedHandler);
|
||||
}
|
||||
|
||||
void init(stream<T>* input, EventHandler<stream<T>*>* outputChangedHandler) {
|
||||
_input = input;
|
||||
onOutputChanged.bindHandler(outputChangedHandler);
|
||||
}
|
||||
|
||||
void add(ChainLinkAny<T>* link) {
|
||||
// Check that link exists
|
||||
if (std::find(links.begin(), links.end(), link) != links.end()) {
|
||||
spdlog::error("Could not add new link to the chain, link already in the chain");
|
||||
return;
|
||||
}
|
||||
|
||||
// Assert that the link is stopped and disabled
|
||||
link->stop();
|
||||
link->enabled = false;
|
||||
|
||||
// Add new link to the list
|
||||
links.push_back(link);
|
||||
}
|
||||
|
||||
void enable(ChainLinkAny<T>* link) {
|
||||
// Check that link exists and locate it
|
||||
auto lnit = std::find(links.begin(), links.end(), link);
|
||||
if (lnit == links.end()) {
|
||||
spdlog::error("Could not enable link");
|
||||
return;
|
||||
}
|
||||
// Enable the link
|
||||
link->enabled = true;
|
||||
|
||||
// Find input
|
||||
stream<T>* input = _input;
|
||||
for (auto i = links.begin(); i < lnit; i++) {
|
||||
if (!(*i)->enabled) { continue; }
|
||||
input = (*i)->getOutput();
|
||||
}
|
||||
|
||||
// Find next block
|
||||
ChainLinkAny<T>* nextLink = NULL;
|
||||
for (auto i = ++lnit; i < links.end(); i++) {
|
||||
if (!(*i)->enabled) { continue; }
|
||||
nextLink = *i;
|
||||
}
|
||||
|
||||
if (nextLink) {
|
||||
// If a next block exists, change its input
|
||||
nextLink->setInput(link->getOutput());
|
||||
}
|
||||
else {
|
||||
// If there are no next blocks, change output of outside reader
|
||||
onOutputChanged.emit(link->getOutput());
|
||||
}
|
||||
|
||||
// Set input of newly enabled link
|
||||
link->setInput(input);
|
||||
|
||||
// If running, start everything
|
||||
if (running) { start(); }
|
||||
}
|
||||
|
||||
void disable(ChainLinkAny<T>* link) {
|
||||
// Check that link exists and locate it
|
||||
auto lnit = std::find(links.begin(), links.end(), link);
|
||||
if (lnit == links.end()) {
|
||||
spdlog::error("Could not disable link");
|
||||
return;
|
||||
}
|
||||
|
||||
// Stop and disable link
|
||||
link->stop();
|
||||
link->enabled = false;
|
||||
|
||||
// Find its input
|
||||
stream<T>* input = _input;
|
||||
for (auto i = links.begin(); i < lnit; i++) {
|
||||
if (!(*i)->enabled) { continue; }
|
||||
input = (*i)->getOutput();
|
||||
}
|
||||
|
||||
// Find next block
|
||||
ChainLinkAny<T>* nextLink = NULL;
|
||||
for (auto i = ++lnit; i < links.end(); i++) {
|
||||
if (!(*i)->enabled) { continue; }
|
||||
nextLink = *i;
|
||||
}
|
||||
|
||||
if (nextLink) {
|
||||
// If a next block exists, change its input
|
||||
nextLink->setInput(input);
|
||||
}
|
||||
else {
|
||||
// If there are no next blocks, change output of outside reader
|
||||
onOutputChanged.emit(input);
|
||||
}
|
||||
}
|
||||
|
||||
void disableAll() {
|
||||
for (auto& ln : links) {
|
||||
disable(ln);
|
||||
}
|
||||
}
|
||||
|
||||
void setState(ChainLinkAny<T>* link, bool enabled) {
|
||||
enabled ? enable(link) : disable(link);
|
||||
}
|
||||
|
||||
void setInput(stream<T>* input) {
|
||||
_input = input;
|
||||
|
||||
// Set input of first enabled link
|
||||
for (auto& ln : links) {
|
||||
if (!ln->enabled) { continue; }
|
||||
ln->setInput(_input);
|
||||
return;
|
||||
}
|
||||
|
||||
// No block found, this means nothing is enabled
|
||||
onOutputChanged.emit(_input);
|
||||
}
|
||||
|
||||
stream<T>* getOutput() {
|
||||
stream<T>* lastOutput = _input;
|
||||
for (auto& ln : links) {
|
||||
if (!ln->enabled) { continue; }
|
||||
lastOutput = ln->getOutput();
|
||||
}
|
||||
return lastOutput;
|
||||
}
|
||||
|
||||
void start() {
|
||||
running = true;
|
||||
for (auto& ln : links) {
|
||||
if (ln->enabled) {
|
||||
ln->start();
|
||||
}
|
||||
else {
|
||||
ln->stop();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void stop() {
|
||||
running = false;
|
||||
for (auto& ln : links) {
|
||||
ln->stop();
|
||||
}
|
||||
}
|
||||
|
||||
Event<stream<T>*> onOutputChanged;
|
||||
|
||||
private:
|
||||
stream<T>* _input;
|
||||
std::vector<ChainLinkAny<T>*> links;
|
||||
bool running = false;
|
||||
|
||||
};
|
||||
}
|
||||
@ -128,4 +128,126 @@ namespace dsp {
|
||||
float* fft_fout;
|
||||
|
||||
};
|
||||
|
||||
class NoiseBlanker : public generic_block<NoiseBlanker> {
|
||||
public:
|
||||
NoiseBlanker() {}
|
||||
|
||||
NoiseBlanker(stream<complex_t>* in, float attack, float decay, float threshold, float level, float sampleRate) { init(in, attack, decay, threshold, level, sampleRate); }
|
||||
|
||||
~NoiseBlanker() {
|
||||
if (!generic_block<NoiseBlanker>::_block_init) { return; }
|
||||
generic_block<NoiseBlanker>::stop();
|
||||
volk_free(ampBuf);
|
||||
}
|
||||
|
||||
void init(stream<complex_t>* in, float attack, float decay, float threshold, float level, float sampleRate) {
|
||||
_in = in;
|
||||
_attack = attack;
|
||||
_decay = decay;
|
||||
_threshold = powf(10.0f, threshold / 10.0f);
|
||||
_level = level;
|
||||
_sampleRate = sampleRate;
|
||||
|
||||
_inv_attack = 1.0f - _attack;
|
||||
_inv_decay = 1.0f - _decay;
|
||||
|
||||
ampBuf = (float*)volk_malloc(STREAM_BUFFER_SIZE*sizeof(float), volk_get_alignment());
|
||||
|
||||
generic_block<NoiseBlanker>::registerInput(_in);
|
||||
generic_block<NoiseBlanker>::registerOutput(&out);
|
||||
generic_block<NoiseBlanker>::_block_init = true;
|
||||
}
|
||||
|
||||
void setAttack(float attack) {
|
||||
_attack = attack;
|
||||
_inv_attack = 1.0f - _attack;
|
||||
}
|
||||
|
||||
void setDecay(float decay) {
|
||||
_decay = decay;
|
||||
_inv_decay = 1.0f - _decay;
|
||||
}
|
||||
|
||||
void setThreshold(float threshold) {
|
||||
_threshold = powf(10.0f, threshold / 10.0f);
|
||||
spdlog::warn("Threshold {0}", _threshold);
|
||||
}
|
||||
|
||||
void setLevel(float level) {
|
||||
_level = level;
|
||||
}
|
||||
|
||||
void setSampleRate(float sampleRate) {
|
||||
_sampleRate = sampleRate;
|
||||
// TODO: Change parameters if the algo needs it
|
||||
}
|
||||
|
||||
void setInput(stream<complex_t>* in) {
|
||||
assert(generic_block<NoiseBlanker>::_block_init);
|
||||
std::lock_guard<std::mutex> lck(generic_block<NoiseBlanker>::ctrlMtx);
|
||||
generic_block<NoiseBlanker>::tempStop();
|
||||
generic_block<NoiseBlanker>::unregisterInput(_in);
|
||||
_in = in;
|
||||
generic_block<NoiseBlanker>::registerInput(_in);
|
||||
generic_block<NoiseBlanker>::tempStart();
|
||||
}
|
||||
|
||||
int run() {
|
||||
int count = _in->read();
|
||||
if (count < 0) { return -1; }
|
||||
|
||||
// Get amplitudes
|
||||
volk_32fc_magnitude_32f(ampBuf, (lv_32fc_t*)_in->readBuf, count);
|
||||
|
||||
// Apply filtering and threshold
|
||||
float val;
|
||||
for (int i = 0; i < count; i++) {
|
||||
// Filter using attack/threshold methode
|
||||
val = ampBuf[i];
|
||||
if (val > lastValue) {
|
||||
lastValue = (_inv_attack*lastValue) + (_attack*val);
|
||||
}
|
||||
else {
|
||||
lastValue = (_inv_decay*lastValue) + (_decay*val);
|
||||
}
|
||||
|
||||
// Apply threshold and invert
|
||||
if (lastValue > _threshold) {
|
||||
ampBuf[i] = _threshold / (lastValue * _level);
|
||||
if (ampBuf[i] == 0) {
|
||||
spdlog::warn("WTF???");
|
||||
}
|
||||
}
|
||||
else {
|
||||
ampBuf[i] = 1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// Multiply
|
||||
volk_32fc_32f_multiply_32fc((lv_32fc_t*)out.writeBuf, (lv_32fc_t*)_in->readBuf, ampBuf, count);
|
||||
|
||||
_in->flush();
|
||||
if (!out.swap(count)) { return -1; }
|
||||
return count;
|
||||
}
|
||||
|
||||
stream<complex_t> out;
|
||||
|
||||
private:
|
||||
float* ampBuf;
|
||||
|
||||
float _attack;
|
||||
float _decay;
|
||||
float _inv_attack;
|
||||
float _inv_decay;
|
||||
float _threshold;
|
||||
float _level;
|
||||
float _sampleRate;
|
||||
|
||||
float lastValue = 0.0f;
|
||||
|
||||
stream<complex_t>* _in;
|
||||
|
||||
};
|
||||
}
|
||||
Reference in New Issue
Block a user