From 0f7972856f6507339323440c70d820b36f912cf7 Mon Sep 17 00:00:00 2001 From: f4exb Date: Fri, 27 May 2022 23:31:43 +0200 Subject: [PATCH] DOA2: DOA implementation --- plugins/channelmimo/doa2/doa2.cpp | 10 ++ plugins/channelmimo/doa2/doa2.h | 2 + plugins/channelmimo/doa2/doa2baseband.cpp | 37 ++++++- plugins/channelmimo/doa2/doa2baseband.h | 8 ++ plugins/channelmimo/doa2/doa2compass.cpp | 4 +- plugins/channelmimo/doa2/doa2corr.cpp | 81 +++++++++------ plugins/channelmimo/doa2/doa2corr.h | 12 +-- plugins/channelmimo/doa2/doa2gui.cpp | 28 +++++- plugins/channelmimo/doa2/doa2gui.h | 2 + plugins/channelmimo/doa2/doa2gui.ui | 116 +++++++++++++++++++++- plugins/channelmimo/doa2/doa2settings.cpp | 4 + plugins/channelmimo/doa2/doa2settings.h | 1 + 12 files changed, 256 insertions(+), 49 deletions(-) diff --git a/plugins/channelmimo/doa2/doa2.cpp b/plugins/channelmimo/doa2/doa2.cpp index c5fde8a3f..3b471436a 100644 --- a/plugins/channelmimo/doa2/doa2.cpp +++ b/plugins/channelmimo/doa2/doa2.cpp @@ -293,6 +293,16 @@ void DOA2::applyChannelSettings(uint32_t log2Decim, uint32_t filterChainHash) m_basebandSink->getInputMessageQueue()->push(msg); } +float DOA2::getPhi() const +{ + return m_basebandSink ? m_basebandSink->getPhi() : 0.0f; +} + +float DOA2::getPositiveDOA() const +{ + return std::acos(getPhi()/M_PI)*(180/M_PI); +} + int DOA2::webapiSettingsGet( SWGSDRangel::SWGChannelSettings& response, QString& errorMessage) diff --git a/plugins/channelmimo/doa2/doa2.h b/plugins/channelmimo/doa2/doa2.h index c6ea41e6b..e596acfab 100644 --- a/plugins/channelmimo/doa2/doa2.h +++ b/plugins/channelmimo/doa2/doa2.h @@ -125,6 +125,8 @@ public: ScopeVis *getScopeVis() { return &m_scopeSink; } void applyChannelSettings(uint32_t log2Decim, uint32_t filterChainHash); + float getPhi() const; + float getPositiveDOA() const; virtual int webapiSettingsGet( SWGSDRangel::SWGChannelSettings& response, diff --git a/plugins/channelmimo/doa2/doa2baseband.cpp b/plugins/channelmimo/doa2/doa2baseband.cpp index d81d82edc..c1aaa1163 100644 --- a/plugins/channelmimo/doa2/doa2baseband.cpp +++ b/plugins/channelmimo/doa2/doa2baseband.cpp @@ -33,6 +33,12 @@ MESSAGE_CLASS_DEFINITION(DOA2Baseband::MsgConfigureCorrelation, Message) DOA2Baseband::DOA2Baseband(int fftSize) : m_correlator(fftSize), + m_correlationType(DOA2Settings::CorrelationFFT), + m_fftSize(fftSize), + m_samplesCount(0), + m_magSum(0.0f), + m_wphSum(0.0f), + m_phi(0.0f), m_scopeSink(nullptr), m_mutex(QMutex::Recursive) { @@ -142,6 +148,10 @@ void DOA2Baseband::run() { if (m_correlator.performCorr(m_sinks[0].getData(), m_sinks[0].getSize(), m_sinks[1].getData(), m_sinks[1].getSize())) { + if (m_correlationType == DOA2Settings::CorrelationType::CorrelationFFT) { + processDOA(m_correlator.m_xcorr.begin(), m_correlator.m_processed); + } + if (m_scopeSink) { std::vector vbegin; @@ -221,12 +231,12 @@ bool DOA2Baseband::handleMessage(const Message& cmd) { QMutexLocker mutexLocker(&m_mutex); MsgConfigureCorrelation& cfg = (MsgConfigureCorrelation&) cmd; - DOA2Settings::CorrelationType correlationType = cfg.getCorrelationType(); + m_correlationType = cfg.getCorrelationType(); qDebug() << "DOA2Baseband::handleMessage: MsgConfigureCorrelation:" - << " correlationType: " << correlationType; + << " correlationType: " << m_correlationType; - m_correlator.setCorrType(correlationType); + m_correlator.setCorrType(m_correlationType); return true; } @@ -245,3 +255,24 @@ void DOA2Baseband ::setBasebandSampleRate(unsigned int sampleRate) m_sinks[istream].reset(); } } + +void DOA2Baseband::processDOA(const std::vector::iterator& begin, int nbSamples) +{ + const std::vector::iterator end = begin + nbSamples; + + for (std::vector::iterator it = begin; it != end; ++it) + { + float ph = std::arg(*it); + float mag = std::norm(*it); + m_magSum += mag; + m_wphSum += mag*ph; + + if (++m_samplesCount == m_fftSize) + { + m_phi = m_wphSum / m_magSum; + m_magSum = 0; + m_wphSum = 0; + m_samplesCount = 0; + } + } +} diff --git a/plugins/channelmimo/doa2/doa2baseband.h b/plugins/channelmimo/doa2/doa2baseband.h index 06d23dbdf..3a7473b7d 100644 --- a/plugins/channelmimo/doa2/doa2baseband.h +++ b/plugins/channelmimo/doa2/doa2baseband.h @@ -110,13 +110,21 @@ public: void feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end, unsigned int streamIndex); void setBasebandSampleRate(unsigned int sampleRate); + float getPhi() const { return m_phi; } private: void processFifo(const std::vector& data, unsigned int ibegin, unsigned int iend); void run(); bool handleMessage(const Message& cmd); + void processDOA(const std::vector::iterator& begin, int nbSamples); DOA2Correlator m_correlator; + DOA2Settings::CorrelationType m_correlationType; + int m_fftSize; + int m_samplesCount; + float m_magSum; + float m_wphSum; + float m_phi; SampleMIFifo m_sampleMIFifo; std::vector m_vbegin; int m_sizes[2]; diff --git a/plugins/channelmimo/doa2/doa2compass.cpp b/plugins/channelmimo/doa2/doa2compass.cpp index c210d010e..72930bc44 100644 --- a/plugins/channelmimo/doa2/doa2compass.cpp +++ b/plugins/channelmimo/doa2/doa2compass.cpp @@ -87,7 +87,7 @@ void DOA2Compass::paintEvent(QPaintEvent *) } - // draw yaw lines + // draw compass lines { int nyawLines = 36; float rotAng = 360.0 / nyawLines; @@ -150,7 +150,7 @@ void DOA2Compass::paintEvent(QPaintEvent *) painter.rotate(m_azAnt); painter.setPen(Qt::NoPen); - painter.setBrush(QBrush(Qt::white)); + painter.setBrush(QBrush(QColor(192, 192, 192))); QPointF pointsN[3] = { QPointF(fx1, fy1), QPointF(fx2, fy2), diff --git a/plugins/channelmimo/doa2/doa2corr.cpp b/plugins/channelmimo/doa2/doa2corr.cpp index 8b2411f1f..a02f7c887 100644 --- a/plugins/channelmimo/doa2/doa2corr.cpp +++ b/plugins/channelmimo/doa2/doa2corr.cpp @@ -138,10 +138,10 @@ DOA2Correlator::DOA2Correlator(int fftSize) : m_invFFT2Sequence = fftFactory->getEngine(fftSize, true, &m_invFFT2); m_dataj = new std::complex[2*fftSize]; // receives actual FFT result hence twice the data FFT size - m_scorr.resize(fftSize); m_tcorr.resize(fftSize); - m_scorrSize = fftSize; + m_xcorr.resize(fftSize); m_tcorrSize = fftSize; + m_xcorrSize = fftSize; } DOA2Correlator::~DOA2Correlator() @@ -268,6 +268,7 @@ bool DOA2Correlator::performOpCorr( { unsigned int size = std::min(size0, size1); adjustTCorrSize(size); + adjustXCorrSize(size); std::transform( data0.begin(), @@ -294,8 +295,8 @@ bool DOA2Correlator::performFFTProd( int nfft = 0; SampleVector::const_iterator begin0 = data0.begin(); SampleVector::const_iterator begin1 = data1.begin(); - adjustSCorrSize(size); adjustTCorrSize(size); + adjustXCorrSize(size); while (size >= m_fftSize) { @@ -340,21 +341,22 @@ bool DOA2Correlator::performFFTProd( } ); - // copy product to time domain - re-order, convert and scale to FFT size - std::transform( + // copy to complex vector for DOA with re-orderong + std::copy( m_invFFT2->in(), m_invFFT2->in() + m_fftSize/2, - m_tcorr.begin() + nfft*m_fftSize + m_fftSize/2, - [](const std::complex& a) -> Sample { - Sample s; - s.setReal(a.real()/2.0f); - s.setImag(a.imag()/2.0f); - return s; - } + m_xcorr.begin() + nfft*m_fftSize + m_fftSize/2 ); - std::transform( + std::copy( m_invFFT2->in() + m_fftSize/2, m_invFFT2->in() + m_fftSize, + m_xcorr.begin() + nfft*m_fftSize + ); + + // convert and scale to FFT size for scope time domain display + std::transform( + m_xcorr.begin() + nfft*m_fftSize, + m_xcorr.begin() + nfft*m_fftSize + m_fftSize, m_tcorr.begin() + nfft*m_fftSize, [](const std::complex& a) -> Sample { Sample s; @@ -364,14 +366,29 @@ bool DOA2Correlator::performFFTProd( } ); - // feed spectrum with the sum - std::transform( - begin0, - begin0 + m_fftSize, - begin1, - m_scorr.begin() + nfft*m_fftSize, - sAdd - ); + // copy product to time domain - re-order, convert and scale to FFT size + // std::transform( + // m_invFFT2->in(), + // m_invFFT2->in() + m_fftSize/2, + // m_tcorr.begin() + nfft*m_fftSize + m_fftSize/2, + // [](const std::complex& a) -> Sample { + // Sample s; + // s.setReal(a.real()/2.0f); + // s.setImag(a.imag()/2.0f); + // return s; + // } + // ); + // std::transform( + // m_invFFT2->in() + m_fftSize/2, + // m_invFFT2->in() + m_fftSize, + // m_tcorr.begin() + nfft*m_fftSize, + // [](const std::complex& a) -> Sample { + // Sample s; + // s.setReal(a.real()/2.0f); + // s.setImag(a.imag()/2.0f); + // return s; + // } + // ); size -= m_fftSize; begin0 += m_fftSize; @@ -387,17 +404,6 @@ bool DOA2Correlator::performFFTProd( return nfft > 0; } -void DOA2Correlator::adjustSCorrSize(int size) -{ - int nFFTSize = (size/m_fftSize)*m_fftSize; - - if (nFFTSize > m_scorrSize) - { - m_scorr.resize(nFFTSize); - m_scorrSize = nFFTSize; - } -} - void DOA2Correlator::adjustTCorrSize(int size) { int nFFTSize = (size/m_fftSize)*m_fftSize; @@ -409,6 +415,17 @@ void DOA2Correlator::adjustTCorrSize(int size) } } +void DOA2Correlator::adjustXCorrSize(int size) +{ + int nFFTSize = (size/m_fftSize)*m_fftSize; + + if (nFFTSize > m_xcorrSize) + { + m_xcorr.resize(nFFTSize); + m_xcorrSize = nFFTSize; + } +} + void DOA2Correlator::setPhase(int phase) { m_phase = phase; diff --git a/plugins/channelmimo/doa2/doa2corr.h b/plugins/channelmimo/doa2/doa2corr.h index e89cab324..df086586e 100644 --- a/plugins/channelmimo/doa2/doa2corr.h +++ b/plugins/channelmimo/doa2/doa2corr.h @@ -46,10 +46,10 @@ public: int getFullFFTSize() const { return 2*m_fftSize; } void setPhase(int phase); - SampleVector m_scorr; //!< raw correlation result (spectrum) - Sample vector expected - SampleVector m_tcorr; //!< correlation result (time or spectrum inverse FFT) - Sample vector expected - int m_processed; //!< number of samples processed at the end of correlation - int m_remaining[2]; //!< number of samples remaining per member at the end of correlation + SampleVector m_tcorr; //!< correlation result (time or spectrum inverse FFT) - Sample vector expected + std::vector m_xcorr; //!< correlation result of inverse FFT of FFT prod with conjugate (performFFTProd) for DOA processing + int m_processed; //!< number of samples processed at the end of correlation + int m_remaining[2]; //!< number of samples remaining per member at the end of correlation signals: void dataReady(int start, int stop); @@ -68,8 +68,8 @@ private: const SampleVector& data1, unsigned int size1 ); - void adjustSCorrSize(int size); void adjustTCorrSize(int size); + void adjustXCorrSize(int size); DOA2Settings::CorrelationType m_corrType; unsigned int m_fftSize; //!< FFT length @@ -86,8 +86,8 @@ private: SampleVector m_data0w; //!< windowed data 0 SampleVector m_data1w; //!< windowed data 1 SampleVector m_data1p; //!< data1 with phase correction - int m_scorrSize; //!< spectrum correlations vector size int m_tcorrSize; //!< time correlations vector size + int m_xcorrSize; //!< DOA correlations vector size int m_phase; //!< phase correction int64_t m_sin; //!< scaled sine of phase correction int64_t m_cos; //!< scaled cosine of phase correction diff --git a/plugins/channelmimo/doa2/doa2gui.cpp b/plugins/channelmimo/doa2/doa2gui.cpp index ad51a4c32..137f5db5d 100644 --- a/plugins/channelmimo/doa2/doa2gui.cpp +++ b/plugins/channelmimo/doa2/doa2gui.cpp @@ -152,6 +152,8 @@ DOA2GUI::DOA2GUI(PluginAPI* pluginAPI, DeviceUISet *deviceUISet, MIMOChannel *ch displayRateAndShift(); applySettings(true); + connect(&MainCore::instance()->getMasterTimer(), SIGNAL(timeout()), this, SLOT(tick())); + // Test ui->compass->setAzNeg(85); ui->compass->setAzPos(315); @@ -200,6 +202,8 @@ void DOA2GUI::displaySettings() applyDecimation(); ui->phaseCorrection->setValue(m_settings.m_phase); ui->phaseCorrectionText->setText(tr("%1").arg(m_settings.m_phase)); + ui->compass->setAzAnt(m_settings.m_antennaAz); + ui->antAz->setValue(m_settings.m_antennaAz); getRollupContents()->restoreState(m_rollupState); updateAbsoluteCenterFrequency(); blockApplySettings(false); @@ -318,6 +322,14 @@ void DOA2GUI::on_correlationType_currentIndexChanged(int index) applySettings(); } +void DOA2GUI::on_antAz_valueChanged(int value) +{ + m_settings.m_antennaAz = value; + ui->compass->setAzAnt(value); + updateDOA(); + applySettings(); +} + void DOA2GUI::applyDecimation() { uint32_t maxHash = 1; @@ -346,7 +358,9 @@ void DOA2GUI::applyPosition() void DOA2GUI::tick() { - if (++m_tickCount == 20) { // once per second + if (++m_tickCount == 20) // once per second + { + updateDOA(); m_tickCount = 0; } } @@ -357,9 +371,21 @@ void DOA2GUI::makeUIConnections() QObject::connect(ui->position, &QSlider::valueChanged, this, &DOA2GUI::on_position_valueChanged); QObject::connect(ui->phaseCorrection, &QSlider::valueChanged, this, &DOA2GUI::on_phaseCorrection_valueChanged); QObject::connect(ui->correlationType, QOverload::of(&QComboBox::currentIndexChanged), this, &DOA2GUI::on_correlationType_currentIndexChanged); + QObject::connect(ui->antAz, QOverload::of(&QSpinBox::valueChanged), this, &DOA2GUI::on_antAz_valueChanged); } void DOA2GUI::updateAbsoluteCenterFrequency() { setStatusFrequency(m_centerFrequency + m_shiftFrequencyFactor * m_sampleRate); } + +void DOA2GUI::updateDOA() +{ + float doaAngle = m_doa2->getPositiveDOA(); + float posAngle = ui->antAz->value() - doaAngle; // DOA angles are trigonometric but displayed angles are clockwise + float negAngle = ui->antAz->value() + doaAngle; + ui->compass->setAzPos(posAngle); + ui->compass->setAzNeg(negAngle); + ui->posText->setText(tr("%1").arg(ui->compass->getAzPos(), 0, 'f', 0)); + ui->negText->setText(tr("%1").arg(ui->compass->getAzNeg(), 0, 'f', 0)); +} diff --git a/plugins/channelmimo/doa2/doa2gui.h b/plugins/channelmimo/doa2/doa2gui.h index 90204d457..a8d314b14 100644 --- a/plugins/channelmimo/doa2/doa2gui.h +++ b/plugins/channelmimo/doa2/doa2gui.h @@ -87,6 +87,7 @@ private: bool handleMessage(const Message& message); void makeUIConnections(); void updateAbsoluteCenterFrequency(); + void updateDOA(); void leaveEvent(QEvent*); void enterEvent(QEvent*); @@ -97,6 +98,7 @@ private slots: void on_position_valueChanged(int value); void on_phaseCorrection_valueChanged(int value); void on_correlationType_currentIndexChanged(int index); + void on_antAz_valueChanged(int value); void onWidgetRolled(QWidget* widget, bool rollDown); void onMenuDialogCalled(const QPoint& p); void tick(); diff --git a/plugins/channelmimo/doa2/doa2gui.ui b/plugins/channelmimo/doa2/doa2gui.ui index 519c0e879..5f085cbfc 100644 --- a/plugins/channelmimo/doa2/doa2gui.ui +++ b/plugins/channelmimo/doa2/doa2gui.ui @@ -335,7 +335,7 @@ 0 98 - 720 + 718 334 @@ -402,8 +402,8 @@ 0 432 - 720 - 284 + 718 + 85 @@ -438,9 +438,115 @@ 2 - - + + + + + + + Pos + + + + + + + 000 + + + + + + + + 10 + 0 + + + + + + + + + + + Neg + + + + + + + 000 + + + + + + + + 10 + 0 + + + + + + + + + + + Ant + + + + + + + + 60 + 0 + + + + true + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 359 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + 359 + 200 + + Liberation Sans diff --git a/plugins/channelmimo/doa2/doa2settings.cpp b/plugins/channelmimo/doa2/doa2settings.cpp index eb4e63b5a..ace849550 100644 --- a/plugins/channelmimo/doa2/doa2settings.cpp +++ b/plugins/channelmimo/doa2/doa2settings.cpp @@ -45,6 +45,7 @@ void DOA2Settings::resetToDefaults() m_reverseAPIChannelIndex = 0; m_workspaceIndex = 0; m_hidden = false; + m_antennaAz = 0; } QByteArray DOA2Settings::serialize() const @@ -65,6 +66,7 @@ QByteArray DOA2Settings::serialize() const s.writeS32(13,m_workspaceIndex); s.writeBlob(14, m_geometryBytes); s.writeBool(15, m_hidden); + s.writeS32(16, m_antennaAz); if (m_scopeGUI) { s.writeBlob(21, m_scopeGUI->serialize()); @@ -121,6 +123,8 @@ bool DOA2Settings::deserialize(const QByteArray& data) d.readS32(13, &m_workspaceIndex); d.readBlob(14, &m_geometryBytes); d.readBool(15, &m_hidden, false); + d.readS32(16, &tmp, 0); + m_antennaAz = tmp < 0 ? 0 : tmp > 359 ? 359 : tmp; if (m_scopeGUI) { diff --git a/plugins/channelmimo/doa2/doa2settings.h b/plugins/channelmimo/doa2/doa2settings.h index 3aea5c945..cf0496b4f 100644 --- a/plugins/channelmimo/doa2/doa2settings.h +++ b/plugins/channelmimo/doa2/doa2settings.h @@ -38,6 +38,7 @@ struct DOA2Settings uint32_t m_log2Decim; uint32_t m_filterChainHash; int m_phase; + int m_antennaAz; bool m_useReverseAPI; QString m_reverseAPIAddress; uint16_t m_reverseAPIPort;