kopia lustrzana https://github.com/AlexandreRouma/SDRPlusPlus
Fixed IF reduction not working with multivfo + Added beginning of IF notch code
AlexandreRouma
rodzic
f8ff67c5b0
commit
241632288e
|
|
@ -384,4 +384,85 @@ namespace dsp {
|
|||
stream<complex_t>* _in;
|
||||
|
||||
};
|
||||
|
||||
class NotchFilter : public generic_block<NotchFilter> {
|
||||
public:
|
||||
NotchFilter() {}
|
||||
|
||||
NotchFilter(stream<complex_t>* in, float rate, float offset, float sampleRate) { init(in, rate, offset, sampleRate); }
|
||||
|
||||
void init(stream<complex_t>* in, float rate, float offset, float sampleRate) {
|
||||
_in = in;
|
||||
correctionRate = rate;
|
||||
_offset = offset;
|
||||
_sampleRate = sampleRate;
|
||||
|
||||
phaseDelta = lv_cmake(std::cos((-_offset / _sampleRate) * 2.0f * FL_M_PI), std::sin((-_offset / _sampleRate) * 2.0f * FL_M_PI));
|
||||
phaseDeltaConj = {phaseDelta.real(), -phaseDelta.imag()};
|
||||
|
||||
generic_block<NotchFilter>::registerInput(_in);
|
||||
generic_block<NotchFilter>::registerOutput(&out);
|
||||
generic_block<NotchFilter>::_block_init = true;
|
||||
}
|
||||
|
||||
void setInput(stream<complex_t>* in) {
|
||||
assert(generic_block<NotchFilter>::_block_init);
|
||||
std::lock_guard<std::mutex> lck(generic_block<NotchFilter>::ctrlMtx);
|
||||
generic_block<NotchFilter>::tempStop();
|
||||
generic_block<NotchFilter>::unregisterInput(_in);
|
||||
_in = in;
|
||||
generic_block<NotchFilter>::registerInput(_in);
|
||||
generic_block<NotchFilter>::tempStart();
|
||||
}
|
||||
|
||||
void setCorrectionRate(float rate) {
|
||||
correctionRate = rate;
|
||||
}
|
||||
|
||||
void setOffset(float offset) {
|
||||
_offset = offset;
|
||||
phaseDelta = lv_cmake(std::cos((-_offset / _sampleRate) * 2.0f * FL_M_PI), std::sin((-_offset / _sampleRate) * 2.0f * FL_M_PI));
|
||||
phaseDeltaConj = {phaseDelta.real(), -phaseDelta.imag()};
|
||||
}
|
||||
|
||||
void setSampleRate(float sampleRate) {
|
||||
_sampleRate = sampleRate;
|
||||
phaseDelta = lv_cmake(std::cos((-_offset / _sampleRate) * 2.0f * FL_M_PI), std::sin((-_offset / _sampleRate) * 2.0f * FL_M_PI));
|
||||
phaseDeltaConj = {phaseDelta.real(), -phaseDelta.imag()};
|
||||
}
|
||||
|
||||
int run() {
|
||||
int count = _in->read();
|
||||
if (count < 0) { return -1; }
|
||||
|
||||
volk_32fc_s32fc_x2_rotator_32fc((lv_32fc_t*)_in->readBuf, (lv_32fc_t*)_in->readBuf, phaseDelta, &inPhase, count);
|
||||
|
||||
for (int i = 0; i < count; i++) {
|
||||
out.writeBuf[i] = _in->readBuf[i] - offset;
|
||||
offset = offset + (out.writeBuf[i] * correctionRate);
|
||||
}
|
||||
|
||||
volk_32fc_s32fc_x2_rotator_32fc((lv_32fc_t*)out.writeBuf, (lv_32fc_t*)out.writeBuf, phaseDeltaConj, &outPhase, count);
|
||||
|
||||
_in->flush();
|
||||
|
||||
if (!out.swap(count)) { return -1; }
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
stream<complex_t> out;
|
||||
|
||||
private:
|
||||
stream<complex_t>* _in;
|
||||
complex_t offset = {0, 0};
|
||||
lv_32fc_t inPhase = {1, 0};
|
||||
lv_32fc_t outPhase = {4, 0};
|
||||
lv_32fc_t phaseDelta;
|
||||
lv_32fc_t phaseDeltaConj;
|
||||
float _offset;
|
||||
float _sampleRate;
|
||||
float correctionRate;
|
||||
|
||||
};
|
||||
}
|
||||
|
|
@ -271,37 +271,125 @@ namespace dsp {
|
|||
|
||||
};
|
||||
|
||||
class NotchWindow : public filter_window::generic_window {
|
||||
// class NotchWindow : public filter_window::generic_complex_window {
|
||||
// public:
|
||||
// NotchWindow() {}
|
||||
|
||||
// NotchWindow(float frequency, float width, float sampleRate, int tapCount) { init(frequency, width, sampleRate, tapCount); }
|
||||
|
||||
// ~NotchWindow() {
|
||||
// if (fft_in) { fftwf_free(fft_in); }
|
||||
// if (fft_out) { fftwf_free(fft_out); }
|
||||
// fftwf_destroy_plan(fft_plan);
|
||||
// }
|
||||
|
||||
// void init(float frequency, float width, float sampleRate, int tapCount) {
|
||||
// _frequency = frequency;
|
||||
// _width = width;
|
||||
// _sampleRate = sampleRate;
|
||||
// _tapCount = tapCount;
|
||||
|
||||
// // Ensure the number of taps is even
|
||||
// if (_tapCount & 1) { _tapCount++; }
|
||||
|
||||
// fft_in = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
|
||||
// fft_out = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
|
||||
|
||||
// fft_plan = fftwf_plan_dft_1d(_tapCount, (fftwf_complex*)fft_in, (fftwf_complex*)fft_out, FFTW_BACKWARD, FFTW_ESTIMATE);
|
||||
// }
|
||||
|
||||
// void setFrequency(float frequency) {
|
||||
// _frequency = frequency;
|
||||
// }
|
||||
|
||||
// void setWidth(float width) {
|
||||
// _width = width;
|
||||
// }
|
||||
|
||||
// void setSampleRate(float sampleRate) {
|
||||
// _sampleRate = sampleRate;
|
||||
// }
|
||||
|
||||
// void setTapCount(int count) {
|
||||
// _tapCount = count;
|
||||
|
||||
// // Ensure the number of taps is even
|
||||
|
||||
// // Free buffers
|
||||
// if (fft_in) { fftwf_free(fft_in); }
|
||||
// if (fft_out) { fftwf_free(fft_out); }
|
||||
// fftwf_destroy_plan(fft_plan);
|
||||
|
||||
// // Reallocate
|
||||
// fft_in = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
|
||||
// fft_out = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
|
||||
|
||||
// // Create new plan
|
||||
// fft_plan = fftwf_plan_dft_1d(_tapCount, (fftwf_complex*)fft_in, (fftwf_complex*)fft_out, FFTW_BACKWARD, FFTW_ESTIMATE);
|
||||
// }
|
||||
|
||||
// int getTapCount() {
|
||||
// return _tapCount;
|
||||
// }
|
||||
|
||||
// void createTaps(complex_t* taps, int tapCount, float factor = 1.0f) {
|
||||
// float ratio = _sampleRate / (float)tapCount;
|
||||
// int thalf = tapCount / 2;
|
||||
// float start = _frequency - (_width / 2.0f);
|
||||
// float stop = _frequency + (_width / 2.0f);
|
||||
|
||||
// // Fill taps
|
||||
// float freq;
|
||||
// float pratio = 2.0f * FL_M_PI / (float)tapCount;
|
||||
// complex_t phaseDiff = {cosf(pratio), -sinf(pratio)};
|
||||
// complex_t phasor = {1, 0};
|
||||
// for (int i = 0; i < tapCount; i++) {
|
||||
// freq = (i < thalf) ? ((float)i * ratio) : -((float)(tapCount - i) * ratio);
|
||||
// if (freq >= start && freq <= stop) {
|
||||
// fft_in[i] = {0, 0};
|
||||
// }
|
||||
// else {
|
||||
// fft_in[i] = phasor;
|
||||
// }
|
||||
// phasor = phasor * phaseDiff;
|
||||
// }
|
||||
|
||||
// // Run IFFT
|
||||
// fftwf_execute(fft_plan);
|
||||
|
||||
// // Apply window and copy to output
|
||||
// for (int i = 0; i < tapCount; i++) {
|
||||
// taps[tapCount - i - 1] = fft_out[i] / (float)tapCount;
|
||||
// }
|
||||
// }
|
||||
|
||||
// private:
|
||||
// complex_t* fft_in = NULL;
|
||||
// complex_t* fft_out = NULL;
|
||||
// float _frequency, _width, _sampleRate;
|
||||
// int _tapCount;
|
||||
|
||||
// fftwf_plan fft_plan;
|
||||
|
||||
// };
|
||||
|
||||
class NotchWindow : public filter_window::generic_complex_window {
|
||||
public:
|
||||
NotchWindow() {}
|
||||
|
||||
NotchWindow(float frequency, float width, float sampleRate, int tapCount) { init(frequency, width, sampleRate, tapCount); }
|
||||
|
||||
~NotchWindow() {
|
||||
if (fft_in) { fftwf_free(fft_in); }
|
||||
if (fft_out) { fftwf_free(fft_out); }
|
||||
fftwf_destroy_plan(fft_plan);
|
||||
}
|
||||
|
||||
void init(float frequency, float width, float sampleRate, int tapCount) {
|
||||
_frequency = frequency;
|
||||
_width = width;
|
||||
_sampleRate = sampleRate;
|
||||
_tapCount = _tapCount;
|
||||
|
||||
fft_in = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
|
||||
fft_out = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
|
||||
|
||||
fft_plan = fftwf_plan_dft_1d(_tapCount, (fftwf_complex*)fft_in, (fftwf_complex*)fft_out, FFTW_BACKWARD, FFTW_ESTIMATE);
|
||||
_tapCount = tapCount;
|
||||
}
|
||||
|
||||
void setFrequency(float frequency) {
|
||||
_frequency = frequency;
|
||||
}
|
||||
|
||||
void setWidth(float width) {
|
||||
_width = width;
|
||||
}
|
||||
void setWidth(float width) {}
|
||||
|
||||
void setSampleRate(float sampleRate) {
|
||||
_sampleRate = sampleRate;
|
||||
|
|
@ -309,31 +397,28 @@ namespace dsp {
|
|||
|
||||
void setTapCount(int count) {
|
||||
_tapCount = count;
|
||||
if (fft_in) { fftwf_free(fft_in); }
|
||||
if (fft_out) { fftwf_free(fft_out); }
|
||||
fftwf_destroy_plan(fft_plan);
|
||||
|
||||
fft_in = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
|
||||
fft_out = (complex_t*)fftwf_malloc(_tapCount * sizeof(complex_t));
|
||||
|
||||
fft_plan = fftwf_plan_dft_1d(_tapCount, (fftwf_complex*)fft_in, (fftwf_complex*)fft_out, FFTW_BACKWARD, FFTW_ESTIMATE);
|
||||
}
|
||||
|
||||
int getTapCount() {
|
||||
return _tapCount;
|
||||
}
|
||||
|
||||
void createTaps(float* taps, int tapCount, float factor = 1.0f) {
|
||||
|
||||
void createTaps(complex_t* taps, int tapCount, float factor = 1.0f) {
|
||||
// Generate exponential decay
|
||||
float fact = 1.0f / (float)tapCount;
|
||||
for (int i = 0; i < tapCount; i++) {
|
||||
taps[tapCount - i - 1] = {expf(-fact*i) * (float)window_function::blackman(i, tapCount - 1), 0};
|
||||
}
|
||||
|
||||
// Frequency translate it to the right place
|
||||
lv_32fc_t phase = lv_cmake(1.0f, 0.0f);
|
||||
lv_32fc_t phaseDelta = lv_cmake(std::cos((-_frequency / _sampleRate) * 2.0f * FL_M_PI), std::sin((-_frequency / _sampleRate) * 2.0f * FL_M_PI));
|
||||
volk_32fc_s32fc_x2_rotator_32fc((lv_32fc_t*)taps, (lv_32fc_t*)taps, phaseDelta, &phase, tapCount);
|
||||
}
|
||||
|
||||
private:
|
||||
complex_t* fft_in = NULL;
|
||||
complex_t* fft_out = NULL;
|
||||
float _frequency, _width, _sampleRate;
|
||||
float _frequency, _sampleRate;
|
||||
int _tapCount;
|
||||
|
||||
fftwf_plan fft_plan;
|
||||
|
||||
};
|
||||
}
|
||||
|
|
@ -60,10 +60,12 @@ public:
|
|||
ifChain.init(vfo->output, &ifChainOutputChanged);
|
||||
|
||||
fmnr.block.init(NULL, 32);
|
||||
notch.block.init(NULL, 0.5, 0, 250000); // TODO: The rate has to depend on IF sample rate so the width is always the same
|
||||
squelch.block.init(NULL, MIN_SQUELCH);
|
||||
|
||||
ifChain.add(&fmnr);
|
||||
ifChain.add(¬ch);
|
||||
ifChain.add(&squelch);
|
||||
ifChain.add(&fmnr);
|
||||
|
||||
// Load configuration for and enabled all demodulators
|
||||
EventHandler<dsp::stream<dsp::stereo_t>*> _demodOutputChangeHandler;
|
||||
|
|
@ -256,9 +258,20 @@ private:
|
|||
}
|
||||
if (!_this->squelchEnabled && _this->enabled) { style::endDisabled(); }
|
||||
|
||||
// // Notch filter
|
||||
// if (ImGui::Checkbox("Notch##_radio_notch_ena_", &_this->notchEnabled)) {
|
||||
// _this->ifChain.setState(&_this->notch, _this->notchEnabled);
|
||||
// }
|
||||
// if (ImGui::SliderFloat(("NF##_radio_notch_freq_" + _this->name).c_str(), &_this->notchPos, -7500, 7500)) {
|
||||
// _this->notch.block.setOffset(_this->notchPos);
|
||||
// }
|
||||
// if (ImGui::SliderFloat(("NW##_radio_notch_width_" + _this->name).c_str(), &_this->notchWidth, 0, 1000)) {
|
||||
// // TODO: Implement
|
||||
// }
|
||||
|
||||
// FM IF Noise Reduction
|
||||
if (_this->FMIFNRAllowed) {
|
||||
if (ImGui::Checkbox("IF Noise Reduction##_radio_fmifnr_ena_", &_this->FMIFNREnabled)) {
|
||||
if (ImGui::Checkbox(("IF Noise Reduction##_radio_fmifnr_ena_" + _this->name).c_str(), &_this->FMIFNREnabled)) {
|
||||
_this->setFMIFNREnabled(_this->FMIFNREnabled);
|
||||
}
|
||||
}
|
||||
|
|
@ -343,6 +356,9 @@ private:
|
|||
// Configure FM IF Noise Reduction
|
||||
setFMIFNREnabled(FMIFNRAllowed ? FMIFNREnabled : false);
|
||||
|
||||
// Configure notch
|
||||
notch.block.setSampleRate(selectedDemod->getIFSampleRate());
|
||||
|
||||
// Configure squelch
|
||||
squelch.block.setLevel(squelchLevel);
|
||||
setSquelchEnabled(squelchEnabled);
|
||||
|
|
@ -550,6 +566,7 @@ private:
|
|||
// IF chain
|
||||
dsp::Chain<dsp::complex_t> ifChain;
|
||||
dsp::ChainLink<dsp::FMIFNoiseReduction, dsp::complex_t> fmnr;
|
||||
dsp::ChainLink<dsp::NotchFilter, dsp::complex_t> notch;
|
||||
dsp::ChainLink<dsp::Squelch, dsp::complex_t> squelch;
|
||||
|
||||
// Audio chain
|
||||
|
|
@ -579,6 +596,10 @@ private:
|
|||
bool FMIFNRAllowed;
|
||||
bool FMIFNREnabled = false;
|
||||
|
||||
bool notchEnabled = false;
|
||||
float notchPos = 0;
|
||||
float notchWidth = 500;
|
||||
|
||||
const double MIN_SQUELCH = -100.0;
|
||||
const double MAX_SQUELCH = 0.0;
|
||||
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue