mirror of
https://github.com/f4exb/sdrangel.git
synced 2024-11-26 01:39:05 -05:00
BFM demod: PLL lock pilot carrier in flow
This commit is contained in:
parent
f69e69a799
commit
baccaba2c8
@ -25,6 +25,14 @@ public:
|
||||
/** Expected pilot frequency (used for PPS events). */
|
||||
static const int pilot_frequency = 19000;
|
||||
|
||||
/** Timestamp event produced once every 19000 pilot periods. */
|
||||
struct PpsEvent
|
||||
{
|
||||
quint64 pps_index;
|
||||
quint64 sample_index;
|
||||
double block_position;
|
||||
};
|
||||
|
||||
/**
|
||||
* Construct phase-locked loop.
|
||||
*
|
||||
@ -35,12 +43,30 @@ public:
|
||||
*/
|
||||
PhaseLock(Real freq, Real bandwidth, Real minsignal);
|
||||
|
||||
/**
|
||||
* Change phase locked loop parameters
|
||||
*
|
||||
* freq :: 19 kHz center frequency relative to sample freq
|
||||
* (0.5 is Nyquist)
|
||||
* bandwidth :: bandwidth relative to sample frequency
|
||||
* minsignal :: minimum pilot amplitude
|
||||
*/
|
||||
void configure(Real freq, Real bandwidth, Real minsignal);
|
||||
|
||||
/**
|
||||
* Process samples and extract 19 kHz pilot tone.
|
||||
* Generate phase-locked 38 kHz tone with unit amplitude.
|
||||
* Bufferized version with input and output vectors
|
||||
*/
|
||||
void process(const std::vector<Real>& samples_in, std::vector<Real>& samples_out);
|
||||
|
||||
/**
|
||||
* Process samples and extract 19 kHz pilot tone.
|
||||
* Generate phase-locked 38 kHz tone with unit amplitude.
|
||||
* In flow version
|
||||
*/
|
||||
void process(const Real& sample_in, Real& sample_out);
|
||||
|
||||
/** Return true if the phase-locked loop is locked. */
|
||||
bool locked() const
|
||||
{
|
||||
@ -64,4 +90,10 @@ private:
|
||||
Real m_pilot_level;
|
||||
int m_lock_delay;
|
||||
int m_lock_cnt;
|
||||
int m_unlock_cnt;
|
||||
int m_unlock_delay;
|
||||
int m_pilot_periods;
|
||||
quint64 m_pps_cnt;
|
||||
quint64 m_sample_cnt;
|
||||
std::vector<PpsEvent> m_pps_events;
|
||||
};
|
||||
|
@ -31,7 +31,8 @@ MESSAGE_CLASS_DEFINITION(BFMDemod::MsgConfigureBFMDemod, Message)
|
||||
BFMDemod::BFMDemod(SampleSink* sampleSink) :
|
||||
m_sampleSink(sampleSink),
|
||||
m_audioFifo(4, 250000),
|
||||
m_settingsMutex(QMutex::Recursive)
|
||||
m_settingsMutex(QMutex::Recursive),
|
||||
m_pilotPLL(19000/384000, 50/384000, 0.01)
|
||||
{
|
||||
setObjectName("BFMDemod");
|
||||
|
||||
@ -118,7 +119,10 @@ 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));
|
||||
Real pilotSample;
|
||||
m_pilotPLL.process(demod, pilotSample);
|
||||
//m_sampleBuffer.push_back(Sample(demod * (1<<15), 0.0));
|
||||
m_sampleBuffer.push_back(Sample(pilotSample * (1<<15), 0.0));
|
||||
Complex e(demod, 0);
|
||||
|
||||
if(m_interpolator.interpolate(&m_interpolatorDistanceRemain, e, &ci))
|
||||
@ -230,6 +234,10 @@ bool BFMDemod::handleMessage(const Message& cmd)
|
||||
|
||||
void BFMDemod::apply()
|
||||
{
|
||||
if (m_config.m_inputSampleRate != m_running.m_inputSampleRate)
|
||||
{
|
||||
m_pilotPLL.configure(19000.0/m_config.m_inputSampleRate, 50.0/m_config.m_inputSampleRate, 0.01);
|
||||
}
|
||||
|
||||
if((m_config.m_inputFrequencyOffset != m_running.m_inputFrequencyOffset) ||
|
||||
(m_config.m_inputSampleRate != m_running.m_inputSampleRate))
|
||||
|
@ -26,6 +26,7 @@
|
||||
#include "dsp/lowpass.h"
|
||||
#include "dsp/movingaverage.h"
|
||||
#include "dsp/fftfilt.h"
|
||||
#include "dsp/phaselock.h"
|
||||
#include "audio/audiofifo.h"
|
||||
#include "util/message.h"
|
||||
|
||||
@ -45,6 +46,8 @@ public:
|
||||
virtual bool handleMessage(const Message& cmd);
|
||||
|
||||
Real getMagSq() const { return m_movingAverage.average(); }
|
||||
bool getPilotLock() const { return m_pilotPLL.locked(); }
|
||||
Real getPilotLevel() const { return m_pilotPLL.get_pilot_level(); }
|
||||
|
||||
private:
|
||||
class MsgConfigureBFMDemod : public Message {
|
||||
@ -133,6 +136,8 @@ private:
|
||||
SampleVector m_sampleBuffer;
|
||||
QMutex m_settingsMutex;
|
||||
|
||||
PhaseLock m_pilotPLL;
|
||||
|
||||
void apply();
|
||||
};
|
||||
|
||||
|
@ -334,4 +334,5 @@ void BFMDemodGUI::tick()
|
||||
Real powDb = CalcDb::dbPower(m_bfmDemod->getMagSq());
|
||||
m_channelPowerDbAvg.feed(powDb);
|
||||
ui->channelPower->setText(QString::number(m_channelPowerDbAvg.average(), 'f', 1));
|
||||
//qDebug() << "Pilot lock: " << m_bfmDemod->getPilotLock() << ":" << m_bfmDemod->getPilotLevel(); TODO: update a GUI item with status
|
||||
}
|
||||
|
@ -41,19 +41,21 @@ PhaseLock::PhaseLock(Real freq, Real bandwidth, Real minsignal)
|
||||
// Set valid signal threshold.
|
||||
m_minsignal = minsignal;
|
||||
m_lock_delay = int(20.0 / bandwidth);
|
||||
m_unlock_delay = int(10.0 / bandwidth);
|
||||
m_lock_cnt = 0;
|
||||
m_unlock_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);
|
||||
double p1 = exp(-1.146 * bandwidth * 2.0 * M_PI);
|
||||
double 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);
|
||||
double 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;
|
||||
|
||||
@ -70,10 +72,75 @@ PhaseLock::PhaseLock(Real freq, Real bandwidth, Real minsignal)
|
||||
m_phasor_q1 = 0;
|
||||
m_phasor_q2 = 0;
|
||||
m_loopfilter_x1 = 0;
|
||||
|
||||
// Initialize PPS generator.
|
||||
m_pilot_periods = 0;
|
||||
m_pps_cnt = 0;
|
||||
m_sample_cnt = 0;
|
||||
}
|
||||
|
||||
void PhaseLock::configure(Real freq, Real bandwidth, Real minsignal)
|
||||
{
|
||||
qDebug("PhaseLock::configure: freq: %f bandwidth: %f minsignal: %f", freq, bandwidth, 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.
|
||||
*/
|
||||
|
||||
// Process samples.
|
||||
// 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_unlock_delay = int(10.0 / bandwidth);
|
||||
m_lock_cnt = 0;
|
||||
m_unlock_cnt = 0;
|
||||
m_pilot_level = 0;
|
||||
|
||||
// Create 2nd order filter for I/Q representation of phase error.
|
||||
// Filter has two poles, unit DC gain.
|
||||
double p1 = exp(-1.146 * bandwidth * 2.0 * M_PI);
|
||||
double 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.
|
||||
double 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;
|
||||
|
||||
// Initialize PPS generator.
|
||||
m_pilot_periods = 0;
|
||||
m_pps_cnt = 0;
|
||||
m_sample_cnt = 0;
|
||||
}
|
||||
|
||||
// Process samples. Bufferized version
|
||||
void PhaseLock::process(const std::vector<Real>& samples_in, std::vector<Real>& samples_out)
|
||||
{
|
||||
unsigned int n = samples_in.size();
|
||||
@ -81,6 +148,7 @@ void PhaseLock::process(const std::vector<Real>& samples_in, std::vector<Real>&
|
||||
samples_out.resize(n);
|
||||
|
||||
bool was_locked = (m_lock_cnt >= m_lock_delay);
|
||||
m_pps_events.clear();
|
||||
|
||||
if (n > 0)
|
||||
m_pilot_level = 1000.0;
|
||||
@ -141,6 +209,20 @@ void PhaseLock::process(const std::vector<Real>& samples_in, std::vector<Real>&
|
||||
m_phase += m_freq;
|
||||
if (m_phase > 2.0 * M_PI) {
|
||||
m_phase -= 2.0 * M_PI;
|
||||
m_pilot_periods++;
|
||||
|
||||
// Generate pulse-per-second.
|
||||
if (m_pilot_periods == pilot_frequency) {
|
||||
m_pilot_periods = 0;
|
||||
if (was_locked) {
|
||||
struct PpsEvent ev;
|
||||
ev.pps_index = m_pps_cnt;
|
||||
ev.sample_index = m_sample_cnt + i;
|
||||
ev.block_position = double(i) / double(n);
|
||||
m_pps_events.push_back(ev);
|
||||
m_pps_cnt++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -152,4 +234,146 @@ void PhaseLock::process(const std::vector<Real>& samples_in, std::vector<Real>&
|
||||
m_lock_cnt = 0;
|
||||
}
|
||||
|
||||
// Drop PPS events when pilot not locked.
|
||||
if (m_lock_cnt < m_lock_delay) {
|
||||
m_pilot_periods = 0;
|
||||
m_pps_cnt = 0;
|
||||
m_pps_events.clear();
|
||||
}
|
||||
|
||||
// Update sample counter.
|
||||
m_sample_cnt += n;
|
||||
}
|
||||
|
||||
/*
|
||||
void PhaseLock::process(const Real& sample_in, Real& sample_out)
|
||||
{
|
||||
m_phase += m_freq;
|
||||
|
||||
if (m_phase > 2.0 * M_PI) {
|
||||
m_phase -= 2.0 * M_PI;
|
||||
}
|
||||
|
||||
Real psin = sin(m_phase);
|
||||
Real pcos = cos(m_phase);
|
||||
|
||||
sample_out = 2 * psin * pcos;
|
||||
}*/
|
||||
|
||||
|
||||
// Process samples.
|
||||
void PhaseLock::process(const Real& sample_in, Real& sample_out)
|
||||
{
|
||||
bool was_locked = (m_lock_cnt >= m_lock_delay);
|
||||
m_pps_events.clear();
|
||||
|
||||
//if (n > 0) m_pilot_level = 1000.0;
|
||||
|
||||
{
|
||||
|
||||
// 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)
|
||||
sample_out = 2 * psin * pcos;
|
||||
|
||||
// Multiply locked tone with input.
|
||||
Real x = sample_in;
|
||||
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);
|
||||
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;
|
||||
m_pilot_periods++;
|
||||
|
||||
// Generate pulse-per-second.
|
||||
if (m_pilot_periods == pilot_frequency) {
|
||||
m_pilot_periods = 0;
|
||||
//if (was_locked) {
|
||||
// struct PpsEvent ev;
|
||||
// ev.pps_index = m_pps_cnt;
|
||||
// ev.sample_index = m_sample_cnt + i;
|
||||
// ev.block_position = double(i) / double(n);
|
||||
// m_pps_events.push_back(ev);
|
||||
// m_pps_cnt++;
|
||||
//}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Update lock status.
|
||||
if (2 * m_pilot_level > m_minsignal)
|
||||
{
|
||||
if (m_lock_cnt < m_lock_delay)
|
||||
{
|
||||
m_lock_cnt += 1; // n
|
||||
}
|
||||
else
|
||||
{
|
||||
m_unlock_cnt = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_unlock_cnt < m_unlock_delay)
|
||||
{
|
||||
m_unlock_cnt += 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lock_cnt = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Drop PPS events when pilot not locked.
|
||||
if (m_lock_cnt < m_lock_delay) {
|
||||
m_pilot_periods = 0;
|
||||
m_pps_cnt = 0;
|
||||
m_pps_events.clear();
|
||||
}
|
||||
|
||||
// Update sample counter.
|
||||
m_sample_cnt += 1; // n
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user