VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2025-07-04 09:55:22 -04:00
Code Issues Projects Releases Wiki Activity

New PLL: heuristics to find locked state

Browse Source
This commit is contained in:
f4exb 2018-05-16 01:57:16 +02:00
parent bb2d530122
commit 660d8d22ae
6 changed files with 379 additions and 337 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
plugins/channelrx/chanalyzerng/chanalyzerng.cpp
View File

@ -197,8 +197,6 @@ bool ChannelAnalyzerNG::handleMessage(const Message& cmd)
} }
} }
void ChannelAnalyzerNG::apply(bool force) void ChannelAnalyzerNG::apply(bool force)
{ {
if ((m_running.m_frequency != m_config.m_frequency) || if ((m_running.m_frequency != m_config.m_frequency) ||
@ -253,6 +251,12 @@ void ChannelAnalyzerNG::apply(bool force)
m_settingsMutex.unlock(); m_settingsMutex.unlock();
} }
if ((m_running.m_channelSampleRate != m_config.m_channelSampleRate) ||
(m_running.m_spanLog2 != m_config.m_spanLog2) || force)
{
m_pll.setSampleRate(m_running.m_channelSampleRate / (1<<m_running.m_spanLog2));
}
if (m_running.m_pll != m_config.m_pll || force) if (m_running.m_pll != m_config.m_pll || force)
{ {
if (m_config.m_pll) { if (m_config.m_pll) {

593
plugins/channelrx/chanalyzerng/chanalyzerng.h
View File

@ -1,297 +1,296 @@
/////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2017 Edouard Griffiths, F4EXB // // Copyright (C) 2017 Edouard Griffiths, F4EXB //
// // // //
// This program is free software; you can redistribute it and/or modify // // 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 // // it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or // // the Free Software Foundation as version 3 of the License, or //
// // // //
// This program is distributed in the hope that it will be useful, // // This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of // // but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. // // GNU General Public License V3 for more details. //
// // // //
// You should have received a copy of the GNU General Public License // // You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. // // along with this program. If not, see <http://www.gnu.org/licenses/>. //
/////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////
#ifndef INCLUDE_CHANALYZERNG_H #ifndef INCLUDE_CHANALYZERNG_H
#define INCLUDE_CHANALYZERNG_H #define INCLUDE_CHANALYZERNG_H
#include <QMutex> #include <QMutex>
#include <vector> #include <vector>
#include "dsp/basebandsamplesink.h" #include "dsp/basebandsamplesink.h"
#include "channel/channelsinkapi.h" #include "channel/channelsinkapi.h"
#include "dsp/interpolator.h" #include "dsp/interpolator.h"
#include "dsp/ncof.h" #include "dsp/ncof.h"
#include "dsp/fftfilt.h" #include "dsp/fftfilt.h"
#include "dsp/phaselockcomplex.h" #include "dsp/phaselockcomplex.h"
#include "audio/audiofifo.h" #include "audio/audiofifo.h"
#include "util/message.h" #include "util/message.h"
#define ssbFftLen 1024 #define ssbFftLen 1024
class DeviceSourceAPI; class DeviceSourceAPI;
class ThreadedBasebandSampleSink; class ThreadedBasebandSampleSink;
class DownChannelizer; class DownChannelizer;
class ChannelAnalyzerNG : public BasebandSampleSink, public ChannelSinkAPI { class ChannelAnalyzerNG : public BasebandSampleSink, public ChannelSinkAPI {
public: public:
class MsgConfigureChannelAnalyzer : public Message { class MsgConfigureChannelAnalyzer : public Message {
MESSAGE_CLASS_DECLARATION MESSAGE_CLASS_DECLARATION
public: public:
int getChannelSampleRate() const { return m_channelSampleRate; } int getChannelSampleRate() const { return m_channelSampleRate; }
Real getBandwidth() const { return m_Bandwidth; } Real getBandwidth() const { return m_Bandwidth; }
Real getLoCutoff() const { return m_LowCutoff; } Real getLoCutoff() const { return m_LowCutoff; }
int getSpanLog2() const { return m_spanLog2; } int getSpanLog2() const { return m_spanLog2; }
bool getSSB() const { return m_ssb; } bool getSSB() const { return m_ssb; }
bool getPLL() const { return m_pll; } bool getPLL() const { return m_pll; }
unsigned int getPLLPSKOrder() const { return m_pllPskOrder; } unsigned int getPLLPSKOrder() const { return m_pllPskOrder; }
static MsgConfigureChannelAnalyzer* create( static MsgConfigureChannelAnalyzer* create(
int channelSampleRate, int channelSampleRate,
Real Bandwidth, Real Bandwidth,
Real LowCutoff, Real LowCutoff,
int spanLog2, int spanLog2,
bool ssb, bool ssb,
bool pll, bool pll,
unsigned int pllPskOrder) unsigned int pllPskOrder)
{ {
return new MsgConfigureChannelAnalyzer( return new MsgConfigureChannelAnalyzer(
channelSampleRate, channelSampleRate,
Bandwidth, Bandwidth,
LowCutoff, LowCutoff,
spanLog2, spanLog2,
ssb, ssb,
pll, pll,
pllPskOrder); pllPskOrder);
} }
private: private:
int m_channelSampleRate; int m_channelSampleRate;
Real m_Bandwidth; Real m_Bandwidth;
Real m_LowCutoff; Real m_LowCutoff;
int m_spanLog2; int m_spanLog2;
bool m_ssb; bool m_ssb;
bool m_pll; bool m_pll;
unsigned int m_pllPskOrder; unsigned int m_pllPskOrder;
MsgConfigureChannelAnalyzer( MsgConfigureChannelAnalyzer(
int channelSampleRate, int channelSampleRate,
Real Bandwidth, Real Bandwidth,
Real LowCutoff, Real LowCutoff,
int spanLog2, int spanLog2,
bool ssb, bool ssb,
bool pll, bool pll,
unsigned int pllPskOrder) : unsigned int pllPskOrder) :
Message(), Message(),
m_channelSampleRate(channelSampleRate), m_channelSampleRate(channelSampleRate),
m_Bandwidth(Bandwidth), m_Bandwidth(Bandwidth),
m_LowCutoff(LowCutoff), m_LowCutoff(LowCutoff),
m_spanLog2(spanLog2), m_spanLog2(spanLog2),
m_ssb(ssb), m_ssb(ssb),
m_pll(pll), m_pll(pll),
m_pllPskOrder(pllPskOrder) m_pllPskOrder(pllPskOrder)
{ } { }
}; };
class MsgConfigureChannelizer : public Message { class MsgConfigureChannelizer : public Message {
MESSAGE_CLASS_DECLARATION MESSAGE_CLASS_DECLARATION
public: public:
int getSampleRate() const { return m_sampleRate; } int getSampleRate() const { return m_sampleRate; }
int getCenterFrequency() const { return m_centerFrequency; } int getCenterFrequency() const { return m_centerFrequency; }
static MsgConfigureChannelizer* create(int sampleRate, int centerFrequency) static MsgConfigureChannelizer* create(int sampleRate, int centerFrequency)
{ {
return new MsgConfigureChannelizer(sampleRate, centerFrequency); return new MsgConfigureChannelizer(sampleRate, centerFrequency);
} }
private: private:
int m_sampleRate; int m_sampleRate;
int m_centerFrequency; int m_centerFrequency;
MsgConfigureChannelizer(int sampleRate, int centerFrequency) : MsgConfigureChannelizer(int sampleRate, int centerFrequency) :
Message(), Message(),
m_sampleRate(sampleRate), m_sampleRate(sampleRate),
m_centerFrequency(centerFrequency) m_centerFrequency(centerFrequency)
{ } { }
}; };
class MsgReportChannelSampleRateChanged : public Message { class MsgReportChannelSampleRateChanged : public Message {
MESSAGE_CLASS_DECLARATION MESSAGE_CLASS_DECLARATION
public: public:
static MsgReportChannelSampleRateChanged* create() static MsgReportChannelSampleRateChanged* create()
{ {
return new MsgReportChannelSampleRateChanged(); return new MsgReportChannelSampleRateChanged();
} }
private: private:
MsgReportChannelSampleRateChanged() : MsgReportChannelSampleRateChanged() :
Message() Message()
{ } { }
}; };
ChannelAnalyzerNG(DeviceSourceAPI *deviceAPI); ChannelAnalyzerNG(DeviceSourceAPI *deviceAPI);
virtual ~ChannelAnalyzerNG(); virtual ~ChannelAnalyzerNG();
virtual void destroy() { delete this; } virtual void destroy() { delete this; }
void setSampleSink(BasebandSampleSink* sampleSink) { m_sampleSink = sampleSink; } void setSampleSink(BasebandSampleSink* sampleSink) { m_sampleSink = sampleSink; }
void configure(MessageQueue* messageQueue, void configure(MessageQueue* messageQueue,
int channelSampleRate, int channelSampleRate,
Real Bandwidth, Real Bandwidth,
Real LowCutoff, Real LowCutoff,
int spanLog2, int spanLog2,
bool ssb, bool ssb,
bool pll, bool pll,
unsigned int pllPskOrder); unsigned int pllPskOrder);
DownChannelizer *getChannelizer() { return m_channelizer; } DownChannelizer *getChannelizer() { return m_channelizer; }
int getInputSampleRate() const { return m_running.m_inputSampleRate; } int getInputSampleRate() const { return m_running.m_inputSampleRate; }
int getChannelSampleRate() const { return m_running.m_channelSampleRate; } int getChannelSampleRate() const { return m_running.m_channelSampleRate; }
double getMagSq() const { return m_magsq; } double getMagSq() const { return m_magsq; }
bool isPllLocked() const { return m_running.m_pll && m_pll.locked(); } bool isPllLocked() const { return m_running.m_pll && m_pll.locked(); }
Real getPllFrequency() const { return m_pll.getFrequency(); } Real getPllDeltaPhase() const { return m_pll.getDeltaPhi(); }
Real getPllDeltaPhase() const { return m_pll.getDeltaPhi(); } Real getPllPhase() const { return m_pll.getPhiHat(); }
Real getPllPhase() const { return m_pll.getPhiHat(); }
virtual void feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end, bool positiveOnly);
virtual void feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end, bool positiveOnly); virtual void start();
virtual void start(); virtual void stop();
virtual void stop(); virtual bool handleMessage(const Message& cmd);
virtual bool handleMessage(const Message& cmd);
virtual void getIdentifier(QString& id) { id = objectName(); }
virtual void getIdentifier(QString& id) { id = objectName(); } virtual void getTitle(QString& title) { title = objectName(); }
virtual void getTitle(QString& title) { title = objectName(); } virtual qint64 getCenterFrequency() const { return m_running.m_frequency; }
virtual qint64 getCenterFrequency() const { return m_running.m_frequency; }
virtual QByteArray serialize() const { return QByteArray(); }
virtual QByteArray serialize() const { return QByteArray(); } virtual bool deserialize(const QByteArray& data __attribute__((unused))) { return false; }
virtual bool deserialize(const QByteArray& data __attribute__((unused))) { return false; }
static const QString m_channelIdURI;
static const QString m_channelIdURI; static const QString m_channelId;
static const QString m_channelId;
private:
private:
struct Config
struct Config {
{ int m_frequency;
int m_frequency; int m_inputSampleRate;
int m_inputSampleRate; int m_channelSampleRate;
int m_channelSampleRate; Real m_Bandwidth;
Real m_Bandwidth; Real m_LowCutoff;
Real m_LowCutoff; int m_spanLog2;
int m_spanLog2; bool m_ssb;
bool m_ssb; bool m_pll;
bool m_pll; unsigned int m_pllPskOrder;
unsigned int m_pllPskOrder;
Config() :
Config() : m_frequency(0),
m_frequency(0), m_inputSampleRate(96000),
m_inputSampleRate(96000), m_channelSampleRate(96000),
m_channelSampleRate(96000), m_Bandwidth(5000),
m_Bandwidth(5000), m_LowCutoff(300),
m_LowCutoff(300), m_spanLog2(3),
m_spanLog2(3), m_ssb(false),
m_ssb(false), m_pll(false),
m_pll(false), m_pllPskOrder(1)
m_pllPskOrder(1) {}
{} };
};
Config m_config;
Config m_config; Config m_running;
Config m_running;
DeviceSourceAPI *m_deviceAPI;
DeviceSourceAPI *m_deviceAPI; ThreadedBasebandSampleSink* m_threadedChannelizer;
ThreadedBasebandSampleSink* m_threadedChannelizer; DownChannelizer* m_channelizer;
DownChannelizer* m_channelizer;
int m_undersampleCount;
int m_undersampleCount; fftfilt::cmplx m_sum;
fftfilt::cmplx m_sum; bool m_usb;
bool m_usb; double m_magsq;
double m_magsq; bool m_useInterpolator;
bool m_useInterpolator;
NCOF m_nco;
NCOF m_nco; PhaseLockComplex m_pll;
PhaseLockComplex m_pll; Interpolator m_interpolator;
Interpolator m_interpolator; Real m_interpolatorDistance;
Real m_interpolatorDistance; Real m_interpolatorDistanceRemain;
Real m_interpolatorDistanceRemain;
fftfilt* SSBFilter;
fftfilt* SSBFilter; fftfilt* DSBFilter;
fftfilt* DSBFilter;
BasebandSampleSink* m_sampleSink;
BasebandSampleSink* m_sampleSink; SampleVector m_sampleBuffer;
SampleVector m_sampleBuffer; QMutex m_settingsMutex;
QMutex m_settingsMutex;
void apply(bool force = false);
void apply(bool force = false);
void processOneSample(Complex& c, fftfilt::cmplx *sideband)
void processOneSample(Complex& c, fftfilt::cmplx *sideband) {
{ int n_out;
int n_out; int decim = 1<<m_running.m_spanLog2;
int decim = 1<<m_running.m_spanLog2;
if (m_running.m_ssb)
if (m_running.m_ssb) {
{ n_out = SSBFilter->runSSB(c, &sideband, m_usb);
n_out = SSBFilter->runSSB(c, &sideband, m_usb); }
} else
else {
{ n_out = DSBFilter->runDSB(c, &sideband);
n_out = DSBFilter->runDSB(c, &sideband); }
}
for (int i = 0; i < n_out; i++)
for (int i = 0; i < n_out; i++) {
{ // Downsample by 2^(m_scaleLog2 - 1) for SSB band spectrum display
// Downsample by 2^(m_scaleLog2 - 1) for SSB band spectrum display // smart decimation with bit gain using float arithmetic (23 bits significand)
// smart decimation with bit gain using float arithmetic (23 bits significand)
m_sum += sideband[i];
m_sum += sideband[i];
if (!(m_undersampleCount++ & (decim - 1))) // counter LSB bit mask for decimation by 2^(m_scaleLog2 - 1)
if (!(m_undersampleCount++ & (decim - 1))) // counter LSB bit mask for decimation by 2^(m_scaleLog2 - 1) {
{ m_sum /= decim;
m_sum /= decim; Real re = m_sum.real() / SDR_RX_SCALED;
Real re = m_sum.real() / SDR_RX_SCALED; Real im = m_sum.imag() / SDR_RX_SCALED;
Real im = m_sum.imag() / SDR_RX_SCALED; m_magsq = re*re + im*im;
m_magsq = re*re + im*im;
if (m_running.m_pll)
if (m_running.m_pll) {
{ m_pll.feed(re, im);
m_pll.feed(re, im);
// Use -fPLL to mix (exchange PLL real and image in the complex multiplication)
// Use -fPLL to mix (exchange PLL real and image in the complex multiplication) Real mixI = m_sum.real() * m_pll.getImag() - m_sum.imag() * m_pll.getReal();
// Real mixI = m_sum.real() * m_pll.getImag() - m_sum.imag() * m_pll.getReal(); Real mixQ = m_sum.real() * m_pll.getReal() + m_sum.imag() * m_pll.getImag();
// Real mixQ = m_sum.real() * m_pll.getReal() + m_sum.imag() * m_pll.getImag(); // Real mixI = m_pll.getReal() * SDR_RX_SCALED;
Real mixI = m_pll.getReal() * SDR_RX_SCALED; // Real mixQ = m_pll.getImag() * SDR_RX_SCALED;
Real mixQ = m_pll.getImag() * SDR_RX_SCALED;
if (m_running.m_ssb & !m_usb)
if (m_running.m_ssb & !m_usb) { // invert spectrum for LSB
{ // invert spectrum for LSB m_sampleBuffer.push_back(Sample(mixQ, mixI));
m_sampleBuffer.push_back(Sample(mixQ, mixI)); }
} else
else {
{ m_sampleBuffer.push_back(Sample(mixI, mixQ));
m_sampleBuffer.push_back(Sample(mixI, mixQ)); }
} }
} else
else {
{ if (m_running.m_ssb & !m_usb)
if (m_running.m_ssb & !m_usb) { // invert spectrum for LSB
{ // invert spectrum for LSB m_sampleBuffer.push_back(Sample(m_sum.imag(), m_sum.real()));
m_sampleBuffer.push_back(Sample(m_sum.imag(), m_sum.real())); }
} else
else {
{ m_sampleBuffer.push_back(Sample(m_sum.real(), m_sum.imag()));
m_sampleBuffer.push_back(Sample(m_sum.real(), m_sum.imag())); }
} }
}
m_sum = 0;
m_sum = 0; }
} }
} }
} };
};
#endif // INCLUDE_CHANALYZERNG_H
#endif // INCLUDE_CHANALYZERNG_H

13
plugins/channelrx/chanalyzerng/chanalyzernggui.cpp
View File

@ -237,15 +237,6 @@ void ChannelAnalyzerNGGUI::tick()
} else { } else {
ui->pll->setStyleSheet("QToolButton { background:rgb(79,79,79); }"); ui->pll->setStyleSheet("QToolButton { background:rgb(79,79,79); }");
} }
if (ui->pll->isChecked())
{
int fHz = round(m_channelAnalyzer->getPllFrequency()*m_rate);
ui->pll->setToolTip(tr("PLL lock (f:%1 Hz e:%2 rad p:%3 rad)")
.arg(fHz)
.arg(m_channelAnalyzer->getPllDeltaPhase())
.arg(m_channelAnalyzer->getPllPhase()));
}
} }
void ChannelAnalyzerNGGUI::on_channelSampleRate_changed(quint64 value) void ChannelAnalyzerNGGUI::on_channelSampleRate_changed(quint64 value)
@ -262,10 +253,6 @@ void ChannelAnalyzerNGGUI::on_channelSampleRate_changed(quint64 value)
void ChannelAnalyzerNGGUI::on_pll_toggled(bool checked) void ChannelAnalyzerNGGUI::on_pll_toggled(bool checked)
{ {
if (!checked && m_usePll) {
ui->pll->setToolTip("PLL lock");
}
m_usePll = checked; m_usePll = checked;
applySettings(); applySettings();
} }

