kopia lustrzana https://github.com/AlexandreRouma/SDRPlusPlus
bugfix + preparations for stereo FM
Ryzerth
rodzic
2baf607b8c
commit
175e361ccd
|
|
@ -52,6 +52,8 @@ namespace dsp {
|
|||
void init(stream<float>* in_left, stream<float>* in_right) {
|
||||
_in_left = in_left;
|
||||
_in_right = in_right;
|
||||
nullbuf = new float[STREAM_BUFFER_SIZE];
|
||||
for (int i = 0; i < STREAM_BUFFER_SIZE; i++) { nullbuf[i] = 0; }
|
||||
generic_block<ChannelsToStereo>::registerInput(_in_left);
|
||||
generic_block<ChannelsToStereo>::registerInput(_in_right);
|
||||
generic_block<ChannelsToStereo>::registerOutput(&out);
|
||||
|
|
@ -95,6 +97,8 @@ namespace dsp {
|
|||
stream<float>* _in_left;
|
||||
stream<float>* _in_right;
|
||||
|
||||
float* nullbuf;
|
||||
|
||||
};
|
||||
|
||||
class StereoToMono : public generic_block<StereoToMono> {
|
||||
|
|
|
|||
|
|
@ -8,6 +8,8 @@
|
|||
#include <dsp/pll.h>
|
||||
#include <dsp/clock_recovery.h>
|
||||
#include <dsp/math.h>
|
||||
#include <dsp/convertion.h>
|
||||
#include <dsp/audio.h>
|
||||
|
||||
#define FAST_ATAN2_COEF1 FL_M_PI / 4.0f
|
||||
#define FAST_ATAN2_COEF2 3.0f * FAST_ATAN2_COEF1
|
||||
|
|
@ -663,4 +665,119 @@ namespace dsp {
|
|||
float _muGain;
|
||||
float _omegaRelLimit;
|
||||
};
|
||||
|
||||
class StereoFMDemod : public generic_hier_block<StereoFMDemod> {
|
||||
public:
|
||||
StereoFMDemod() {}
|
||||
|
||||
StereoFMDemod(stream<float>* input, float sampleRate, float baudRate, float agcRate = 0.02e-3f, float pllLoopBandwidth = (0.06f*0.06f) / 4.0f, int rrcTapCount = 31, float rrcAlpha = 0.6f, float omegaGain = (0.01*0.01) / 4, float muGain = 0.01f, float omegaRelLimit = 0.005f) {
|
||||
init(input, sampleRate);
|
||||
}
|
||||
|
||||
void init(stream<float>* input, float sampleRate) {
|
||||
_sampleRate = sampleRate;
|
||||
|
||||
r2c.init(input);
|
||||
split.init(&r2c.out);
|
||||
|
||||
split.bindStream(&APlusBIn);
|
||||
split.bindStream(&AMinusBIn);
|
||||
split.bindStream(&PilotIn);
|
||||
|
||||
APlusBWin.init(0, 17000, 2500, _sampleRate);
|
||||
AMinusBWin.init(38000, 38000 + 17000, 2500, _sampleRate);
|
||||
PilotWin.init(18500, 19500, 1500, _sampleRate);
|
||||
|
||||
APlusBFir.init(&APlusBIn, &APlusBWin);
|
||||
AMinusBFir.init(&AMinusBIn, &AMinusBWin);
|
||||
PilotFir.init(&PilotIn, &PilotWin);
|
||||
|
||||
pll.init(&PilotFir.out, 0.1f);
|
||||
|
||||
p2s.init(&pll.out);
|
||||
|
||||
mixer.init(&AMinusBFir.out, &p2s.out);
|
||||
|
||||
c2rAPlusB.init(&APlusBFir.out);
|
||||
c2rAMinusB.init(&mixer.out);
|
||||
|
||||
APlusBSplit.init(&c2rAPlusB.out);
|
||||
AMinusBSplit.init(&c2rAMinusB.out);
|
||||
|
||||
APlusBSplit.bindStream(&AdderAPlusB);
|
||||
APlusBSplit.bindStream(&SubtractorAPlusB);
|
||||
AMinusBSplit.bindStream(&AdderAMinusB);
|
||||
AMinusBSplit.bindStream(&SubtractorAMinusB);
|
||||
|
||||
Adder.init(&AdderAPlusB, &AdderAMinusB);
|
||||
Subtractor.init(&SubtractorAPlusB, &SubtractorAMinusB);
|
||||
|
||||
c2s.init(&Adder.out, &Subtractor.out);
|
||||
|
||||
out = &c2s.out;
|
||||
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&r2c);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&split);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&APlusBFir);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&AMinusBFir);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&PilotFir);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&pll);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&p2s);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&mixer);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&c2rAPlusB);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&c2rAMinusB);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&APlusBSplit);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&AMinusBSplit);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&Adder);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&Subtractor);
|
||||
generic_hier_block<StereoFMDemod>::registerBlock(&c2s);
|
||||
generic_hier_block<StereoFMDemod>::_block_init = true;
|
||||
}
|
||||
|
||||
void setInput(stream<float>* input) {
|
||||
assert(generic_hier_block<StereoFMDemod>::_block_init);
|
||||
r2c.setInput(input);
|
||||
}
|
||||
|
||||
stream<stereo_t>* out = NULL;
|
||||
|
||||
private:
|
||||
filter_window::BandPassBlackmanWindow APlusBWin;
|
||||
filter_window::BandPassBlackmanWindow AMinusBWin;
|
||||
filter_window::BandPassBlackmanWindow PilotWin;
|
||||
|
||||
RealToComplex r2c;
|
||||
Splitter<complex_t> split;
|
||||
|
||||
stream<complex_t> APlusBIn;
|
||||
stream<complex_t> AMinusBIn;
|
||||
stream<complex_t> PilotIn;
|
||||
ComplexFIR APlusBFir;
|
||||
ComplexFIR AMinusBFir;
|
||||
ComplexFIR PilotFir;
|
||||
|
||||
PLL pll;
|
||||
|
||||
BFMPilotToStereo p2s;
|
||||
|
||||
Multiply<complex_t> mixer;
|
||||
|
||||
ComplexToReal c2rAPlusB;
|
||||
ComplexToReal c2rAMinusB;
|
||||
|
||||
Splitter<float> APlusBSplit;
|
||||
Splitter<float> AMinusBSplit;
|
||||
|
||||
stream<float> AdderAPlusB;
|
||||
stream<float> AdderAMinusB;
|
||||
stream<float> SubtractorAPlusB;
|
||||
stream<float> SubtractorAMinusB;
|
||||
|
||||
Add<float> Adder;
|
||||
Add<float> Subtractor;
|
||||
|
||||
ChannelsToStereo c2s;
|
||||
|
||||
float _sampleRate;
|
||||
};
|
||||
}
|
||||
|
|
@ -98,6 +98,91 @@ namespace dsp {
|
|||
|
||||
};
|
||||
|
||||
class ComplexFIR : public generic_block<ComplexFIR> {
|
||||
public:
|
||||
ComplexFIR() {}
|
||||
|
||||
ComplexFIR(stream<complex_t>* in, dsp::filter_window::generic_complex_window* window) { init(in, window); }
|
||||
|
||||
~ComplexFIR() {
|
||||
if (!generic_block<ComplexFIR>::_block_init) { return; }
|
||||
generic_block<ComplexFIR>::stop();
|
||||
volk_free(buffer);
|
||||
volk_free(taps);
|
||||
generic_block<ComplexFIR>::_block_init = false;
|
||||
}
|
||||
|
||||
void init(stream<complex_t>* in, dsp::filter_window::generic_complex_window* window) {
|
||||
_in = in;
|
||||
|
||||
tapCount = window->getTapCount();
|
||||
taps = (complex_t*)volk_malloc(tapCount * sizeof(complex_t), volk_get_alignment());
|
||||
window->createTaps(taps, tapCount);
|
||||
|
||||
buffer = (complex_t*)volk_malloc(STREAM_BUFFER_SIZE * sizeof(complex_t) * 2, volk_get_alignment());
|
||||
bufStart = &buffer[tapCount];
|
||||
generic_block<ComplexFIR>::registerInput(_in);
|
||||
generic_block<ComplexFIR>::registerOutput(&out);
|
||||
generic_block<ComplexFIR>::_block_init = true;
|
||||
}
|
||||
|
||||
void setInput(stream<complex_t>* in) {
|
||||
assert(generic_block<ComplexFIR>::_block_init);
|
||||
std::lock_guard<std::mutex> lck(generic_block<ComplexFIR>::ctrlMtx);
|
||||
generic_block<ComplexFIR>::tempStop();
|
||||
generic_block<ComplexFIR>::unregisterInput(_in);
|
||||
_in = in;
|
||||
generic_block<ComplexFIR>::registerInput(_in);
|
||||
generic_block<ComplexFIR>::tempStart();
|
||||
}
|
||||
|
||||
void updateWindow(dsp::filter_window::generic_complex_window* window) {
|
||||
assert(generic_block<ComplexFIR>::_block_init);
|
||||
std::lock_guard<std::mutex> lck(generic_block<ComplexFIR>::ctrlMtx);
|
||||
_window = window;
|
||||
volk_free(taps);
|
||||
tapCount = window->getTapCount();
|
||||
taps = (complex_t*)volk_malloc(tapCount * sizeof(complex_t), volk_get_alignment());
|
||||
bufStart = &buffer[tapCount];
|
||||
window->createTaps(taps, tapCount);
|
||||
}
|
||||
|
||||
int run() {
|
||||
int count = _in->read();
|
||||
if (count < 0) { return -1; }
|
||||
|
||||
generic_block<ComplexFIR>::ctrlMtx.lock();
|
||||
|
||||
memcpy(bufStart, _in->readBuf, count * sizeof(complex_t));
|
||||
_in->flush();
|
||||
|
||||
for (int i = 0; i < count; i++) {
|
||||
volk_32fc_x2_dot_prod_32fc((lv_32fc_t*)&out.writeBuf[i], (lv_32fc_t*)&buffer[i+1], (lv_32fc_t*)taps, tapCount);
|
||||
}
|
||||
|
||||
if (!out.swap(count)) { return -1; }
|
||||
|
||||
memmove(buffer, &buffer[count], tapCount * sizeof(complex_t));
|
||||
|
||||
generic_block<ComplexFIR>::ctrlMtx.unlock();
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
stream<complex_t> out;
|
||||
|
||||
private:
|
||||
stream<complex_t>* _in;
|
||||
|
||||
dsp::filter_window::generic_complex_window* _window;
|
||||
|
||||
complex_t* bufStart;
|
||||
complex_t* buffer;
|
||||
int tapCount;
|
||||
complex_t* taps;
|
||||
|
||||
};
|
||||
|
||||
class BFMDeemp : public generic_block<BFMDeemp> {
|
||||
public:
|
||||
BFMDeemp() {}
|
||||
|
|
|
|||
|
|
@ -175,10 +175,10 @@ namespace dsp {
|
|||
void init(stream<T>* a, stream<T>* b) {
|
||||
_a = a;
|
||||
_b = b;
|
||||
generic_block<Multiply>::registerInput(a);
|
||||
generic_block<Multiply>::registerInput(b);
|
||||
generic_block<Multiply>::registerOutput(&out);
|
||||
generic_block<Multiply>::_block_init = true;
|
||||
generic_block<Multiply<T>>::registerInput(a);
|
||||
generic_block<Multiply<T>>::registerInput(b);
|
||||
generic_block<Multiply<T>>::registerOutput(&out);
|
||||
generic_block<Multiply<T>>::_block_init = true;
|
||||
}
|
||||
|
||||
void setInputs(stream<T>* a, stream<T>* b) {
|
||||
|
|
|
|||
|
|
@ -323,7 +323,7 @@ namespace dsp {
|
|||
|
||||
float _alpha; // Integral coefficient
|
||||
float _beta; // Proportional coefficient
|
||||
float vcoFrequency = 0.0f;
|
||||
float vcoFrequency = ((19000.0f / 250000.0f) * 2.0f * FL_M_PI);
|
||||
float vcoPhase = 0.0f;
|
||||
complex_t lastVCO;
|
||||
|
||||
|
|
|
|||
|
|
@ -559,4 +559,57 @@ namespace dsp {
|
|||
stream<float>* _in;
|
||||
|
||||
};
|
||||
|
||||
class BFMPilotToStereo : public generic_block<BFMPilotToStereo> {
|
||||
public:
|
||||
BFMPilotToStereo() {}
|
||||
|
||||
BFMPilotToStereo(stream<complex_t>* in) { init(in); }
|
||||
|
||||
~BFMPilotToStereo() {
|
||||
generic_block<BFMPilotToStereo>::stop();
|
||||
delete[] buffer;
|
||||
}
|
||||
|
||||
void init(stream<complex_t>* in) {
|
||||
_in = in;
|
||||
|
||||
buffer = new complex_t[STREAM_BUFFER_SIZE];
|
||||
|
||||
generic_block<BFMPilotToStereo>::registerInput(_in);
|
||||
generic_block<BFMPilotToStereo>::registerOutput(&out);
|
||||
generic_block<BFMPilotToStereo>::_block_init = true;
|
||||
}
|
||||
|
||||
void setInputs(stream<complex_t>* in) {
|
||||
assert(generic_block<BFMPilotToStereo>::_block_init);
|
||||
std::lock_guard<std::mutex> lck(generic_block<BFMPilotToStereo>::ctrlMtx);
|
||||
generic_block<BFMPilotToStereo>::tempStop();
|
||||
generic_block<BFMPilotToStereo>::unregisterInput(_in);
|
||||
_in = in;
|
||||
generic_block<BFMPilotToStereo>::registerInput(_in);
|
||||
generic_block<BFMPilotToStereo>::tempStart();
|
||||
}
|
||||
|
||||
int run() {
|
||||
int count = _in->read();
|
||||
if (count < 0) { return -1; }
|
||||
|
||||
volk_32fc_x2_multiply_32fc((lv_32fc_t*)buffer, (lv_32fc_t*)_in->readBuf, (lv_32fc_t*)_in->readBuf, count);
|
||||
_in->flush();
|
||||
|
||||
volk_32fc_conjugate_32fc((lv_32fc_t*)out.writeBuf, (lv_32fc_t*)buffer, count);
|
||||
|
||||
if (!out.swap(count)) { return -1; }
|
||||
return count;
|
||||
}
|
||||
|
||||
stream<complex_t> out;
|
||||
|
||||
private:
|
||||
stream<complex_t>* _in;
|
||||
|
||||
complex_t* buffer;
|
||||
|
||||
};
|
||||
}
|
||||
|
|
@ -11,6 +11,12 @@ namespace dsp {
|
|||
virtual void createTaps(float* taps, int tapCount, float factor = 1.0f) {}
|
||||
};
|
||||
|
||||
class generic_complex_window {
|
||||
public:
|
||||
virtual int getTapCount() { return -1; }
|
||||
virtual void createTaps(dsp::complex_t* taps, int tapCount, float factor = 1.0f) {}
|
||||
};
|
||||
|
||||
class BlackmanWindow : public filter_window::generic_window {
|
||||
public:
|
||||
BlackmanWindow() {}
|
||||
|
|
@ -76,6 +82,108 @@ namespace dsp {
|
|||
float _cutoff, _transWidth, _sampleRate;
|
||||
|
||||
};
|
||||
|
||||
class BandPassBlackmanWindow : public filter_window::generic_complex_window {
|
||||
public:
|
||||
BandPassBlackmanWindow() {}
|
||||
BandPassBlackmanWindow(float lowCutoff, float highCutoff, float transWidth, float sampleRate) { init(lowCutoff, highCutoff, transWidth, sampleRate); }
|
||||
|
||||
void init(float lowCutoff, float highCutoff, float transWidth, float sampleRate) {
|
||||
assert(lowCutoff <= highCutoff);
|
||||
_lowCutoff = lowCutoff;
|
||||
_highCutoff = highCutoff;
|
||||
_transWidth = transWidth;
|
||||
_sampleRate = sampleRate;
|
||||
|
||||
// Calculate other values
|
||||
_offset = (_lowCutoff + _highCutoff) / 2.0f;
|
||||
_cutoff = fabs((_highCutoff - _lowCutoff) / 2.0f);
|
||||
}
|
||||
|
||||
void setSampleRate(float sampleRate) {
|
||||
_sampleRate = sampleRate;
|
||||
}
|
||||
|
||||
void setCutoffs(float lowCutoff, float highCutoff) {
|
||||
assert(lowCutoff <= highCutoff);
|
||||
_lowCutoff = lowCutoff;
|
||||
_highCutoff = highCutoff;
|
||||
|
||||
// Calculate other values
|
||||
_offset = (_lowCutoff + _highCutoff) / 2.0f;
|
||||
_cutoff = fabs((_highCutoff - _lowCutoff) / 2.0f);
|
||||
}
|
||||
|
||||
void setLowCutoff(float lowCutoff) {
|
||||
assert(lowCutoff <= _highCutoff);
|
||||
_lowCutoff = lowCutoff;
|
||||
|
||||
// Calculate other values
|
||||
_offset = (_lowCutoff + _highCutoff) / 2.0f;
|
||||
_cutoff = fabs((_highCutoff - _lowCutoff) / 2.0f);
|
||||
}
|
||||
|
||||
void setHighCutoff(float highCutoff) {
|
||||
assert(_lowCutoff <= highCutoff);
|
||||
_highCutoff = highCutoff;
|
||||
|
||||
// Calculate other values
|
||||
_offset = (_lowCutoff + _highCutoff) / 2.0f;
|
||||
_cutoff = fabs((_highCutoff - _lowCutoff) / 2.0f);
|
||||
}
|
||||
|
||||
void setTransWidth(float transWidth) {
|
||||
_transWidth = transWidth;
|
||||
}
|
||||
|
||||
int getTapCount() {
|
||||
float fc = _cutoff / _sampleRate;
|
||||
if (fc > 1.0f) {
|
||||
fc = 1.0f;
|
||||
}
|
||||
|
||||
int _M = 4.0f / (_transWidth / _sampleRate);
|
||||
if (_M < 4) {
|
||||
_M = 4;
|
||||
}
|
||||
|
||||
if (_M % 2 == 0) { _M++; }
|
||||
|
||||
return _M;
|
||||
}
|
||||
|
||||
void createTaps(dsp::complex_t* taps, int tapCount, float factor = 1.0f) {
|
||||
// Calculate cuttoff frequency
|
||||
float omega = 2.0f * FL_M_PI * (_cutoff / _sampleRate);
|
||||
if (omega > FL_M_PI) { omega = FL_M_PI; }
|
||||
|
||||
// Generate taps
|
||||
float val;
|
||||
float sum = 0.0f;
|
||||
float tc = tapCount;
|
||||
for (int i = 0; i < tapCount; i++) {
|
||||
val = math::sinc(omega, (float)i - (tc/2), FL_M_PI) * window_function::blackman(i, tc - 1);
|
||||
taps[i].re = val;
|
||||
taps[i].im = 0;
|
||||
sum += val;
|
||||
}
|
||||
|
||||
// Normalize taps and multiply by supplied factor
|
||||
for (int i = 0; i < tapCount; i++) {
|
||||
taps[i] = taps[i] * factor;
|
||||
taps[i] = taps[i] / sum;
|
||||
}
|
||||
|
||||
// Add offset
|
||||
lv_32fc_t phase = lv_cmake(1.0f, 0.0f);
|
||||
lv_32fc_t phaseDelta = lv_cmake(std::cos((-_offset / _sampleRate) * 2.0f * FL_M_PI), std::sin((-_offset / _sampleRate) * 2.0f * FL_M_PI));
|
||||
volk_32fc_s32fc_x2_rotator_32fc((lv_32fc_t*)taps, (lv_32fc_t*)taps, phaseDelta, &phase, tapCount);
|
||||
}
|
||||
|
||||
private:
|
||||
float _lowCutoff, _highCutoff;
|
||||
float _cutoff, _transWidth, _sampleRate, _offset;
|
||||
};
|
||||
}
|
||||
|
||||
class RRCTaps : public filter_window::generic_window {
|
||||
|
|
|
|||
|
|
@ -69,16 +69,16 @@ private:
|
|||
tuner::tune(tuner::TUNER_MODE_IQ_ONLY, "", _this->centerFreq);
|
||||
sigpath::signalPath.setBuffering(false);
|
||||
gui::waterfall.centerFrequencyLocked = true;
|
||||
gui::freqSelect.minFreq = _this->centerFreq - (_this->sampleRate/2);
|
||||
gui::freqSelect.maxFreq = _this->centerFreq + (_this->sampleRate/2);
|
||||
gui::freqSelect.limitFreq = true;
|
||||
//gui::freqSelect.minFreq = _this->centerFreq - (_this->sampleRate/2);
|
||||
//gui::freqSelect.maxFreq = _this->centerFreq + (_this->sampleRate/2);
|
||||
//gui::freqSelect.limitFreq = true;
|
||||
spdlog::info("FileSourceModule '{0}': Menu Select!", _this->name);
|
||||
}
|
||||
|
||||
static void menuDeselected(void* ctx) {
|
||||
FileSourceModule* _this = (FileSourceModule*)ctx;
|
||||
sigpath::signalPath.setBuffering(true);
|
||||
gui::waterfall.centerFrequencyLocked = false;
|
||||
//gui::waterfall.centerFrequencyLocked = false;
|
||||
spdlog::info("FileSourceModule '{0}': Menu Deselect!", _this->name);
|
||||
}
|
||||
|
||||
|
|
@ -124,9 +124,9 @@ private:
|
|||
std::string filename = std::filesystem::path(_this->fileSelect.path).filename().string();
|
||||
_this->centerFreq = _this->getFrequency(filename);
|
||||
tuner::tune(tuner::TUNER_MODE_IQ_ONLY, "", _this->centerFreq);
|
||||
gui::freqSelect.minFreq = _this->centerFreq - (_this->sampleRate/2);
|
||||
gui::freqSelect.maxFreq = _this->centerFreq + (_this->sampleRate/2);
|
||||
gui::freqSelect.limitFreq = true;
|
||||
//gui::freqSelect.minFreq = _this->centerFreq - (_this->sampleRate/2);
|
||||
//gui::freqSelect.maxFreq = _this->centerFreq + (_this->sampleRate/2);
|
||||
//gui::freqSelect.limitFreq = true;
|
||||
}
|
||||
catch (std::exception e) {
|
||||
spdlog::error("Error: {0}", e.what());
|
||||
|
|
@ -186,10 +186,10 @@ private:
|
|||
WavReader* reader = NULL;
|
||||
bool running = false;
|
||||
bool enabled = true;
|
||||
float sampleRate = 48000;
|
||||
float sampleRate = 1000000;
|
||||
std::thread workerThread;
|
||||
|
||||
double centerFreq = 0;
|
||||
double centerFreq = 100000000;
|
||||
|
||||
bool float32Mode = false;
|
||||
};
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue