ryzerth/sdrpp
1
0
Fork 0
mirror of https://github.com/AlexandreRouma/SDRPlusPlus.git synced 2026-04-19 14:52:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

new stuff

Browse Source
This commit is contained in:
Ryzerth
2020-06-15 15:53:45 +02:00
parent 7120e848ef
commit fa2f6a0a9b
14 changed files with 1067 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files

123
src/cdsp/demodulation.h
View File

@@ -3,6 +3,28 @@
#include <cdsp/stream.h>
#include <cdsp/types.h>
#define FAST_ATAN2_COEF1 3.1415926535f / 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;
}
return angle;
}
namespace cdsp {
class FMDemodulator {
@@ -12,6 +34,7 @@ namespace cdsp {
}
FMDemodulator(stream<complex_t>* in, float deviation, long sampleRate, int bufferSize) : output(bufferSize * 2) {
running = false;
_input = in;
_bufferSize = bufferSize;
_phase = 0.0f;
@@ -20,6 +43,7 @@ namespace cdsp {
void init(stream<complex_t>* in, float deviation, long sampleRate, int bufferSize) {
output.init(bufferSize * 2);
running = false;
_input = in;
_bufferSize = bufferSize;
_phase = 0.0f;
@@ -27,9 +51,25 @@ namespace cdsp {
}
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();
}
stream<float> output;
private:
@@ -39,21 +79,98 @@ namespace cdsp {
float diff = 0;
float currentPhase = 0;
while (true) {
_this->_input->read(inBuf, _this->_bufferSize);
if (_this->_input->read(inBuf, _this->_bufferSize) < 0) { return; };
for (int i = 0; i < _this->_bufferSize; i++) {
currentPhase = atan2f(inBuf[i].i, inBuf[i].q);
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;
}
_this->output.write(outBuf, _this->_bufferSize);
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { return; };
}
}
stream<complex_t>* _input;
bool running;
int _bufferSize;
float _phase;
float _phasorSpeed;
std::thread _workerThread;
};
class AMDemodulator {
public:
AMDemodulator() {
}
AMDemodulator(stream<complex_t>* in, int bufferSize) : output(bufferSize * 2) {
running = false;
_input = in;
_bufferSize = bufferSize;
}
void init(stream<complex_t>* in, int bufferSize) {
output.init(bufferSize * 2);
running = false;
_input = in;
_bufferSize = bufferSize;
}
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();
}
stream<float> output;
private:
static void _worker(AMDemodulator* _this) {
complex_t* inBuf = new complex_t[_this->_bufferSize];
float* outBuf = new float[_this->_bufferSize];
float min, max, amp;
while (true) {
if (_this->_input->read(inBuf, _this->_bufferSize) < 0) { return; };
min = INFINITY;
max = 0.0f;
for (int i = 0; i < _this->_bufferSize; 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);
for (int i = 0; i < _this->_bufferSize; i++) {
outBuf[i] = (outBuf[i] - min) / (max - min);
}
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { return; };
}
}
stream<complex_t>* _input;
bool running;
int _bufferSize;
std::thread _workerThread;
};
};

370
src/cdsp/filter.h
View File

@@ -4,78 +4,338 @@
#include <cdsp/types.h>
#include <vector>
#define GET_FROM_RIGHT_BUF(buffer, delayLine, delayLineSz, n) (((n) < 0) ? delayLine[(delayLineSz) + (n)] : buffer[(n)])
#define M_PI 3.1415926535f
inline void BlackmanWindow(std::vector<float>& taps, float sampleRate, float cutoff, float transWidth) {
taps.clear();
float fc = cutoff / sampleRate;
int _M = 4.0f / (transWidth / sampleRate);
if (_M % 2 == 0) { _M++; }
float M = _M;
float sum = 0.0f;
for (int i = 0; i < _M; i++) {
float 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;
}
}
namespace cdsp {
class FIRFilter {
class DecimatingFIRFilter {
public:
FIRFilter() {
DecimatingFIRFilter() {
}
FIRFilter(stream<complex_t>* input, std::vector<float> taps, int bufferSize) : output(bufferSize * 2) {
_in = input;
_bufferSize = bufferSize;
_tapCount = taps.size();
delayBuf = new complex_t[_tapCount];
_taps = taps;
}
void init(stream<complex_t>* input, std::vector<float> taps, int bufferSize) {
DecimatingFIRFilter(stream<complex_t>* input, std::vector<float> taps, int bufferSize, float decim) : output(bufferSize * 2) {
output.init(bufferSize * 2);
_in = input;
_bufferSize = bufferSize;
_tapCount = taps.size();
delayBuf = new complex_t[_tapCount];
_taps = taps;
_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 init(stream<complex_t>* input, std::vector<float>& taps, int bufferSize, float decim) {
output.init(bufferSize * 2);
_in = input;
_bufferSize = bufferSize;
_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 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<float>& taps) {
if (running) {
return;
}
_tapCount = taps.size();
delete[] _taps;
_taps = new float[_tapCount];
for (int i = 0; i < _tapCount; i++) {
_taps[i] = taps[i];
}
}
void setInput(stream<complex_t>* input) {
if (running) {
return;
}
_in = input;
}
void setDecimation(float decimation) {
if (running) {
return;
}
_decim = decimation;
}
void setBufferSize(int bufferSize) {
if (running) {
return;
}
_bufferSize = bufferSize;
}
stream<complex_t> output;
private:
static void _worker(FIRFilter* _this) {
static void _worker(DecimatingFIRFilter* _this) {
int outputSize = _this->_bufferSize / _this->_decim;
complex_t* inBuf = new complex_t[_this->_bufferSize];
complex_t* outBuf = new complex_t[_this->_bufferSize];
complex_t* outBuf = new complex_t[outputSize];
float tap = 0.0f;
int delayOff;
void* delayStart = &inBuf[_this->_bufferSize - (_this->_tapCount - 1)];
int delaySize = (_this->_tapCount - 1) * sizeof(complex_t);
int bufferSize = _this->_bufferSize;
int outBufferLength = outputSize * sizeof(complex_t);
int tapCount = _this->_tapCount;
int decim = _this->_decim;
complex_t* delayBuf = _this->delayBuf;
int id = 0;
while (true) {
_this->_in->read(inBuf, _this->_bufferSize);
for (int i = _this->_tapCount; i < _this->_bufferSize - _this->_tapCount; i++) {
outBuf[i].i = 0.0f;
outBuf[i].q = 0.0f;
}
for (int t = 0; t < _this->_tapCount; t++) {
if (_this->_in->read(inBuf, bufferSize) < 0) { break; };
memset(outBuf, 0, outBufferLength);
for (int t = 0; t < tapCount; t++) {
tap = _this->_taps[t];
if (tap == 0.0f) {
continue;
}
for (int i = 0; i < t; i++) {
outBuf[i].i += tap * _this->delayBuf[_this->_tapCount - t - 1].i;
outBuf[i].q += tap * _this->delayBuf[_this->_tapCount - t - 1].q;
}
for (int i = t; i < _this->_bufferSize; i++) {
outBuf[i].i += tap * inBuf[i - t].i;
outBuf[i].q += tap * inBuf[i - t].q;
delayOff = tapCount - t;
id = 0;
for (int i = 0; i < bufferSize; i += decim) {
if (i < t) {
outBuf[id].i += tap * delayBuf[delayOff + i].i;
outBuf[id].q += tap * delayBuf[delayOff + i].q;
id++;
continue;
}
outBuf[id].i += tap * inBuf[i - t].i;
outBuf[id].q += tap * inBuf[i - t].q;
id++;
}
}
// for (int i = _this->_tapCount; i < _this->_bufferSize - _this->_tapCount; i++) {
// outBuf[i].i /= (float)_this->_tapCount;
// outBuf[i].q /= (float)_this->_tapCount;
// }
memcpy(_this->delayBuf, &inBuf[_this->_bufferSize - _this->_tapCount], _this->_tapCount * sizeof(complex_t));
_this->output.write(outBuf, _this->_bufferSize);
memcpy(delayBuf, delayStart, delaySize);
if (_this->output.write(outBuf, outputSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<complex_t>* _in;
complex_t* delayBuf;
int _bufferSize;
int _tapCount = 0;
std::vector<float> _taps;
float _decim;
std::thread _workerThread;
float* _taps;
bool running;
};
class FloatDecimatingFIRFilter {
public:
FloatDecimatingFIRFilter() {
}
FloatDecimatingFIRFilter(stream<float>* input, std::vector<float> taps, int bufferSize, float decim) : output(bufferSize * 2) {
output.init(bufferSize * 2);
_in = input;
_bufferSize = bufferSize;
_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 init(stream<float>* input, std::vector<float>& taps, int bufferSize, float decim) {
output.init(bufferSize * 2);
_in = input;
_bufferSize = bufferSize;
_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() {
running = true;
_workerThread = std::thread(_worker, this);
}
void stop() {
_in->stopReader();
output.stopWriter();
_workerThread.join();
_in->clearReadStop();
output.clearWriteStop();
running = false;
}
void setTaps(std::vector<float>& taps) {
if (running) {
return;
}
_tapCount = taps.size();
delete[] _taps;
_taps = new float[_tapCount];
for (int i = 0; i < _tapCount; i++) {
_taps[i] = taps[i];
}
}
void setInput(stream<float>* input) {
if (running) {
return;
}
_in = input;
}
void setDecimation(float decimation) {
if (running) {
return;
}
_decim = decimation;
}
stream<float> output;
private:
static void _worker(FloatDecimatingFIRFilter* _this) {
int outputSize = _this->_bufferSize / _this->_decim;
float* inBuf = new float[_this->_bufferSize];
float* outBuf = new float[outputSize];
float tap = 0.0f;
int delayOff;
void* delayStart = &inBuf[_this->_bufferSize - (_this->_tapCount - 1)];
int delaySize = (_this->_tapCount - 1) * sizeof(float);
int bufferSize = _this->_bufferSize;
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, bufferSize) < 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 < bufferSize; 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<float>* _in;
float* delayBuf;
int _bufferSize;
int _tapCount = 0;
float _decim;
std::thread _workerThread;
float* _taps;
bool running;
};
class DCBiasRemover {
public:
@@ -173,4 +433,46 @@ namespace cdsp {
std::thread _workerThread;
void (*_handler)(complex_t*);
};
class Splitter {
public:
Splitter() {
}
Splitter(stream<complex_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
output_a.init(bufferSize);
output_b.init(bufferSize);
}
void init(stream<complex_t>* input, int bufferSize) {
_in = input;
_bufferSize = bufferSize;
output_a.init(bufferSize);
output_b.init(bufferSize);
}
void start() {
_workerThread = std::thread(_worker, this);
}
stream<complex_t> output_a;
stream<complex_t> output_b;
private:
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);
}
}
stream<complex_t>* _in;
int _bufferSize;
std::thread _workerThread;
};
};

27
src/cdsp/generator.h
View File

@@ -92,19 +92,41 @@ namespace cdsp {
ComplexSineSource(float frequency, long sampleRate, int bufferSize) : output(bufferSize * 2) {
_bufferSize = bufferSize;
_sampleRate = sampleRate;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / frequency);
_phase = 0;
running = false;
}
void init(float frequency, long sampleRate, int bufferSize) {
output.init(bufferSize * 2);
_sampleRate = sampleRate;
_bufferSize = bufferSize;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / frequency);
_phase = 0;
running = false;
}
void start() {
if (running) {
return;
}
_workerThread = std::thread(_worker, this);
running = true;
}
void stop() {
if (!running) {
return;
}
output.stopWriter();
_workerThread.join();
output.clearWriteStop();
running = false;
}
void setFrequency(float frequency) {
_phasorSpeed = (2 * 3.1415926535) / (_sampleRate / frequency);
}
stream<complex_t> output;
@@ -119,13 +141,16 @@ namespace cdsp {
outBuf[i].q = cos(_this->_phase);
}
_this->_phase = fmodf(_this->_phase, 2.0f * 3.1415926535);
_this->output.write(outBuf, _this->_bufferSize);
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { break; };
}
delete[] outBuf;
}
int _bufferSize;
float _phasorSpeed;
float _phase;
long _sampleRate;
std::thread _workerThread;
bool running;
};
};

38
src/cdsp/math.h
View File

@@ -2,6 +2,7 @@
#include <thread>
#include <cdsp/stream.h>
#include <cdsp/types.h>
#include <cdsp/fast_math.h>
namespace cdsp {
class Multiplier {
@@ -16,10 +17,35 @@ namespace cdsp {
_bufferSize = bufferSize;
}
void init(stream<complex_t>* a, stream<complex_t>* b, int bufferSize) {
output.init(bufferSize * 2);
_a = a;
_b = b;
_bufferSize = bufferSize;
}
void start() {
if (running) {
return;
}
running = true;
_workerThread = std::thread(_worker, this);
}
void stop() {
if (!running) {
return;
}
_a->stopReader();
_b->stopReader();
output.stopWriter();
_workerThread.join();
running = false;
_a->clearReadStop();
_b->clearReadStop();
output.clearWriteStop();
}
stream<complex_t> output;
private:
@@ -28,19 +54,17 @@ namespace cdsp {
complex_t* bBuf = new complex_t[_this->_bufferSize];
complex_t* outBuf = new complex_t[_this->_bufferSize];
while (true) {
_this->_a->read(aBuf, _this->_bufferSize);
_this->_b->read(bBuf, _this->_bufferSize);
for (int i = 0; i < _this->_bufferSize; i++) {
outBuf[i].i = (aBuf[i].q * bBuf[i].i) + (bBuf[i].q * aBuf[i].i); // BC + AD
outBuf[i].q = (aBuf[i].q * bBuf[i].q) - (aBuf[i].i * bBuf[i].i);
}
_this->output.write(outBuf, _this->_bufferSize);
if (_this->_a->read(aBuf, _this->_bufferSize) < 0) { printf("A Received stop signal\n"); return; };
if (_this->_b->read(bBuf, _this->_bufferSize) < 0) { printf("B Received stop signal\n"); return; };
do_mul(aBuf, bBuf, _this->_bufferSize);
if (_this->output.write(aBuf, _this->_bufferSize) < 0) { printf("OUT Received stop signal\n"); return; };
}
}
stream<complex_t>* _a;
stream<complex_t>* _b;
int _bufferSize;
bool running = false;
std::thread _workerThread;
};
};

12
src/cdsp/modulation.h
View File

@@ -7,6 +7,10 @@
namespace cdsp {
class FMModulator {
public:
FMModulator() {
}
FMModulator(stream<float>* in, float deviation, long sampleRate, int bufferSize) : output(bufferSize * 2) {
_input = in;
_bufferSize = bufferSize;
@@ -14,6 +18,14 @@ namespace cdsp {
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / deviation);
}
void init(stream<float>* in, float deviation, long sampleRate, int bufferSize) {
output.init(bufferSize * 2);
_input = in;
_bufferSize = bufferSize;
_phase = 0.0f;
_phasorSpeed = (2 * 3.1415926535) / (sampleRate / deviation);
}
void start() {
_workerThread = std::thread(_worker, this);
}

229
src/cdsp/resampling.h
View File

@@ -2,20 +2,66 @@
#include <thread>
#include <cdsp/stream.h>
#include <cdsp/types.h>
#include <cdsp/filter.h>
#include <numeric>
namespace cdsp {
class Interpolator {
public:
Interpolator() {
}
Interpolator(stream<float>* in, float interpolation, int bufferSize) : output(bufferSize * 2) {
_input = in;
_interpolation = interpolation;
_bufferSize = bufferSize;
running = false;
}
void init(stream<float>* in, float interpolation, int bufferSize) {
output.init(bufferSize * 2);
_input = in;
_interpolation = interpolation;
_bufferSize = bufferSize;
running = false;
}
void start() {
if (running) {
return;
}
running = true;
_workerThread = std::thread(_worker, this);
}
void stop() {
if (!running) {
return;
}
running = false;
_input->stopReader();
output.stopWriter();
_workerThread.join();
_input->clearReadStop();
output.clearWriteStop();
}
void setInterpolation(float interpolation) {
if (running) {
return;
}
_interpolation = interpolation;
}
void setInput(stream<float>* in) {
if (running) {
return;
}
_input = in;
}
stream<float> output;
private:
@@ -23,62 +69,116 @@ namespace cdsp {
float* inBuf = new float[(int)((float)_this->_bufferSize / _this->_interpolation)];
float* outBuf = new float[_this->_bufferSize];
while (true) {
_this->_input->read(inBuf, (int)((float)_this->_bufferSize / _this->_interpolation));
if (_this->_input->read(inBuf, (int)((float)_this->_bufferSize / _this->_interpolation)) < 0) { break; };
for (int i = 0; i < _this->_bufferSize; i++) {
outBuf[i] = inBuf[(int)((float)i / _this->_interpolation)];
}
_this->output.write(outBuf, _this->_bufferSize);
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<float>* _input;
int _bufferSize;
float _interpolation;
std::thread _workerThread;
bool running;
};
class IQInterpolator {
public:
IQInterpolator() {
}
IQInterpolator(stream<complex_t>* in, float interpolation, int bufferSize) : output(bufferSize * 2) {
_input = in;
_interpolation = interpolation;
_bufferSize = bufferSize;
running = false;
}
void init(stream<complex_t>* in, float interpolation, int bufferSize) {
output.init(bufferSize * 2);
_input = in;
_interpolation = interpolation;
_bufferSize = bufferSize;
running = false;
}
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 setInterpolation(float interpolation) {
if (running) {
return;
}
_interpolation = interpolation;
}
stream<complex_t> output;
private:
static void _worker(IQInterpolator* _this) {
complex_t* inBuf = new complex_t[(int)((float)_this->_bufferSize / _this->_interpolation)];
complex_t* outBuf = new complex_t[_this->_bufferSize];
complex_t* inBuf = new complex_t[_this->_bufferSize];
complex_t* outBuf = new complex_t[_this->_bufferSize * _this->_interpolation];
int outCount = _this->_bufferSize * _this->_interpolation;
while (true) {
_this->_input->read(inBuf, (int)((float)_this->_bufferSize / _this->_interpolation));
for (int i = 0; i < _this->_bufferSize; i++) {
if (_this->_input->read(inBuf, _this->_bufferSize) < 0) { break; };
for (int i = 0; i < outCount; i++) {
outBuf[i] = inBuf[(int)((float)i / _this->_interpolation)];
}
_this->output.write(outBuf, _this->_bufferSize);
if (_this->output.write(outBuf, _this->_bufferSize) < 0) { break; };
}
delete[] inBuf;
delete[] outBuf;
}
stream<complex_t>* _input;
int _bufferSize;
float _interpolation;
std::thread _workerThread;
bool running;
};
class BlockDecimator {
public:
BlockDecimator() {
}
BlockDecimator(stream<complex_t>* in, int skip, int bufferSize) : output(bufferSize * 2) {
_input = in;
_skip = skip;
_bufferSize = bufferSize;
}
void init(stream<complex_t>* in, int skip, int bufferSize) {
output.init(bufferSize * 2);
_input = in;
_skip = skip;
_bufferSize = bufferSize;
}
void start() {
_workerThread = std::thread(_worker, this);
}
@@ -99,4 +199,119 @@ namespace cdsp {
int _skip;
std::thread _workerThread;
};
class FractionalResampler {
public:
FractionalResampler() {
}
void init(stream<float>* input, float inputSampleRate, float outputSampleRate, int bufferSize, float customCutoff = INFINITY) {
_input = input;
float lowestFreq = std::min<float>(inputSampleRate, outputSampleRate);
int _gcd = std::gcd((int)inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
_inputSampleRate = inputSampleRate;
_outputSampleRate = outputSampleRate;
running = false;
interpolator.init(input, _interp, bufferSize);
BlackmanWindow(decimTaps, inputSampleRate * _interp, lowestFreq / 2.0f, lowestFreq / 2.0f);
if (_interp != 1) {
printf("FR Interpolation needed\n");
decimator.init(&interpolator.output, decimTaps, bufferSize * _interp, _decim);
}
else {
decimator.init(input, decimTaps, bufferSize, _decim);
printf("FR Interpolation NOT needed: %d %d %d\n", bufferSize / _decim, _decim, _interp);
}
output = &decimator.output;
}
void start() {
if (_interp != 1) {
interpolator.start();
}
decimator.start();
running = true;
}
void stop() {
interpolator.stop();
decimator.stop();
running = false;
}
void setInputSampleRate(float inputSampleRate) {
if (running) {
return;
}
float lowestFreq = std::min<float>(inputSampleRate, _outputSampleRate);
int _gcd = std::gcd((int)inputSampleRate, (int)_outputSampleRate);
_interp = _outputSampleRate / _gcd;
_decim = inputSampleRate / _gcd;
// TODO: Add checks from VFO to remove the need to stop both
interpolator.setInterpolation(_interp);
decimator.setDecimation(_decim);
if (_interp != 1) {
decimator.setInput(&interpolator.output);
}
else {
decimator.setInput(_input);
}
}
void setOutputSampleRate(float outputSampleRate) {
if (running) {
return;
}
float lowestFreq = std::min<float>(_inputSampleRate, outputSampleRate);
int _gcd = std::gcd((int)_inputSampleRate, (int)outputSampleRate);
_interp = outputSampleRate / _gcd;
_decim = _inputSampleRate / _gcd;
// TODO: Add checks from VFO to remove the need to stop both
interpolator.setInterpolation(_interp);
decimator.setDecimation(_decim);
if (_interp != 1) {
decimator.setInput(&interpolator.output);
}
else {
decimator.setInput(_input);
}
}
void setInput(stream<float>* input) {
if (running) {
return;
}
_input = input;
if (_interp != 1) {
interpolator.setInput(input);
decimator.setInput(&interpolator.output);
}
else {
decimator.setInput(input);
}
}
stream<float>* output;
private:
Interpolator interpolator;
FloatDecimatingFIRFilter decimator;
std::vector<float> decimTaps;
stream<float>* _input;
int _interp;
int _decim;
int _bufferSize;
float _inputSampleRate;
float _outputSampleRate;
bool running;
};
};

67
src/cdsp/stream.h
View File

@@ -13,6 +13,8 @@ namespace cdsp {
stream(int size) {
_buffer = new T[size];
_stopReader = false;
_stopWriter = false;
this->size = size;
writec = 0;
readc = size - 1;
@@ -20,15 +22,27 @@ namespace cdsp {
void init(int size) {
_buffer = new T[size];
_stopReader = false;
_stopWriter = false;
this->size = size;
writec = 0;
readc = size - 1;
}
void read(T* data, int len) {
int read(T* data, int len) {
int dataRead = 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);
int len1 = (toRead >= (size - readc) ? (size - readc) : (toRead));
@@ -45,10 +59,14 @@ namespace cdsp {
}
}
void readAndSkip(T* data, int len, int skip) {
int readAndSkip(T* data, int len, int skip) {
int dataRead = 0;
while (dataRead < len) {
int canRead = waitUntilReadable();
if (canRead < 0) {
clearReadStop();
return -1;
}
int toRead = std::min(canRead, len - dataRead);
int len1 = (toRead >= (size - readc) ? (size - readc) : (toRead));
@@ -85,7 +103,10 @@ namespace cdsp {
return canRead;
}
std::unique_lock<std::mutex> lck(writec_mtx);
canReadVar.wait(lck, [=](){ return (this->readable(false) > 0); });
canReadVar.wait(lck, [=](){ return ((this->readable(false) > 0) || this->getReadStop()); });
if (this->getReadStop()) {
return -1;
}
return this->readable(false);
}
@@ -100,10 +121,14 @@ namespace cdsp {
return readable - 1;
}
void write(T* data, int len) {
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 len1 = (toWrite >= (size - writec) ? (size - writec) : (toWrite));
@@ -118,6 +143,7 @@ namespace cdsp {
writec_mtx.unlock();
canReadVar.notify_one();
}
return len;
}
int waitUntilWriteable() {
@@ -126,7 +152,10 @@ namespace cdsp {
return canWrite;
}
std::unique_lock<std::mutex> lck(readc_mtx);
canWriteVar.wait(lck, [=](){ return (this->writeable(false) > 0); });
canWriteVar.wait(lck, [=](){ return ((this->writeable(false) > 0) || this->getWriteStop()); });
if (this->getWriteStop()) {
return -1;
}
return this->writeable(false);
}
@@ -141,11 +170,39 @@ namespace cdsp {
return writeable - 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;
}
private:
T* _buffer;
int size;
int readc;
int writec;
bool _stopReader;
bool _stopWriter;
std::mutex readc_mtx;
std::mutex writec_mtx;
std::condition_variable canReadVar;