From 70ba5520ca37e5faa5689435365999fa307be422 Mon Sep 17 00:00:00 2001 From: Joris van Rantwijk Date: Sun, 29 Dec 2013 00:34:13 +0100 Subject: [PATCH] Clean exit on Ctrl-C or SIGTERM. Use double instead of float for real-valued samples. Start implementation of stereo pilot PLL. --- Filter.cc | 3 +- Filter.h | 2 +- FmDecode.cc | 122 ++++++++++++++++++++++++++++++++++++++++++++++++++-- FmDecode.h | 46 +++++++++++++++++--- SoftFM.h | 6 +-- main.cc | 40 +++++++++++++++-- 6 files changed, 201 insertions(+), 18 deletions(-) diff --git a/Filter.cc b/Filter.cc index d0f94c2..6d88613 100644 --- a/Filter.cc +++ b/Filter.cc @@ -11,8 +11,9 @@ using namespace std; /** Prepare Lanczos FIR filter coefficients. */ +template static void make_lanczos_coeff(unsigned int filter_order, double cutoff, - SampleVector& coeff) + vector& coeff) { coeff.resize(filter_order + 1); diff --git a/Filter.h b/Filter.h index 75f6fe4..e36764b 100644 --- a/Filter.h +++ b/Filter.h @@ -48,7 +48,7 @@ public: void process(const IQSampleVector& samples_in, IQSampleVector& samples_out); private: - CoeffVector m_coeff; + std::vector m_coeff; IQSampleVector m_state; }; diff --git a/FmDecode.cc b/FmDecode.cc index fa17e28..9200444 100644 --- a/FmDecode.cc +++ b/FmDecode.cc @@ -33,12 +33,12 @@ static inline Sample fast_atan(Sample x) /** Compute RMS level over a small prefix of the specified sample vector. */ -static Sample rms_level_approx(const IQSampleVector& samples) +static IQSample::value_type rms_level_approx(const IQSampleVector& samples) { unsigned int n = samples.size(); n = (n + 63) / 64; - Sample level = 0; + IQSample::value_type level = 0; for (unsigned int i = 0; i < n; i++) { const IQSample& s = samples[i]; IQSample::value_type re = s.real(), im = s.imag(); @@ -79,6 +79,120 @@ void PhaseDiscriminator::process(const IQSampleVector& samples_in, } +/* **************** class PilotPhaseLock **************** */ + +// Construct phase-locked loop. +PilotPhaseLock::PilotPhaseLock(double freq, double bandwidth, double minsignal) +{ + // This is a type-2, 4th order phase-locked loop. + // I don't understand what I'm doing; hopefully it just works. + + // 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(10.0 / bandwidth); + m_lock_cnt = 0; + + // Create 2nd order filter for IQ representation of phase error + // with both poles at z = exp(-2.5 * bandwidth * 2*PI). + double t = exp(-2.5 * bandwidth * 2.0 * M_PI); + m_iqfilter_a1 = -2.0 * t; + m_iqfilter_a2 = t * t; + m_iqfilter_b0 = m_iqfilter_a1 + m_iqfilter_a2; + + // Create loop filter to stabilize the loop. + // Zero at z = exp(-0.2 * bandwidth * 2*PI), + m_loopfilter_b0 = 1.0; + m_loopfilter_b1 = - exp(-0.2 * bandwidth * 2.0 * M_PI); +// TODO : loop gain + + // Reset frequency and phase. + m_freq = freq * 2.0 * M_PI; + m_phase = 0; + + m_iqfilter_i1 = 0; + m_iqfilter_i2 = 0; + m_iqfilter_q1 = 0; + m_iqfilter_q2 = 0; +} + + +// Process samples. +void PilotPhaseLock::process(const SampleVector& samples_in, + SampleVector& samples_out) +{ + unsigned int n = samples_in.size(); + + samples_out.resize(n); + + for (unsigned int i = 0; i < n; i++) { + + // Generate locked pilot tone. + Sample psin = sin(m_phase); + Sample pcos = cos(m_phase); + samples_out[i] = pcos; + + // Multiply locked tone with input. + Sample x = samples_in[i]; + Sample phasor_i = pcos * x; + Sample phasor_q = psin * x; + + // Run IQ phase error through low-pass filter. + phasor_i = m_iqfilter_b0 * phasor_i + - m_iqfilter_a1 * m_iqfilter_i1 + - m_iqfilter_a2 * m_iqfilter_i2; + phasor_q = m_iqfilter_b0 * phasor_q + - m_iqfilter_a1 * m_iqfilter_q1 + - m_iqfilter_a2 * m_iqfilter_q2; + m_iqfilter_i2 = m_iqfilter_i1; + m_iqfilter_i1 = phasor_i; + m_iqfilter_q2 = m_iqfilter_q1; + m_iqfilter_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 signal threshold. + if (phasor_i > m_minsignal) { + m_lock_cnt++; + } else { + m_lock_cnt = 0; + } + + // 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 = max(m_minfreq, 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; + else if (m_phase > 2.0 * M_PI) + m_phase -= 2.0 * M_PI; + } + + m_lock_cnt = min(m_lock_delay, m_lock_cnt); +} + + /* **************** class FmDecoder **************** */ FmDecoder::FmDecoder(double sample_rate_if, @@ -125,7 +239,7 @@ void FmDecoder::process(const IQSampleVector& samples_in, m_iffilter.process(m_buf_iftuned, m_buf_iffiltered); // Measure IF level. - Sample if_rms = rms_level_approx(m_buf_iffiltered); + double if_rms = rms_level_approx(m_buf_iffiltered); m_if_level = 0.95 * m_if_level + 0.05 * if_rms; // Extract carrier frequency. @@ -138,7 +252,7 @@ void FmDecoder::process(const IQSampleVector& samples_in, } // Measure baseband level. - Sample baseband_mean, baseband_rms; + double baseband_mean, baseband_rms; samples_mean_rms(m_buf_baseband, baseband_mean, baseband_rms); m_baseband_mean = 0.95 * m_baseband_mean + 0.05 * baseband_mean; m_baseband_level = 0.95 * m_baseband_level + 0.05 * baseband_rms; diff --git a/FmDecode.h b/FmDecode.h index 2363192..06c7b57 100644 --- a/FmDecode.h +++ b/FmDecode.h @@ -26,12 +26,46 @@ public: void process(const IQSampleVector& samples_in, SampleVector& samples_out); private: - const Sample m_freq_scale_factor; - IQSample m_last_sample; + const Coeff m_freq_scale_factor; + IQSample m_last_sample; }; -// TODO : maybe CIC downsampling from 1 MS/s to ~ 250 kS/s +/** Phase-locked loop for stereo pilot. */ +class PilotPhaseLock +{ +public: + + /** + * Construct phase-locked loop. + * + * freq :: center frequency of capture range relative to sample freq + * (0.5 is Nyquist) + * bandwidth :: approximate bandwidth relative to sample frequency + * minsignal :: minimum pilot amplitude + */ + PilotPhaseLock(double freq, double bandwidth, double minsignal); + + /** Process samples and extract pilot tone at unit amplitude. */ + void process(const SampleVector& samples_in, SampleVector& samples_out); + + /** Return true if the phase-locked loop is locked. */ + bool locked() const + { + return m_lock_cnt >= m_lock_delay; + } + +private: + Coeff m_minfreq, m_maxfreq; + Coeff m_iqfilter_b0, m_iqfilter_a1, m_iqfilter_a2; + Coeff m_loopfilter_b0, m_loopfilter_b1; + Sample m_iqfilter_i1, m_iqfilter_i2, m_iqfilter_q1, m_iqfilter_q2; + Sample m_loopfilter_x1; + Sample m_freq, m_phase; + Sample m_minsignal; + int m_lock_delay; + int m_lock_cnt; +}; /** Complete decoder for FM broadcast signal. */ @@ -120,9 +154,9 @@ private: const unsigned int m_downsample; const bool m_stereo_enabled; bool m_stereo_detected; - Sample m_if_level; - Sample m_baseband_mean; - Sample m_baseband_level; + double m_if_level; + double m_baseband_mean; + double m_baseband_level; IQSampleVector m_buf_iftuned; IQSampleVector m_buf_iffiltered; diff --git a/SoftFM.h b/SoftFM.h index a9a27d2..1dcc4a4 100644 --- a/SoftFM.h +++ b/SoftFM.h @@ -7,15 +7,15 @@ typedef std::complex IQSample; typedef std::vector IQSampleVector; -typedef float Sample; +typedef double Sample; typedef std::vector SampleVector; -typedef float Coeff; +typedef double Coeff; typedef std::vector CoeffVector; /** Compute mean and RMS over a sample vector. */ -inline void samples_mean_rms(const SampleVector& samples, Sample& mean, Sample& rms) +inline void samples_mean_rms(const SampleVector& samples, double& mean, double& rms) { Sample vsum = 0; Sample vsumsq = 0; diff --git a/main.cc b/main.cc index 20854fd..6dcb25e 100644 --- a/main.cc +++ b/main.cc @@ -5,14 +5,18 @@ */ #include +#include #include #include +#include +#include #include #include #include #include #include #include +#include #include #include "SoftFM.h" @@ -178,6 +182,20 @@ void write_output_data(AudioOutput *output, DataBuffer *buf, } +/** Handle Ctrl-C and SIGTERM. */ +static void handle_sigterm(int sig) +{ + stop_flag.store(true); + + string msg = "Got signal "; + msg += strsignal(sig); + msg += ", stopping ...\n"; + + const char *s = msg.c_str(); + write(STDERR_FILENO, s, strlen(s)); +} + + void usage() { fprintf(stderr, @@ -315,7 +333,19 @@ int main(int argc, char **argv) exit(1); } -// TODO : catch Ctrl-C + // Catch Ctrl-C and SIGTERM + struct sigaction sigact; + sigact.sa_handler = handle_sigterm; + sigemptyset(&sigact.sa_mask); + sigact.sa_flags = SA_RESETHAND; + if (sigaction(SIGINT, &sigact, NULL) < 0) { + fprintf(stderr, "WARNING: can not install SIGINT handler (%s)\n", + strerror(errno)); + } + if (sigaction(SIGTERM, &sigact, NULL) < 0) { + fprintf(stderr, "WARNING: can not install SIGTERM handler (%s)\n", + strerror(errno)); + } // Intentionally tune at a higher frequency to avoid DC offset. double tuner_freq = freq; @@ -422,7 +452,7 @@ int main(int argc, char **argv) double audio_level = 0; // Main loop. - for (unsigned int block = 0; ; block++) { + for (unsigned int block = 0; !stop_flag.load(); block++) { // Check for overflow of source buffer. if (!inbuf_length_warning && @@ -434,12 +464,14 @@ int main(int argc, char **argv) // Pull next block from source buffer. IQSampleVector iqsamples = source_buffer.pull(); + if (iqsamples.empty()) + break; // Decode FM signal. fm.process(iqsamples, audiosamples); // Measure audio level. - Sample audio_mean, audio_rms; + double audio_mean, audio_rms; samples_mean_rms(audiosamples, audio_mean, audio_rms); audio_level = 0.95 * audio_level + 0.05 * audio_rms; @@ -482,6 +514,8 @@ int main(int argc, char **argv) } + fprintf(stderr, "\n"); + // Join background threads. source_thread.join(); if (outputbuf_samples > 0) {