// Duplicate mono signal in left/right channels.
void FmDecoder::mono_to_left_right(const SampleVector& samples_mono,
SampleVector& audio)
{
unsigned int n = samples_mono.size();
audio.resize(2*n);
for (unsigned int i = 0; i < n; i++) {
Sample m = samples_mono[i];
audio[2*i] = m;
audio[2*i+1] = m;
}
}
// Extract left/right channels from (L+R) / (L-R) signals.
void FmDecoder::stereo_to_left_right(const SampleVector& samples_mono,
const SampleVector& samples_stereo,
SampleVector& audio)
{
unsigned int n = samples_mono.size();
assert(n == samples_stereo.size());
audio.resize(2*n);
for (unsigned int i = 0; i < n; i++) {
Sample m = samples_mono[i];
Sample s = samples_stereo[i];
audio[2*i] = m + s;
audio[2*i+1] = m - s;
}
}
// Process samples.
void PhaseDiscriminator::process(const IQSampleVector& samples_in,
SampleVector& samples_out)
{
unsigned int n = samples_in.size();
IQSample s0 = m_last1_sample;
samples_out.resize(n);
for (unsigned int i = 0; i < n; i++) {
IQSample s1(samples_in[i]);
IQSample d(conj(s0) * s1);
//Sample w = atan2(d.imag(), d.real());
Sample w = fastatan2(d.imag(), d.real()); // fast approximation of atan2
samples_out[i] = w * m_freq_scale_factor;
s0 = s1;
}
m_last2_sample = m_last1_sample;
m_last1_sample = s0;
}
// Process samples.
void PilotPhaseLock::process(const SampleVector& samples_in,
SampleVector& samples_out)
{
unsigned int n = samples_in.size();
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;
for (unsigned int i = 0; i < n; i++) {
// Generate locked pilot tone.
Sample psin = sin(m_phase);
Sample 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.
Sample x = samples_in[i];
Sample phasor_i = psin * x;
Sample 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.
Sample 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;
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 += n;
} 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 += n;
}