VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2024-11-26 01:39:05 -05:00
Code Issues Projects Releases Wiki Activity

Implemented phase lock with configurable outputs

Browse Source
This commit is contained in:
Edouard Griffiths 2015-12-07 18:20:27 +01:00
parent 6d3e18f79c
commit ea5cdb034f
2 changed files with 175 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
include/dsp/phaselock.h
View File

@ -15,6 +15,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
#include <vector>
#include "dsp/dsptypes.h"
/** Phase-locked loop mainly for broadcadt FM stereo pilot. */
@ -43,6 +44,9 @@ public:
*/
PhaseLock(Real freq, Real bandwidth, Real minsignal);
virtual ~PhaseLock()
{}
/**
* Change phase locked loop parameters
*
@ -61,12 +65,21 @@ public:
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.
* Process samples and extract pilot tone. Generate phase-locked twice
* the frequency tone with unit amplitude. Mostly useful for 19 kHz stereo
* pilot tone on broadcast FM.
* In flow version
*/
void process(const Real& sample_in, Real& sample_out);
/**
* Process samples and track a pilot tone. Generate samples for multiple phase-locked
* signals. Implement the processPhase virtual method to produce the output samples.
* In flow version. Ex: Use 19 kHz stereo pilot tone to generate 38 kHz (stereo) and 57 kHz
* pilots (see RDSPhaseLock class below).
*/
void process(const Real& sample_in, std::vector<Real>& samples_out);
/** Return true if the phase-locked loop is locked. */
bool locked() const
{
@ -79,13 +92,23 @@ public:
return 2 * m_pilot_level;
}
protected:
Real m_phase;
Real m_psin;
Real m_pcos;
/**
* Callback method to produce multiple outputs from the current phase value in m_phase
* and/or the sin and cos values in m_psin and m_pcos
*/
virtual void processPhase(std::vector<Real>& samples_out) const {};
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_freq;
Real m_minsignal;
Real m_pilot_level;
int m_lock_delay;
@ -97,3 +120,45 @@ private:
quint64 m_sample_cnt;
std::vector<PpsEvent> m_pps_events;
};
class StereoPhaseLock : public PhaseLock
{
public:
StereoPhaseLock(Real freq, Real bandwidth, Real minsignal) :
PhaseLock(freq, bandwidth, minsignal)
{}
virtual ~StereoPhaseLock()
{}
protected:
virtual void processPhase(std::vector<Real>& samples_out) const
{
samples_out[0] = m_psin; // f Pilot
// Generate double-frequency output.
// sin(2*x) = 2 * sin(x) * cos(x)
samples_out[1] = 2.0 * m_psin * m_pcos; // 2f Pilot
}
};
class RDSPhaseLock : public PhaseLock
{
public:
RDSPhaseLock(Real freq, Real bandwidth, Real minsignal) :
PhaseLock(freq, bandwidth, minsignal)
{}
virtual ~RDSPhaseLock()
{}
protected:
virtual void processPhase(std::vector<Real>& samples_out) const
{
samples_out[0] = m_psin; // f Pilot
// Generate double-frequency output.
// sin(2*x) = 2 * sin(x) * cos(x)
samples_out[1] = 2.0 * m_psin * m_pcos; // 2f Pilot
samples_out[2] = sin(3.0 * m_phase); // 3f pilot
}
};

107
sdrbase/dsp/phaselock.cpp
View File

@ -45,6 +45,8 @@ PhaseLock::PhaseLock(Real freq, Real bandwidth, Real minsignal)
m_lock_cnt = 0;
m_unlock_cnt = 0;
m_pilot_level = 0;
m_psin = 0.0;
m_pcos = 1.0;
// Create 2nd order filter for I/Q representation of phase error.
// Filter has two poles, unit DC gain.
@ -377,3 +379,108 @@ void PhaseLock::process(const Real& sample_in, Real& sample_out)
// Update sample counter.
m_sample_cnt += 1; // n
}
// Process samples. Multiple output
void PhaseLock::process(const Real& sample_in, std::vector<Real>& samples_out)
{
bool was_locked = (m_lock_cnt >= m_lock_delay);
m_pps_events.clear();
// Generate locked pilot tone.
m_psin = sin(m_phase);
m_pcos = cos(m_phase);
// Generate output
processPhase(samples_out);
// Multiply locked tone with input.
Real x = sample_in;
Real phasor_i = m_psin * x;
Real phasor_q = m_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;
}
}
// 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
}