VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2025-11-04 05:30:32 -05:00
Code Issues Projects Releases Wiki Activity

BFM demod: added phase lock class. Updated copyright notices

Browse Source
This commit is contained in:
f4exb 2015-12-06 19:47:55 +01:00
parent 7815531b3c
commit f69e69a799
16 changed files with 477 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -86,6 +86,7 @@ set(sdrbase_SOURCES
sdrbase/dsp/movingaverage.cpp
sdrbase/dsp/nco.cpp
sdrbase/dsp/pidcontroller.cpp
sdrbase/dsp/phaselock.cpp
sdrbase/dsp/samplefifo.cpp
sdrbase/dsp/samplesink.cpp
sdrbase/dsp/nullsink.cpp
@ -164,6 +165,7 @@ set(sdrbase_HEADERS
include/dsp/misc.h
include/dsp/movingaverage.h
include/dsp/nco.h
include/dsp/phaselock.h
sdrbase/dsp/pidcontroller.h
include/dsp/samplefifo.h
include/dsp/samplesink.h

67
include/dsp/phaselock.h Normal file
View File

@ -0,0 +1,67 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#include "dsp/dsptypes.h"
/** Phase-locked loop mainly for broadcadt FM stereo pilot. */
class PhaseLock
{
public:
/** Expected pilot frequency (used for PPS events). */
static const int pilot_frequency = 19000;
/**
* Construct phase-locked loop.
*
* freq :: 19 kHz center frequency relative to sample freq
* (0.5 is Nyquist)
* bandwidth :: bandwidth relative to sample frequency
* minsignal :: minimum pilot amplitude
*/
PhaseLock(Real freq, Real bandwidth, Real minsignal);
/**
* Process samples and extract 19 kHz pilot tone.
* Generate phase-locked 38 kHz tone with unit amplitude.
*/
void process(const std::vector<Real>& samples_in, std::vector<Real>& samples_out);
/** Return true if the phase-locked loop is locked. */
bool locked() const
{
return m_lock_cnt >= m_lock_delay;
}
/** Return detected amplitude of pilot signal. */
Real get_pilot_level() const
{
return 2 * m_pilot_level;
}
private:
Real m_minfreq, m_maxfreq;
Real m_phasor_b0, m_phasor_a1, m_phasor_a2;
Real m_phasor_i1, m_phasor_i2, m_phasor_q1, m_phasor_q2;
Real m_loopfilter_b0, m_loopfilter_b1;
Real m_loopfilter_x1;
Real m_freq, m_phase;
Real m_minsignal;
Real m_pilot_level;
int m_lock_delay;
int m_lock_cnt;
};

19
plugins/channel/bfm/bfmdemod.cpp
View File

@ -1,6 +1,6 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2012 maintech GmbH, Otto-Hahn-Str. 15, 97204 Hoechberg, Germany //
// written by Christian Daniel //
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
@ -41,7 +41,7 @@ BFMDemod::BFMDemod(SampleSink* sampleSink) :
m_config.m_afBandwidth = 15000;
m_config.m_squelch = -60.0;
m_config.m_volume = 2.0;
m_config.m_audioSampleRate = DSPEngine::instance()->getAudioSampleRate();
m_config.m_audioSampleRate = DSPEngine::instance()->getAudioSampleRate(); // normally 48 kHz
m_rfFilter = new fftfilt(-50000.0 / 384000.0, 50000.0 / 384000.0, rfFilterFftLength);
apply();
@ -77,6 +77,8 @@ void BFMDemod::feed(const SampleVector::const_iterator& begin, const SampleVecto
int rf_out;
Real msq, demod;
m_sampleBuffer.clear();
m_settingsMutex.lock();
for (SampleVector::const_iterator it = begin; it != end; ++it)
@ -116,12 +118,13 @@ void BFMDemod::feed(const SampleVector::const_iterator& begin, const SampleVecto
m_m2Sample = m_m1Sample;
m_m1Sample = rf[i];
m_sampleBuffer.push_back(Sample(demod * (1<<15), 0.0));
Complex e(demod, 0);
if(m_interpolator.interpolate(&m_interpolatorDistanceRemain, e, &ci))
{
quint16 sample = (qint16)(ci.real() * 3000 * m_running.m_volume);
m_sampleBuffer.push_back(Sample(sample, sample));
//m_sampleBuffer.push_back(Sample(sample, sample));
m_audioBuffer[m_audioBufferFill].l = sample;
m_audioBuffer[m_audioBufferFill].r = sample;
++m_audioBufferFill;
@ -157,7 +160,7 @@ void BFMDemod::feed(const SampleVector::const_iterator& begin, const SampleVecto
if(m_sampleSink != 0)
{
m_sampleSink->feed(m_sampleBuffer.begin(), m_sampleBuffer.end(), false);
m_sampleSink->feed(m_sampleBuffer.begin(), m_sampleBuffer.end(), true);
}
m_sampleBuffer.clear();
@ -251,11 +254,15 @@ void BFMDemod::apply()
(m_config.m_inputFrequencyOffset != m_running.m_inputFrequencyOffset))
{
m_settingsMutex.lock();
qDebug() << "BFMDemod::handleMessage: m_rfFilter->create_filter";
Real lowCut = -(m_config.m_rfBandwidth / 2.0) / m_config.m_inputSampleRate;
Real hiCut = (m_config.m_rfBandwidth / 2.0) / m_config.m_inputSampleRate;
m_rfFilter->create_filter(lowCut, hiCut);
m_settingsMutex.unlock();
qDebug() << "BFMDemod::handleMessage: m_rfFilter->create_filter: sampleRate: "
<< m_config.m_inputSampleRate
<< " lowCut: " << lowCut * m_config.m_inputSampleRate
<< " hiCut: " << hiCut * m_config.m_inputSampleRate;
}
if((m_config.m_afBandwidth != m_running.m_afBandwidth) ||

7
plugins/channel/bfm/bfmdemod.h
View File

@ -1,6 +1,6 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2012 maintech GmbH, Otto-Hahn-Str. 15, 97204 Hoechberg, Germany //
// written by Christian Daniel //
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
@ -38,6 +38,7 @@ public:
void configure(MessageQueue* messageQueue, Real rfBandwidth, Real afBandwidth, Real volume, Real squelch);
int getSampleRate() const { return m_config.m_inputSampleRate; }
virtual void feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end, bool po);
virtual void start();
virtual void stop();
@ -110,7 +111,7 @@ private:
Config m_running;
NCO m_nco;
Interpolator m_interpolator; //!< Interpolator between sample rate sent from DSP engine and requested RF bandwidth (rational)
Interpolator m_interpolator; //!< Interpolator between fixed demod bandwidth and audio bandwidth (rational)
Real m_interpolatorDistance;
Real m_interpolatorDistanceRemain;
Lowpass<Real> m_lowpass;

47
plugins/channel/bfm/bfmdemodgui.cpp
View File

@ -1,9 +1,27 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#include <QDockWidget>
#include <QMainWindow>
#include <QDebug>
#include "dsp/threadedsamplesink.h"
#include "dsp/channelizer.h"
#include "dsp/dspengine.h"
#include "dsp/spectrumvis.h"
#include "gui/glspectrum.h"
#include "plugin/pluginapi.h"
#include "util/simpleserializer.h"
@ -17,7 +35,7 @@
#include "bfmdemod.h"
const int BFMDemodGUI::m_rfBW[] = {
48000, 80000, 120000, 140000, 160000, 180000, 200000, 220000, 250000
80000, 100000, 120000, 140000, 160000, 180000, 200000, 220000, 250000
};
int requiredBW(int rfBW)
@ -85,6 +103,7 @@ QByteArray BFMDemodGUI::serialize() const
s.writeS32(4, ui->volume->value());
s.writeS32(5, ui->squelch->value());
s.writeU32(7, m_channelMarker.getColor().rgb());
s.writeBlob(8, ui->spectrumGUI->serialize());
return s.final();
}
@ -123,6 +142,9 @@ bool BFMDemodGUI::deserialize(const QByteArray& data)
m_channelMarker.setColor(u32tmp);
}
d.readBlob(8, &bytetmp);
ui->spectrumGUI->deserialize(bytetmp);
blockApplySettings(false);
m_channelMarker.blockSignals(false);
@ -225,11 +247,22 @@ BFMDemodGUI::BFMDemodGUI(PluginAPI* pluginAPI, QWidget* parent) :
connect(this, SIGNAL(widgetRolled(QWidget*,bool)), this, SLOT(onWidgetRolled(QWidget*,bool)));
connect(this, SIGNAL(menuDoubleClickEvent()), this, SLOT(onMenuDoubleClicked()));
m_wfmDemod = new BFMDemod(0);
m_channelizer = new Channelizer(m_wfmDemod);
m_spectrumVis = new SpectrumVis(ui->glSpectrum);
m_bfmDemod = new BFMDemod(m_spectrumVis);
m_channelizer = new Channelizer(m_bfmDemod);
m_threadedChannelizer = new ThreadedSampleSink(m_channelizer, this);
DSPEngine::instance()->addThreadedSink(m_threadedChannelizer);
//ui->glSpectrum->setCenterFrequency(BFMDemodGUI::m_rfBW[ui->rfBW->value()] / 4);
//ui->glSpectrum->setSampleRate(BFMDemodGUI::m_rfBW[ui->rfBW->value()] / 2);
ui->glSpectrum->setCenterFrequency(625000 / 4);
ui->glSpectrum->setSampleRate(625000 / 2);
//ui->glSpectrum->setCenterFrequency(48000 / 4);
//ui->glSpectrum->setSampleRate(48000 / 2);
ui->glSpectrum->setDisplayWaterfall(false);
ui->glSpectrum->setDisplayMaxHold(false);
ui->glSpectrum->setSsbSpectrum(true);
m_spectrumVis->configure(m_spectrumVis->getInputMessageQueue(), 64, 10, FFTWindow::BlackmanHarris);
connect(&m_pluginAPI->getMainWindow()->getMasterTimer(), SIGNAL(timeout()), this, SLOT(tick()));
//m_channelMarker = new ChannelMarker(this);
@ -240,6 +273,8 @@ BFMDemodGUI::BFMDemodGUI(PluginAPI* pluginAPI, QWidget* parent) :
connect(&m_channelMarker, SIGNAL(changed()), this, SLOT(viewChanged()));
m_pluginAPI->addChannelMarker(&m_channelMarker);
ui->spectrumGUI->setBuddies(m_spectrumVis->getInputMessageQueue(), m_spectrumVis, ui->glSpectrum);
applySettings();
}
@ -249,7 +284,7 @@ BFMDemodGUI::~BFMDemodGUI()
DSPEngine::instance()->removeThreadedSink(m_threadedChannelizer);
delete m_threadedChannelizer;
delete m_channelizer;
delete m_wfmDemod;
delete m_bfmDemod;
//delete m_channelMarker;
delete ui;
}
@ -272,7 +307,7 @@ void BFMDemodGUI::applySettings()
ui->deltaFrequency->setValue(abs(m_channelMarker.getCenterFrequency()));
ui->deltaMinus->setChecked(m_channelMarker.getCenterFrequency() < 0);
m_wfmDemod->configure(m_wfmDemod->getInputMessageQueue(),
m_bfmDemod->configure(m_bfmDemod->getInputMessageQueue(),
m_rfBW[ui->rfBW->value()],
ui->afBW->value() * 1000.0,
ui->volume->value() / 10.0,
@ -296,7 +331,7 @@ void BFMDemodGUI::enterEvent(QEvent*)
void BFMDemodGUI::tick()
{
Real powDb = CalcDb::dbPower(m_wfmDemod->getMagSq());
Real powDb = CalcDb::dbPower(m_bfmDemod->getMagSq());
m_channelPowerDbAvg.feed(powDb);
ui->channelPower->setText(QString::number(m_channelPowerDbAvg.average(), 'f', 1));
}

22
plugins/channel/bfm/bfmdemodgui.h
View File

@ -1,3 +1,20 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#ifndef INCLUDE_BFMDEMODGUI_H
#define INCLUDE_BFMDEMODGUI_H
@ -10,6 +27,7 @@ class PluginAPI;
class ThreadedSampleSink;
class Channelizer;
class SpectrumVis;
class BFMDemod;
namespace Ui {
@ -55,7 +73,9 @@ private:
ThreadedSampleSink* m_threadedChannelizer;
Channelizer* m_channelizer;
BFMDemod* m_wfmDemod;
SpectrumVis* m_spectrumVis;
BFMDemod* m_bfmDemod;
MovingAverage<Real> m_channelPowerDbAvg;
static const int m_rfBW[];

54
plugins/channel/bfm/bfmdemodgui.ui
View File

@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>302</width>
<height>373</height>
<width>252</width>
<height>324</height>
</rect>
</property>
<property name="windowTitle">
@ -16,7 +16,7 @@
<widget class="QWidget" name="settingsContainer" native="true">
<property name="geometry">
<rect>
<x>20</x>
<x>10</x>
<y>20</y>
<width>235</width>
<height>121</height>
@ -314,6 +314,42 @@
</item>
</layout>
</widget>
<widget class="QWidget" name="spectrumBox" native="true">
<property name="geometry">
<rect>
<x>10</x>
<y>150</y>
<width>231</width>
<height>156</height>
</rect>
</property>
<property name="windowTitle">
<string>Channel Spectrum</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<property name="spacing">
<number>3</number>
</property>
<property name="leftMargin">
<number>2</number>
</property>
<property name="topMargin">
<number>2</number>
</property>
<property name="rightMargin">
<number>2</number>
</property>
<property name="bottomMargin">
<number>2</number>
</property>
<item>
<widget class="GLSpectrum" name="glSpectrum" native="true"/>
</item>
<item>
<widget class="GLSpectrumGUI" name="spectrumGUI" native="true"/>
</item>
</layout>
</widget>
</widget>
<customwidgets>
<customwidget>
@ -328,6 +364,18 @@
<header>gui/valuedial.h</header>
<container>1</container>
</customwidget>
<customwidget>
<class>GLSpectrum</class>
<extends>QWidget</extends>
<header>gui/glspectrum.h</header>
<container>1</container>
</customwidget>
<customwidget>
<class>GLSpectrumGUI</class>
<extends>QWidget</extends>
<header>gui/glspectrumgui.h</header>
<container>1</container>
</customwidget>
</customwidgets>
<resources>
<include location="../../../sdrbase/resources/res.qrc"/>

17
plugins/channel/bfm/bfmplugin.cpp
View File

@ -1,3 +1,20 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#include <QtPlugin>
#include <QAction>
#include "plugin/pluginapi.h"

17
plugins/channel/bfm/bfmplugin.h
View File

@ -1,3 +1,20 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#ifndef INCLUDE_BFMPLUGIN_H
#define INCLUDE_BFMPLUGIN_H

5
plugins/channel/udpsrc/udpsrc.cpp
View File

@ -1,5 +1,6 @@
// Copyright (C) 2012 maintech GmbH, Otto-Hahn-Str. 15, 97204 Hoechberg, Germany //
// (C) 2015 John Greb //
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //

17
plugins/channel/udpsrc/udpsrc.h
View File

@ -1,3 +1,20 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#ifndef INCLUDE_UDPSRC_H
#define INCLUDE_UDPSRC_H

17
plugins/channel/udpsrc/udpsrcgui.cpp
View File

@ -1,3 +1,20 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#include "udpsrcgui.h"
#include "plugin/pluginapi.h"

17
plugins/channel/udpsrc/udpsrcgui.h
View File

@ -1,3 +1,20 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#ifndef INCLUDE_UDPSRCGUI_H
#define INCLUDE_UDPSRCGUI_H

17
plugins/channel/udpsrc/udpsrcplugin.cpp
View File

@ -1,3 +1,20 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#include "udpsrcplugin.h"
#include <QtPlugin>

17
plugins/channel/udpsrc/udpsrcplugin.h
View File

@ -1,3 +1,20 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#ifndef INCLUDE_UDPSRCPLUGIN_H
#define INCLUDE_UDPSRCPLUGIN_H

155
sdrbase/dsp/phaselock.cpp Normal file
View File

@ -0,0 +1,155 @@
///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 F4EXB //
// written by Edouard Griffiths //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#include <math.h>
#include "dsp/phaselock.h"
// Construct phase-locked loop.
PhaseLock::PhaseLock(Real freq, Real bandwidth, Real minsignal)
{
/*
* This is a type-2, 4th order phase-locked loop.
*
* Open-loop transfer function:
* G(z) = K * (z - q1) / ((z - p1) * (z - p2) * (z - 1) * (z - 1))
* K = 3.788 * (bandwidth * 2 * Pi)**3
* q1 = exp(-0.1153 * bandwidth * 2*Pi)
* p1 = exp(-1.146 * bandwidth * 2*Pi)
* p2 = exp(-5.331 * bandwidth * 2*Pi)
*
* I don't understand what I'm doing; hopefully it will work.
*/
// Set min/max locking frequencies.
m_minfreq = (freq - bandwidth) * 2.0 * M_PI;
m_maxfreq = (freq + bandwidth) * 2.0 * M_PI;
// Set valid signal threshold.
m_minsignal = minsignal;
m_lock_delay = int(20.0 / bandwidth);
m_lock_cnt = 0;
m_pilot_level = 0;
// Create 2nd order filter for I/Q representation of phase error.
// Filter has two poles, unit DC gain.
Real p1 = exp(-1.146 * bandwidth * 2.0 * M_PI);
Real p2 = exp(-5.331 * bandwidth * 2.0 * M_PI);
m_phasor_a1 = - p1 - p2;
m_phasor_a2 = p1 * p2;
m_phasor_b0 = 1 + m_phasor_a1 + m_phasor_a2;
// Create loop filter to stabilize the loop.
Real q1 = exp(-0.1153 * bandwidth * 2.0 * M_PI);
m_loopfilter_b0 = 0.62 * bandwidth * 2.0 * M_PI;
m_loopfilter_b1 = - m_loopfilter_b0 * q1;
// After the loop filter, the phase error is integrated to produce
// the frequency. Then the frequency is integrated to produce the phase.
// These integrators form the two remaining poles, both at z = 1.
// Initialize frequency and phase.
m_freq = freq * 2.0 * M_PI;
m_phase = 0;
m_phasor_i1 = 0;
m_phasor_i2 = 0;
m_phasor_q1 = 0;
m_phasor_q2 = 0;
m_loopfilter_x1 = 0;
}
// Process samples.
void PhaseLock::process(const std::vector<Real>& samples_in, std::vector<Real>& samples_out)
{
unsigned int n = samples_in.size();
samples_out.resize(n);
bool was_locked = (m_lock_cnt >= m_lock_delay);
if (n > 0)
m_pilot_level = 1000.0;
for (unsigned int i = 0; i < n; i++) {
// Generate locked pilot tone.
Real psin = sin(m_phase);
Real pcos = cos(m_phase);
// Generate double-frequency output.
// sin(2*x) = 2 * sin(x) * cos(x)
samples_out[i] = 2 * psin * pcos;
// Multiply locked tone with input.
Real x = samples_in[i];
Real phasor_i = psin * x;
Real phasor_q = pcos * x;
// Run IQ phase error through low-pass filter.
phasor_i = m_phasor_b0 * phasor_i
- m_phasor_a1 * m_phasor_i1
- m_phasor_a2 * m_phasor_i2;
phasor_q = m_phasor_b0 * phasor_q
- m_phasor_a1 * m_phasor_q1
- m_phasor_a2 * m_phasor_q2;
m_phasor_i2 = m_phasor_i1;
m_phasor_i1 = phasor_i;
m_phasor_q2 = m_phasor_q1;
m_phasor_q1 = phasor_q;
// Convert I/Q ratio to estimate of phase error.
Real phase_err;
if (phasor_i > abs(phasor_q)) {
// We are within +/- 45 degrees from lock.
// Use simple linear approximation of arctan.
phase_err = phasor_q / phasor_i;
} else if (phasor_q > 0) {
// We are lagging more than 45 degrees behind the input.
phase_err = 1;
} else {
// We are more than 45 degrees ahead of the input.
phase_err = -1;
}
// Detect pilot level (conservative).
m_pilot_level = std::min(m_pilot_level, phasor_i);
// Run phase error through loop filter and update frequency estimate.
m_freq += m_loopfilter_b0 * phase_err
+ m_loopfilter_b1 * m_loopfilter_x1;
m_loopfilter_x1 = phase_err;
// Limit frequency to allowable range.
m_freq = std::max(m_minfreq, std::min(m_maxfreq, m_freq));
// Update locked phase.
m_phase += m_freq;
if (m_phase > 2.0 * M_PI) {
m_phase -= 2.0 * M_PI;
}
}
// Update lock status.
if (2 * m_pilot_level > m_minsignal) {
if (m_lock_cnt < m_lock_delay)
m_lock_cnt += n;
} else {
m_lock_cnt = 0;
}
}