5
plugins/channelrx/demodam/amdemod.cpp
View File

@ -82,7 +82,7 @@ AMDemod::AMDemod(DeviceSourceAPI *deviceAPI) :
m_deviceAPI->addThreadedSink(m_threadedChannelizer); m_deviceAPI->addThreadedSink(m_threadedChannelizer);
m_deviceAPI->addChannelAPI(this); m_deviceAPI->addChannelAPI(this);
m_pllFilt.create(101, m_audioSampleRate, 500.0); m_pllFilt.create(101, m_audioSampleRate, 200.0);
m_pll.computeCoefficients(0.05, 0.707, 1000); m_pll.computeCoefficients(0.05, 0.707, 1000);
m_syncAMBuffIndex = 0; m_syncAMBuffIndex = 0;
} }
@ -374,7 +374,7 @@ void AMDemod::applyAudioSampleRate(int sampleRate)
m_audioFifo.setSize(sampleRate); m_audioFifo.setSize(sampleRate);
m_squelchDelayLine.resize(sampleRate/5); m_squelchDelayLine.resize(sampleRate/5);
DSBFilter->create_dsb_filter((2.0f * m_settings.m_rfBandwidth) / (float) sampleRate); DSBFilter->create_dsb_filter((2.0f * m_settings.m_rfBandwidth) / (float) sampleRate);
m_pllFilt.create(101, sampleRate, 500.0); m_pllFilt.create(101, sampleRate, 200.0);
if (m_settings.m_pll) { if (m_settings.m_pll) {
m_volumeAGC.resizeNew(sampleRate, 0.003); m_volumeAGC.resizeNew(sampleRate, 0.003);
@ -383,6 +383,7 @@ void AMDemod::applyAudioSampleRate(int sampleRate)
} }
m_syncAMAGC.resize(sampleRate/4, sampleRate/8, 0.1); m_syncAMAGC.resize(sampleRate/4, sampleRate/8, 0.1);
m_pll.setSampleRate(sampleRate);
m_settingsMutex.unlock(); m_settingsMutex.unlock();
m_audioSampleRate = sampleRate; m_audioSampleRate = sampleRate;

