diff --git a/plugins/channelmimo/interferometer/interferometer.cpp b/plugins/channelmimo/interferometer/interferometer.cpp index dc264aa12..bd2470eb1 100644 --- a/plugins/channelmimo/interferometer/interferometer.cpp +++ b/plugins/channelmimo/interferometer/interferometer.cpp @@ -98,15 +98,6 @@ void Interferometer::stop() void Interferometer::feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end, unsigned int sinkIndex) { - // if (sinkIndex == 0) { - // m_count0 = end - begin; - // } else if (sinkIndex == 1) { - // m_count1 = end - begin; - // if (m_count1 != m_count0) { - // qDebug("Interferometer::feed: c0: %d 1: %d", m_count0, m_count1); - // } - // } - m_sink->feed(begin, end, sinkIndex); } diff --git a/plugins/channelmimo/interferometer/interferometercorr.cpp b/plugins/channelmimo/interferometer/interferometercorr.cpp index a12579aef..27388670b 100644 --- a/plugins/channelmimo/interferometer/interferometercorr.cpp +++ b/plugins/channelmimo/interferometer/interferometercorr.cpp @@ -21,6 +21,18 @@ #include "dsp/fftengine.h" #include "interferometercorr.h" +Sample sFirst(const Sample& a, const Sample& b) { + return a; +} + +Sample sSecond(const Sample& a, const Sample& b) { + return b; +} + +Sample sSecondInv(const Sample& a, const Sample& b) { + return Sample{-b.real(), -b.imag()}; +} + Sample sAdd(const Sample& a, const Sample& b) { //!< Sample addition return Sample{a.real() + b.real(), a.imag() + b.imag()}; } @@ -68,8 +80,8 @@ Sample sMulConjInv(const Sample& a, const Sample& b) { //!< Sample multiply with Sample invfft2s(const std::complex& a) { //!< Complex float to Sample Sample s; - s.setReal(a.real()); - s.setImag(a.imag()); + s.setReal(a.real()/2.0f); + s.setImag(a.imag()/2.0f); return s; } @@ -92,8 +104,10 @@ InterferometerCorrelator::InterferometerCorrelator(int fftSize) : m_invFFT->configure(2*fftSize, true); m_dataj = new std::complex[2*fftSize]; // receives actual FFT result hence twice the data FFT size - m_scorr.resize(2*fftSize); - m_tcorr.resize(2*fftSize); + m_scorr.resize(fftSize); + m_tcorr.resize(fftSize); + m_scorrSize = fftSize; + m_tcorrSize = fftSize; } InterferometerCorrelator::~InterferometerCorrelator() @@ -119,6 +133,12 @@ bool InterferometerCorrelator::performCorr( { switch (m_corrType) { + case InterferometerSettings::Correlation0: + results = performOpCorr(data0, size0, data1, size1, sFirst); + break; + case InterferometerSettings::Correlation1: + results = performOpCorr(data0, size0, data1, size1, sSecond); + break; case InterferometerSettings::CorrelationAdd: results = performOpCorr(data0, size0, data1, size1, sAdd); break; @@ -136,6 +156,12 @@ bool InterferometerCorrelator::performCorr( { switch (m_corrType) { + case InterferometerSettings::Correlation0: + results = performOpCorr(data0, size0, data1, size1, sFirst); + break; + case InterferometerSettings::Correlation1: + results = performOpCorr(data0, size0, data1, size1, sSecondInv); + break; case InterferometerSettings::CorrelationAdd: results = performOpCorr(data0, size0, data1, size1, sAddInv); break; @@ -173,6 +199,12 @@ bool InterferometerCorrelator::performCorr( switch (m_corrType) { + case InterferometerSettings::Correlation0: + results = performOpCorr(data0, size0, m_data1p, size1, sFirst); + break; + case InterferometerSettings::Correlation1: + results = performOpCorr(data0, size0, m_data1p, size1, sSecond); + break; case InterferometerSettings::CorrelationAdd: results = performOpCorr(data0, size0, m_data1p, size1, sAdd); break; @@ -199,9 +231,6 @@ bool InterferometerCorrelator::performOpCorr( ) { unsigned int size = std::min(size0, size1); - // if (size0 != size1) { - // qDebug("InterferometerCorrelator::performOpCorr: size0: %d, size1: %d", size0, size1); - // } adjustTCorrSize(size); std::transform( @@ -232,7 +261,7 @@ bool InterferometerCorrelator::performFFTCorr( adjustSCorrSize(size); adjustTCorrSize(size); - while (size > m_fftSize) + while (size >= m_fftSize) { // FFT[0] std::transform( @@ -263,7 +292,7 @@ bool InterferometerCorrelator::performFFTCorr( // conjugate FFT[1] std::transform( m_fft[1]->out(), - m_fft[1]->out()+2*m_fftSize, + m_fft[1]->out() + 2*m_fftSize, m_dataj, [](const std::complex& c) -> std::complex { return std::conj(c); @@ -273,7 +302,7 @@ bool InterferometerCorrelator::performFFTCorr( // product of FFT[1]* with FFT[0] and store in inverse FFT input std::transform( m_fft[0]->out(), - m_fft[0]->out()+2*m_fftSize, + m_fft[0]->out() + 2*m_fftSize, m_dataj, m_invFFT->in(), [](std::complex& a, const std::complex& b) -> std::complex { @@ -284,8 +313,8 @@ bool InterferometerCorrelator::performFFTCorr( // copy product to correlation spectrum - convert and scale to FFT size std::transform( m_invFFT->in(), - m_invFFT->in() + 2*m_fftSize, - m_scorr.begin(), + m_invFFT->in() + m_fftSize, + m_scorr.begin() + nfft*m_fftSize, [this](const std::complex& a) -> Sample { Sample s; s.setReal(a.real()*(SDR_RX_SCALEF/m_fftSize)); @@ -298,8 +327,8 @@ bool InterferometerCorrelator::performFFTCorr( m_invFFT->transform(); std::transform( m_invFFT->out(), - m_invFFT->out() + 2*m_fftSize, - m_tcorr.begin(), + m_invFFT->out() + m_fftSize, + m_tcorr.begin() + nfft*m_fftSize, invfft2s ); @@ -319,19 +348,23 @@ bool InterferometerCorrelator::performFFTCorr( void InterferometerCorrelator::adjustSCorrSize(int size) { - if (size > m_scorrSize) + int nFFTSize = (size/m_fftSize)*m_fftSize; + + if (nFFTSize > m_scorrSize) { - m_scorr.resize(size); - m_scorrSize = size; + m_scorr.resize(nFFTSize); + m_scorrSize = nFFTSize; } } void InterferometerCorrelator::adjustTCorrSize(int size) { - if (size > m_tcorrSize) + int nFFTSize = (size/m_fftSize)*m_fftSize; + + if (nFFTSize > m_tcorrSize) { - m_tcorr.resize(size); - m_tcorrSize = size; + m_tcorr.resize(nFFTSize); + m_tcorrSize = nFFTSize; } } diff --git a/plugins/channelmimo/interferometer/interferometergui.ui b/plugins/channelmimo/interferometer/interferometergui.ui index 999dd2038..ed6de8984 100644 --- a/plugins/channelmimo/interferometer/interferometergui.ui +++ b/plugins/channelmimo/interferometer/interferometergui.ui @@ -171,12 +171,22 @@ - Add + A - Mul* + B + + + + + A+B + + + + + A.B* diff --git a/plugins/channelmimo/interferometer/interferometersettings.h b/plugins/channelmimo/interferometer/interferometersettings.h index 491875e85..38dfd001f 100644 --- a/plugins/channelmimo/interferometer/interferometersettings.h +++ b/plugins/channelmimo/interferometer/interferometersettings.h @@ -27,6 +27,8 @@ struct InterferometerSettings { enum CorrelationType { + Correlation0, + Correlation1, CorrelationAdd, CorrelationMultiply, CorrelationFFT diff --git a/plugins/channelmimo/interferometer/interferometersink.cpp b/plugins/channelmimo/interferometer/interferometersink.cpp index 85eb1bc19..95f1f6a87 100644 --- a/plugins/channelmimo/interferometer/interferometersink.cpp +++ b/plugins/channelmimo/interferometer/interferometersink.cpp @@ -15,6 +15,7 @@ // along with this program. If not, see . // /////////////////////////////////////////////////////////////////////////////////// +#include #include #include "dsp/downchannelizer.h" @@ -30,25 +31,29 @@ MESSAGE_CLASS_DEFINITION(InterferometerSink::MsgConfigureCorrelation, Message) InterferometerSink::InterferometerSink(int fftSize) : m_correlator(fftSize), m_spectrumSink(nullptr), - m_scopeSink(nullptr) + m_scopeSink(nullptr), + m_mutex(QMutex::Recursive) { - m_sampleMIFifo.init(2, 96000 * 4); + m_sampleMIFifo.init(2, 96000 * 8); + m_vbegin.resize(2); for (int i = 0; i < 2; i++) { m_sinks[i].setStreamIndex(i); m_channelizers[i] = new DownChannelizer(&m_sinks[i]); + m_sizes[i] = 0; } QObject::connect( &m_sampleMIFifo, - &SampleMIFifo::dataAsyncReady, + &SampleMIFifo::dataSyncReady, this, - &InterferometerSink::handleDataAsync, + &InterferometerSink::handleData, Qt::QueuedConnection ); connect(&m_inputMessageQueue, SIGNAL(messageEnqueued()), this, SLOT(handleInputMessages())); + m_lastStream = 0; } InterferometerSink::~InterferometerSink() @@ -61,7 +66,12 @@ InterferometerSink::~InterferometerSink() void InterferometerSink::reset() { + QMutexLocker mutexLocker(&m_mutex); m_sampleMIFifo.reset(); + + for (int i = 0; i < 2; i++) { + m_sinks[i].reset(); + } } void InterferometerSink::feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end, unsigned int streamIndex) @@ -70,86 +80,64 @@ void InterferometerSink::feed(const SampleVector::const_iterator& begin, const S return; } - m_sampleMIFifo.writeAsync(begin, end - begin, streamIndex); -} + if (streamIndex == m_lastStream) { + qWarning("InterferometerSink::feed: twice same stream in a row: %u", streamIndex); + } -void InterferometerSink::handleDataAsync(int sinkIndex) -{ - SampleVector::const_iterator part1begin; - SampleVector::const_iterator part1end; - SampleVector::const_iterator part2begin; - SampleVector::const_iterator part2end; + m_lastStream = streamIndex; + m_vbegin[streamIndex] = begin; + m_sizes[streamIndex] = end - begin; - while ((m_sampleMIFifo.fillAsync(sinkIndex) > 0) && (m_inputMessageQueue.size() == 0)) + if (streamIndex == 1) { - m_sampleMIFifo.readAsync(&part1begin, &part1end, &part2begin, &part2end, sinkIndex); - - if (part1begin != part1end) { // first part of FIFO data - //qDebug("InterferometerSink::handleSinkFifo: part1-stream: %u count: %u", sinkIndex, count); - processFifo(part1begin, part1end, sinkIndex); + if (m_sizes[0] != m_sizes[1]) + { + qWarning("InterferometerSink::feed: unequal sizes: [0]: %d [1]: %d", m_sizes[0], m_sizes[1]); + m_sampleMIFifo.writeSync(m_vbegin, std::min(m_sizes[0], m_sizes[1])); } - - if (part2begin != part2end) { // second part of FIFO data (used when block wraps around) - //qDebug("InterferometerSink::handleSinkFifo: part2-stream: %u count: %u", sinkIndex, count); - processFifo(part2begin, part2end, sinkIndex); + else + { + m_sampleMIFifo.writeSync(m_vbegin, m_sizes[0]); } - } - - // int samplesDone = 0; - - // while ((m_sinkFifos[sinkIndex].fill() > 0) - // && (m_inputMessageQueue.size() == 0)) - // //&& (samplesDone < m_channelizers[sinkIndex]->getInputSampleRate())) - // { - // SampleVector::iterator part1begin; - // SampleVector::iterator part1end; - // SampleVector::iterator part2begin; - // SampleVector::iterator part2end; - - // unsigned int count = m_sinkFifos[sinkIndex].readBegin(m_sinkFifos[sinkIndex].fill(), &part1begin, &part1end, &part2begin, &part2end); - - // if (part1begin != part1end) { // first part of FIFO data - // //qDebug("InterferometerSink::handleSinkFifo: part1-stream: %u count: %u", sinkIndex, count); - // processFifo(part1begin, part1end, sinkIndex); - // } - - // if (part2begin != part2end) { // second part of FIFO data (used when block wraps around) - // //qDebug("InterferometerSink::handleSinkFifo: part2-stream: %u count: %u", sinkIndex, count); - // processFifo(part2begin, part2end, sinkIndex); - // } - - // m_sinkFifos[sinkIndex].readCommit((unsigned int) count); // adjust FIFO pointers - // samplesDone += count; - // } - - //qDebug("InterferometerSink::handleSinkFifo: done"); } -void InterferometerSink::processFifo(const SampleVector::const_iterator& vbegin, const SampleVector::const_iterator& vend, unsigned int sinkIndex) +void InterferometerSink::handleData() { - // if (sinkIndex == 0) { - // m_count0 = vend - vbegin; - // } else if (sinkIndex == 1) { - // m_count1 = vend - vbegin; - // if (m_count1 != m_count0) { - // qDebug("InterferometerSink::processFifo: c0: %d 1: %d", m_count0, m_count1); - // } - // } + QMutexLocker mutexLocker(&m_mutex); - m_channelizers[sinkIndex]->feed(vbegin, vend, false); + const std::vector& data = m_sampleMIFifo.getData(); - if (sinkIndex == 1) { - run(); + unsigned int ipart1begin; + unsigned int ipart1end; + unsigned int ipart2begin; + unsigned int ipart2end; + + while ((m_sampleMIFifo.fillSync() > 0) && (m_inputMessageQueue.size() == 0)) + { + m_sampleMIFifo.readSync(ipart1begin, ipart1end, ipart2begin, ipart2end); + + if (ipart1begin != ipart1end) { // first part of FIFO data + processFifo(data, ipart1begin, ipart1end); + } + + if (ipart2begin != ipart2end) { // second part of FIFO data (used when block wraps around) + processFifo(data, ipart2begin, ipart2end); + } } } +void InterferometerSink::processFifo(const std::vector& data, unsigned int ibegin, unsigned int iend) +{ + for (unsigned int stream = 0; stream < 2; stream++) { + m_channelizers[stream]->feed(data[stream].begin() + ibegin, data[stream].begin() + iend, false); + } + + run(); +} + void InterferometerSink::run() { - // if (m_sinks[0].getSize() != m_sinks[1].getSize()) { - // qDebug("InterferometerSink::run: size0: %d, size1: %d", m_sinks[0].getSize(), m_sinks[1].getSize()); - // } - if (m_correlator.performCorr(m_sinks[0].getData(), m_sinks[0].getSize(), m_sinks[1].getData(), m_sinks[1].getSize())) { if (m_scopeSink) { @@ -195,6 +183,7 @@ bool InterferometerSink::handleMessage(const Message& cmd) { if (MsgConfigureChannelizer::match(cmd)) { + QMutexLocker mutexLocker(&m_mutex); MsgConfigureChannelizer& cfg = (MsgConfigureChannelizer&) cmd; int log2Decim = cfg.getLog2Decim(); int filterChainHash = cfg.getFilterChainHash(); @@ -208,6 +197,7 @@ bool InterferometerSink::handleMessage(const Message& cmd) m_channelizers[i]->set(m_channelizers[i]->getInputMessageQueue(), log2Decim, filterChainHash); + m_sinks[i].reset(); } return true; diff --git a/plugins/channelmimo/interferometer/interferometersink.h b/plugins/channelmimo/interferometer/interferometersink.h index 454475ebb..adc45ac2b 100644 --- a/plugins/channelmimo/interferometer/interferometersink.h +++ b/plugins/channelmimo/interferometer/interferometersink.h @@ -19,6 +19,7 @@ #define INCLUDE_INTERFEROMETERSINK_H #include +#include //#include "dsp/samplesinkvector.h" #include "dsp/samplemififo.h" @@ -110,22 +111,25 @@ public: void feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end, unsigned int streamIndex); private: - void processFifo(const SampleVector::const_iterator& vbegin, const SampleVector::const_iterator& vend, unsigned int sinkIndex); + void processFifo(const std::vector& data, unsigned int ibegin, unsigned int iend); void run(); bool handleMessage(const Message& cmd); InterferometerCorrelator m_correlator; SampleMIFifo m_sampleMIFifo; + std::vector m_vbegin; + int m_sizes[2]; InterferometerStreamSink m_sinks[2]; DownChannelizer *m_channelizers[2]; BasebandSampleSink *m_spectrumSink; BasebandSampleSink *m_scopeSink; MessageQueue m_inputMessageQueue; //!< Queue for asynchronous inbound communication - int m_count0, m_count1; + QMutex m_mutex; + unsigned int m_lastStream; private slots: void handleInputMessages(); - void handleDataAsync(int sinkIndex); //!< Handle data when samples have to be processed + void handleData(); //!< Handle data when samples have to be processed }; #endif // INCLUDE_INTERFEROMETERSINK_H diff --git a/plugins/channelmimo/interferometer/interferometerstreamsink.cpp b/plugins/channelmimo/interferometer/interferometerstreamsink.cpp index f9f8bcd91..ea58cbf23 100644 --- a/plugins/channelmimo/interferometer/interferometerstreamsink.cpp +++ b/plugins/channelmimo/interferometer/interferometerstreamsink.cpp @@ -15,7 +15,9 @@ // along with this program. If not, see . // /////////////////////////////////////////////////////////////////////////////////// +#include #include + #include "dsp/downchannelizer.h" #include "interferometerstreamsink.h" @@ -40,7 +42,7 @@ void InterferometerStreamSink::stop() void InterferometerStreamSink::feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end, bool positiveOnly) { - m_settingsMutex.lock(); + QMutexLocker mutexLocker(&m_settingsMutex); m_dataSize = (end - begin) + m_dataStart; if (m_dataSize > m_bufferSize) @@ -50,7 +52,6 @@ void InterferometerStreamSink::feed(const SampleVector::const_iterator& begin, c } std::copy(begin, end, m_data.begin() + m_dataStart); - m_settingsMutex.unlock(); } bool InterferometerStreamSink::handleMessage(const Message& cmd) @@ -72,3 +73,9 @@ bool InterferometerStreamSink::handleMessage(const Message& cmd) return false; } } + +void InterferometerStreamSink::reset() +{ + QMutexLocker mutexLocker(&m_settingsMutex); + m_dataStart = 0; +} diff --git a/plugins/channelmimo/interferometer/interferometerstreamsink.h b/plugins/channelmimo/interferometer/interferometerstreamsink.h index 68b567eb8..918c86029 100644 --- a/plugins/channelmimo/interferometer/interferometerstreamsink.h +++ b/plugins/channelmimo/interferometer/interferometerstreamsink.h @@ -36,6 +36,7 @@ public: virtual void feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end, bool positiveOnly); virtual bool handleMessage(const Message& cmd); //!< Processing of a message. Returns true if message has actually been processed + void reset(); unsigned int getStreamIndex() const { return m_streamIndex; } void setStreamIndex(unsigned int streamIndex) { m_streamIndex = streamIndex; } SampleVector& getData() { return m_data; } diff --git a/plugins/samplemimo/bladerf2mimo/bladerf2mimogui.cpp b/plugins/samplemimo/bladerf2mimo/bladerf2mimogui.cpp index 103ff5f48..f6c4c1a42 100644 --- a/plugins/samplemimo/bladerf2mimo/bladerf2mimogui.cpp +++ b/plugins/samplemimo/bladerf2mimo/bladerf2mimogui.cpp @@ -490,6 +490,7 @@ void BladeRF2MIMOGui::on_sampleRate_changed(quint64 value) } } + displaySampleRate(); displayFcTooltip(); sendSettings(); } diff --git a/plugins/samplemimo/bladerf2mimo/bladerf2mithread.cpp b/plugins/samplemimo/bladerf2mimo/bladerf2mithread.cpp index e89f185e1..9f9d45598 100644 --- a/plugins/samplemimo/bladerf2mimo/bladerf2mithread.cpp +++ b/plugins/samplemimo/bladerf2mimo/bladerf2mithread.cpp @@ -129,17 +129,26 @@ void BladeRF2MIThread::callback(const qint16* buf, qint32 samplesPerChannel) } std::vector vbegin; + int lengths[2]; for (unsigned int channel = 0; channel < 2; channel++) { - channelCallback(&buf[2*samplesPerChannel*channel], 2*samplesPerChannel, channel); + lengths[channel] = channelCallback(&buf[2*samplesPerChannel*channel], 2*samplesPerChannel, channel); vbegin.push_back(m_convertBuffer[channel].begin()); } - m_sampleFifo->writeSync(vbegin, samplesPerChannel/(1<writeSync(vbegin, lengths[0]); + } + else + { + qWarning("BladeRF2MIThread::callback: unequal channel lengths: [0]=%d [1]=%d", lengths[0], lengths[1]); + m_sampleFifo->writeSync(vbegin, std::min(lengths[0], lengths[1])); + } } -void BladeRF2MIThread::channelCallback(const qint16* buf, qint32 len, int channel) +int BladeRF2MIThread::channelCallback(const qint16* buf, qint32 len, int channel) { SampleVector::iterator it = m_convertBuffer[channel].begin(); @@ -228,4 +237,6 @@ void BladeRF2MIThread::channelCallback(const qint16* buf, qint32 len, int channe } } } + + return it - m_convertBuffer[channel].begin(); } \ No newline at end of file diff --git a/plugins/samplemimo/bladerf2mimo/bladerf2mithread.h b/plugins/samplemimo/bladerf2mimo/bladerf2mithread.h index c14654463..84577c70e 100644 --- a/plugins/samplemimo/bladerf2mimo/bladerf2mithread.h +++ b/plugins/samplemimo/bladerf2mimo/bladerf2mithread.h @@ -63,7 +63,7 @@ private: void run(); void callback(const qint16* buf, qint32 samplesPerChannel); - void channelCallback(const qint16* buf, qint32 len, int channel); + int channelCallback(const qint16* buf, qint32 len, int channel); }; #endif // PLUGINS_SAMPLEMIMO_BLADERF2MIMO_BLADERF2MITHREAD_H_