mirror of
https://github.com/AlexandreRouma/SDRPlusPlus.git
synced 2026-04-19 06:42:43 +00:00
New stuff lmao
This commit is contained in:
Ryzerth
@@ -260,6 +260,9 @@ namespace dsp {
|
||||
else if (mode == MODE_LSB) {
|
||||
lo.setFrequency(-_bandWidth / 2.0f);
|
||||
}
|
||||
else if (mode == MODE_LSB) {
|
||||
lo.setFrequency(0);
|
||||
}
|
||||
}
|
||||
|
||||
stream<float> output;
|
||||
@@ -267,6 +270,7 @@ namespace dsp {
|
||||
enum {
|
||||
MODE_USB,
|
||||
MODE_LSB,
|
||||
MODE_DSB,
|
||||
_MODE_COUNT
|
||||
};
|
||||
|
||||
@@ -309,4 +313,102 @@ namespace dsp {
|
||||
int _mode;
|
||||
bool running = false;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// class CWDemod {
|
||||
// public:
|
||||
// CWDemod() {
|
||||
|
||||
// }
|
||||
|
||||
// void init(stream<complex_t>* input, float sampleRate, float bandWidth, int blockSize) {
|
||||
// _blockSize = blockSize;
|
||||
// _bandWidth = bandWidth;
|
||||
// _mode = MODE_USB;
|
||||
// output.init(blockSize * 2);
|
||||
// lo.init(bandWidth / 2.0f, sampleRate, blockSize);
|
||||
// mixer.init(input, &lo.output, blockSize);
|
||||
// lo.start();
|
||||
// }
|
||||
|
||||
// void start() {
|
||||
// mixer.start();
|
||||
// _workerThread = std::thread(_worker, this);
|
||||
// running = true;
|
||||
// }
|
||||
|
||||
// void stop() {
|
||||
// mixer.stop();
|
||||
// mixer.output.stopReader();
|
||||
// output.stopWriter();
|
||||
// _workerThread.join();
|
||||
// mixer.output.clearReadStop();
|
||||
// output.clearWriteStop();
|
||||
// running = false;
|
||||
// }
|
||||
|
||||
// void setBlockSize(int blockSize) {
|
||||
// if (running) {
|
||||
// return;
|
||||
// }
|
||||
// _blockSize = blockSize;
|
||||
// }
|
||||
|
||||
// void setMode(int mode) {
|
||||
// if (mode < 0 && mode >= _MODE_COUNT) {
|
||||
// return;
|
||||
// }
|
||||
// _mode = mode;
|
||||
// if (mode == MODE_USB) {
|
||||
// lo.setFrequency(_bandWidth / 2.0f);
|
||||
// }
|
||||
// else if (mode == MODE_LSB) {
|
||||
// lo.setFrequency(-_bandWidth / 2.0f);
|
||||
// }
|
||||
// }
|
||||
|
||||
// stream<float> output;
|
||||
|
||||
// private:
|
||||
// static void _worker(CWDemod* _this) {
|
||||
// complex_t* inBuf = new complex_t[_this->_blockSize];
|
||||
// float* outBuf = new float[_this->_blockSize];
|
||||
|
||||
// float min, max, factor;
|
||||
|
||||
// while (true) {
|
||||
// if (_this->mixer.output.read(inBuf, _this->_blockSize) < 0) { break; };
|
||||
// min = INFINITY;
|
||||
// max = -INFINITY;
|
||||
// for (int i = 0; i < _this->_blockSize; i++) {
|
||||
// outBuf[i] = inBuf[i].q;
|
||||
// if (inBuf[i].q < min) {
|
||||
// min = inBuf[i].q;
|
||||
// }
|
||||
// if (inBuf[i].q > max) {
|
||||
// max = inBuf[i].q;
|
||||
// }
|
||||
// }
|
||||
// factor = (max - min) / 2;
|
||||
// for (int i = 0; i < _this->_blockSize; i++) {
|
||||
// outBuf[i] /= factor;
|
||||
// }
|
||||
// if (_this->output.write(outBuf, _this->_blockSize) < 0) { break; };
|
||||
// }
|
||||
|
||||
// delete[] inBuf;
|
||||
// delete[] outBuf;
|
||||
// }
|
||||
|
||||
// std::thread _workerThread;
|
||||
// SineSource lo;
|
||||
// Multiplier mixer;
|
||||
// int _blockSize;
|
||||
// float _bandWidth;
|
||||
// int _mode;
|
||||
// bool running = false;
|
||||
// };
|
||||
};
|
||||
114
src/dsp/filter.h
114
src/dsp/filter.h
@@ -4,11 +4,12 @@
|
||||
#include <dsp/types.h>
|
||||
#include <vector>
|
||||
#include <dsp/math.h>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#define GET_FROM_RIGHT_BUF(buffer, delayLine, delayLineSz, n) (((n) < 0) ? delayLine[(delayLineSz) + (n)] : buffer[(n)])
|
||||
|
||||
namespace dsp {
|
||||
inline void BlackmanWindow(std::vector<float>& taps, float sampleRate, float cutoff, float transWidth) {
|
||||
inline void BlackmanWindow(std::vector<float>& taps, float sampleRate, float cutoff, float transWidth, int addedTaps = 0) {
|
||||
taps.clear();
|
||||
|
||||
float fc = cutoff / sampleRate;
|
||||
@@ -16,7 +17,7 @@ namespace dsp {
|
||||
fc = 1.0f;
|
||||
}
|
||||
|
||||
int _M = 4.0f / (transWidth / sampleRate);
|
||||
int _M = (4.0f / (transWidth / sampleRate)) + (float)addedTaps;
|
||||
if (_M < 4) {
|
||||
_M = 4;
|
||||
}
|
||||
@@ -377,4 +378,113 @@ namespace dsp {
|
||||
float* _taps;
|
||||
bool running;
|
||||
};
|
||||
|
||||
class FMDeemphasis {
|
||||
public:
|
||||
FMDeemphasis() {
|
||||
|
||||
}
|
||||
|
||||
FMDeemphasis(stream<float>* input, int bufferSize, float tau, float sampleRate) : output(bufferSize * 2) {
|
||||
_in = input;
|
||||
_bufferSize = bufferSize;
|
||||
bypass = false;
|
||||
_tau = tau;
|
||||
_sampleRate = sampleRate;
|
||||
}
|
||||
|
||||
void init(stream<float>* input, int bufferSize, float tau, float sampleRate) {
|
||||
output.init(bufferSize * 2);
|
||||
_in = input;
|
||||
_bufferSize = bufferSize;
|
||||
bypass = false;
|
||||
_tau = tau;
|
||||
_sampleRate = sampleRate;
|
||||
}
|
||||
|
||||
void start() {
|
||||
if (running) {
|
||||
return;
|
||||
}
|
||||
_workerThread = std::thread(_worker, this);
|
||||
running = true;
|
||||
}
|
||||
|
||||
void stop() {
|
||||
if (!running) {
|
||||
return;
|
||||
}
|
||||
_in->stopReader();
|
||||
output.stopWriter();
|
||||
_workerThread.join();
|
||||
_in->clearReadStop();
|
||||
output.clearWriteStop();
|
||||
running = false;
|
||||
}
|
||||
|
||||
void setBlockSize(int blockSize) {
|
||||
if (running) {
|
||||
return;
|
||||
}
|
||||
_bufferSize = blockSize;
|
||||
output.setMaxLatency(blockSize * 2);
|
||||
}
|
||||
|
||||
void setSamplerate(float sampleRate) {
|
||||
if (running) {
|
||||
return;
|
||||
}
|
||||
_sampleRate = sampleRate;
|
||||
}
|
||||
|
||||
void setTau(float tau) {
|
||||
if (running) {
|
||||
return;
|
||||
}
|
||||
_tau = tau;
|
||||
}
|
||||
|
||||
stream<float> output;
|
||||
bool bypass;
|
||||
|
||||
private:
|
||||
static void _worker(FMDeemphasis* _this) {
|
||||
float* inBuf = new float[_this->_bufferSize];
|
||||
float* outBuf = new float[_this->_bufferSize];
|
||||
int count = _this->_bufferSize;
|
||||
float lastOut = 0.0f;
|
||||
float dt = 1.0f / _this->_sampleRate;
|
||||
float alpha = dt / (_this->_tau + dt);
|
||||
|
||||
spdlog::warn("Deemp filter started: {0}, {1}", _this->_tau * 1000000.0, _this->_sampleRate);
|
||||
|
||||
while (true) {
|
||||
if (_this->_in->read(inBuf, count) < 0) { break; };
|
||||
if (_this->bypass) {
|
||||
if (_this->output.write(inBuf, count) < 0) { break; };
|
||||
continue;
|
||||
}
|
||||
|
||||
if (isnan(lastOut)) {
|
||||
lastOut = 0.0f;
|
||||
}
|
||||
outBuf[0] = (alpha * inBuf[0]) + ((1-alpha) * lastOut);
|
||||
for (int i = 1; i < count; i++) {
|
||||
outBuf[i] = (alpha * inBuf[i]) + ((1 - alpha) * outBuf[i - 1]);
|
||||
}
|
||||
lastOut = outBuf[count - 1];
|
||||
|
||||
if (_this->output.write(outBuf, count) < 0) { break; };
|
||||
}
|
||||
delete[] inBuf;
|
||||
delete[] outBuf;
|
||||
}
|
||||
|
||||
stream<float>* _in;
|
||||
int _bufferSize;
|
||||
std::thread _workerThread;
|
||||
bool running = false;
|
||||
float _sampleRate;
|
||||
float _tau;
|
||||
};
|
||||
};
|
||||
@@ -564,4 +564,212 @@ namespace dsp {
|
||||
int _blockSize;
|
||||
bool running = false;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
class FloatPolyphaseFIRResampler {
|
||||
public:
|
||||
FloatPolyphaseFIRResampler() {
|
||||
|
||||
}
|
||||
|
||||
void init(stream<float>* in, float inputSampleRate, float outputSampleRate, int blockSize, float passBand = -1.0f, float transWidth = -1.0f) {
|
||||
_input = in;
|
||||
_outputSampleRate = outputSampleRate;
|
||||
_inputSampleRate = inputSampleRate;
|
||||
int _gcd = std::gcd((int)inputSampleRate, (int)outputSampleRate);
|
||||
_interp = outputSampleRate / _gcd;
|
||||
_decim = inputSampleRate / _gcd;
|
||||
_blockSize = blockSize;
|
||||
outputBlockSize = (blockSize * _interp) / _decim;
|
||||
output.init(outputBlockSize * 2);
|
||||
|
||||
float cutoff = std::min<float>(_outputSampleRate / 2.0f, _inputSampleRate / 2.0f);
|
||||
if (passBand > 0.0f && transWidth > 0.0f) {
|
||||
dsp::BlackmanWindow(_taps, _outputSampleRate, passBand, transWidth, _interp - 1);
|
||||
}
|
||||
else {
|
||||
dsp::BlackmanWindow(_taps, _outputSampleRate, cutoff, cutoff, _interp - 1);
|
||||
}
|
||||
}
|
||||
|
||||
void start() {
|
||||
if (running) {
|
||||
return;
|
||||
}
|
||||
_workerThread = std::thread(_worker, this);
|
||||
running = true;
|
||||
}
|
||||
|
||||
void stop() {
|
||||
if (!running) {
|
||||
return;
|
||||
}
|
||||
_input->stopReader();
|
||||
output.stopWriter();
|
||||
_workerThread.join();
|
||||
_input->clearReadStop();
|
||||
output.clearWriteStop();
|
||||
running = false;
|
||||
}
|
||||
|
||||
void setInputSampleRate(float inputSampleRate, int blockSize = -1, float passBand = -1.0f, float transWidth = -1.0f) {
|
||||
stop();
|
||||
_inputSampleRate = inputSampleRate;
|
||||
int _gcd = std::gcd((int)inputSampleRate, (int)_outputSampleRate);
|
||||
_interp = _outputSampleRate / _gcd;
|
||||
_decim = inputSampleRate / _gcd;
|
||||
|
||||
float cutoff = std::min<float>(_outputSampleRate / 2.0f, _inputSampleRate / 2.0f);
|
||||
if (passBand > 0.0f && transWidth > 0.0f) {
|
||||
dsp::BlackmanWindow(_taps, _outputSampleRate, passBand, transWidth, _interp - 1);
|
||||
}
|
||||
else {
|
||||
dsp::BlackmanWindow(_taps,_outputSampleRate, cutoff, cutoff, _interp - 1);
|
||||
}
|
||||
|
||||
if (blockSize > 0) {
|
||||
_blockSize = blockSize;
|
||||
}
|
||||
outputBlockSize = (blockSize * _interp) / _decim;
|
||||
output.setMaxLatency(outputBlockSize * 2);
|
||||
start();
|
||||
}
|
||||
|
||||
void setOutputSampleRate(float outputSampleRate, float passBand = -1.0f, float transWidth = -1.0f) {
|
||||
stop();
|
||||
_outputSampleRate = outputSampleRate;
|
||||
int _gcd = std::gcd((int)_inputSampleRate, (int)outputSampleRate);
|
||||
_interp = outputSampleRate / _gcd;
|
||||
_decim = _inputSampleRate / _gcd;
|
||||
outputBlockSize = (_blockSize * _interp) / _decim;
|
||||
output.setMaxLatency(outputBlockSize * 2);
|
||||
|
||||
float cutoff = std::min<float>(_outputSampleRate / 2.0f, _inputSampleRate / 2.0f);
|
||||
if (passBand > 0.0f && transWidth > 0.0f) {
|
||||
dsp::BlackmanWindow(_taps, _outputSampleRate, passBand, transWidth, _interp - 1);
|
||||
}
|
||||
else {
|
||||
dsp::BlackmanWindow(_taps, _outputSampleRate, cutoff, cutoff, _interp - 1);
|
||||
}
|
||||
|
||||
start();
|
||||
}
|
||||
|
||||
void setFilterParams(float passBand, float transWidth) {
|
||||
stop();
|
||||
dsp::BlackmanWindow(_taps, _outputSampleRate, passBand, transWidth, _interp - 1);
|
||||
start();
|
||||
}
|
||||
|
||||
void setBlockSize(int blockSize) {
|
||||
stop();
|
||||
_blockSize = blockSize;
|
||||
outputBlockSize = (_blockSize * _interp) / _decim;
|
||||
output.setMaxLatency(outputBlockSize * 2);
|
||||
start();
|
||||
}
|
||||
|
||||
void setInput(stream<float>* input) {
|
||||
if (running) {
|
||||
return;
|
||||
}
|
||||
_input = input;
|
||||
}
|
||||
|
||||
int getOutputBlockSize() {
|
||||
return outputBlockSize;
|
||||
}
|
||||
|
||||
stream<float> output;
|
||||
|
||||
private:
|
||||
static void _worker(FloatPolyphaseFIRResampler* _this) {
|
||||
float* inBuf = new float[_this->_blockSize];
|
||||
float* outBuf = new float[_this->outputBlockSize];
|
||||
|
||||
int inCount = _this->_blockSize;
|
||||
int outCount = _this->outputBlockSize;
|
||||
|
||||
int interp = _this->_interp;
|
||||
int decim = _this->_decim;
|
||||
float correction = interp;//(float)sqrt((float)interp);
|
||||
|
||||
int tapCount = _this->_taps.size();
|
||||
float* taps = new float[tapCount];
|
||||
for (int i = 0; i < tapCount; i++) {
|
||||
taps[i] = _this->_taps[i] * correction;
|
||||
}
|
||||
|
||||
float* delayBuf = new float[tapCount];
|
||||
|
||||
float* delayStart = &inBuf[std::max<int>(inCount - tapCount, 0)];
|
||||
int delaySize = tapCount * sizeof(float);
|
||||
float* delayBufEnd = &delayBuf[std::max<int>(tapCount - inCount, 0)];
|
||||
int moveSize = std::min<int>(inCount, tapCount - inCount) * sizeof(float);
|
||||
int inSize = inCount * sizeof(float);
|
||||
|
||||
int afterInterp = inCount * interp;
|
||||
int outIndex = 0;
|
||||
|
||||
tapCount -= interp - 1;
|
||||
|
||||
while (true) {
|
||||
if (_this->_input->read(inBuf, inCount) < 0) { break; };
|
||||
|
||||
|
||||
for (int i = 0; i < outCount; i++) {
|
||||
outBuf[i] = 0;
|
||||
int filterId = (i * decim) % interp;
|
||||
int inputId = (i * decim) / interp;
|
||||
for (int j = 0; j < tapCount; j++) {
|
||||
outBuf[i] += GET_FROM_RIGHT_BUF(inBuf, delayBuf, tapCount, inputId - j) * taps[j + filterId];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if (tapCount > inCount) {
|
||||
memmove(delayBuf, delayBufEnd, moveSize);
|
||||
memcpy(delayBufEnd, delayStart, inSize);
|
||||
}
|
||||
else {
|
||||
memcpy(delayBuf, delayStart, delaySize);
|
||||
}
|
||||
|
||||
if (_this->output.write(outBuf, _this->outputBlockSize) < 0) { break; };
|
||||
}
|
||||
delete[] inBuf;
|
||||
delete[] outBuf;
|
||||
delete[] delayBuf;
|
||||
}
|
||||
|
||||
std::thread _workerThread;
|
||||
|
||||
stream<float>* _input;
|
||||
std::vector<float> _taps;
|
||||
int _interp;
|
||||
int _decim;
|
||||
int outputBlockSize;
|
||||
float _outputSampleRate;
|
||||
float _inputSampleRate;
|
||||
int _blockSize;
|
||||
bool running = false;
|
||||
};
|
||||
};
|
||||
Reference in New Issue
Block a user