87
sdrbase/dsp/phaselockcomplex.cpp
View File

@ -44,9 +44,14 @@ PhaseLockComplex::PhaseLockComplex() :
m_yRe(1.0), m_yRe(1.0),
m_yIm(0.0), m_yIm(0.0),
m_freq(0.0), m_freq(0.0),
m_freqPrev(0.0),
m_lock(0.0), m_lock(0.0),
m_lockCount(0), m_lockCount(0),
m_pskOrder(1) m_pskOrder(1),
m_lockTime1(480),
m_lockTime(2400),
m_lockTimef(2400.0f),
m_lockThreshold(4.8f)
{ {
} }
@ -83,6 +88,16 @@ void PhaseLockComplex::computeCoefficients(Real wn, Real zeta, Real K)
void PhaseLockComplex::setPskOrder(unsigned int order) void PhaseLockComplex::setPskOrder(unsigned int order)
{ {
m_pskOrder = order > 0 ? order : 1; m_pskOrder = order > 0 ? order : 1;
reset();
}
void PhaseLockComplex::setSampleRate(unsigned int sampleRate)
{
m_lockTime1 = sampleRate / 100; // 10ms for order 1
m_lockTime = sampleRate / 20; // 50ms for order > 1
m_lockTimef = (float) m_lockTime;
m_lockThreshold = m_lockTime * 0.002f; // threshold of 0.002 taking division by lock time into account
reset();
} }
void PhaseLockComplex::reset() void PhaseLockComplex::reset()
@ -103,6 +118,7 @@ void PhaseLockComplex::reset()
m_yRe = 1.0f; m_yRe = 1.0f;
m_yIm = 0.0f; m_yIm = 0.0f;
m_freq = 0.0f; m_freq = 0.0f;
m_freqPrev = 0.0f;
m_lock = 0.0f; m_lock = 0.0f;
m_lockCount = 0; m_lockCount = 0;
} }
@ -148,40 +164,69 @@ void PhaseLockComplex::feed(float re, float im)
m_phiHat += 2.0*M_PI; m_phiHat += 2.0*M_PI;
} }
float dPhi = normalizeAngle(m_phiHat - m_phiHatPrev); // lock estimation
m_phiHatPrev = m_phiHat; if (m_pskOrder > 1)
if (m_phiHatCount < 9)
{ {
m_dPhiHatAccum += dPhi; float dPhi = normalizeAngle(m_phiHat - m_phiHatPrev);
if (m_phiHatCount < (m_lockTime-1))
{
m_dPhiHatAccum += dPhi; // re-accumulate phase for differential calculation
m_phiHatCount++;
}
else
{
float dPhi11 = (m_dPhiHatAccum - m_phiHat1); // optimized out division by lock time
float dPhi12 = (m_phiHat1 - m_phiHat2);
m_lock = dPhi11 - dPhi12; // second derivative of phase to get lock status
if ((m_lock > -m_lockThreshold) && (m_lock < m_lockThreshold)) // includes re-multiplication by lock time
{
if (m_lockCount < 20) { // [0..20]
m_lockCount++;
}
}
else
{
if (m_lockCount > 0) {
m_lockCount -= 2;
}
}
m_phiHat2 = m_phiHat1;
m_phiHat1 = m_dPhiHatAccum;
m_dPhiHatAccum = 0.0f;
m_phiHatCount = 0;
}
m_phiHatPrev = m_phiHat;
} }
else else
{ {
float dPhi1 = (m_phiHat1 - m_dPhiHatAccum) / 10.0f; m_freq = (m_phiHat - m_phiHatPrev) / (2.0*M_PI);
float dPhi1Prev = (m_phiHat2 - m_phiHat1) / 10.0f;
m_lock = dPhi1 - dPhi1Prev; // second derivative of phase
if ((m_lock > -0.01) && (m_lock < 0.01)) if (m_freq < -1.0f) {
m_freq += 2.0f;
} else if (m_freq > 1.0f) {
m_freq -= 2.0f;
}
float dFreq = m_freq - m_freqPrev;
if ((dFreq > -0.01) && (dFreq < 0.01))
{ {
if (m_lockCount < 1000) { if (m_lockCount < (m_lockTime1-1)) { // [0..479]
m_lockCount++; m_lockCount++;
} }
} }
else else
{ {
if (m_lockCount > 0) { m_lockCount = 0;
m_lockCount--;
}
} }
m_freq = dPhi1 / 2.0*M_PI; // first derivative of phase m_phiHatPrev = m_phiHat;
m_phiHat2 = m_phiHat1; m_freqPrev = m_freq;
m_phiHat1 = m_dPhiHatAccum;
m_dPhiHatAccum = 0.0f;
m_phiHatCount = 0;
} }
m_dPhiHatAccum += dPhi;
} }
float PhaseLockComplex::normalizeAngle(float angle) float PhaseLockComplex::normalizeAngle(float angle)

10
sdrbase/dsp/phaselockcomplex.h
View File

@ -41,13 +41,14 @@ public:
* \param order 0,1: no PSK (CW), 2: BPSK, 4: QPSK, 8: 8-PSK, ... use powers of two for real cases * \param order 0,1: no PSK (CW), 2: BPSK, 4: QPSK, 8: 8-PSK, ... use powers of two for real cases
*/ */
void setPskOrder(unsigned int order); void setPskOrder(unsigned int order);
/** Set sample rate information only for frequency and lock condition calculation */
void setSampleRate(unsigned int sampleRate);
void reset(); void reset();
void feed(float re, float im); void feed(float re, float im);
const std::complex<float>& getComplex() const { return m_y; } const std::complex<float>& getComplex() const { return m_y; }
float getReal() const { return m_yRe; } float getReal() const { return m_yRe; }
float getImag() const { return m_yIm; } float getImag() const { return m_yIm; }
bool locked() const { return m_lockCount > 500; } bool locked() const { return m_lockCount > (m_pskOrder > 1 ? 15 : (m_lockTime1-2)); } // 6
float getFrequency() const { return m_freq; }
float getDeltaPhi() const { return m_deltaPhi; } float getDeltaPhi() const { return m_deltaPhi; }
float getPhiHat() const { return m_phiHat; } float getPhiHat() const { return m_phiHat; }
@ -75,9 +76,14 @@ private:
float m_yRe; float m_yRe;
float m_yIm; float m_yIm;
float m_freq; float m_freq;
float m_freqPrev;
float m_lock; float m_lock;
int m_lockCount; int m_lockCount;
unsigned int m_pskOrder; unsigned int m_pskOrder;
int m_lockTime1;
int m_lockTime;
float m_lockTimef;
float m_lockThreshold;
}; };