From 6e08f8a55c161d1faf527de0cdeb518a3edcb696 Mon Sep 17 00:00:00 2001 From: Joris van Rantwijk Date: Wed, 25 Dec 2013 08:59:10 +0100 Subject: [PATCH] Receiver working. Produces reasonable mono audio. --- AudioOutput.cc | 151 ++++++++++++++++ AudioOutput.h | 125 +++++++++++++ Filter.cc | 450 +++++++++++++++++++++++++++++++++++++++++++++ Filter.h | 164 +++++++++++++++++ FmDecode.cc | 159 ++++++++++++++++ FmDecode.h | 141 +++++++++++++++ Makefile | 41 +++++ RtlSdrSource.cc | 185 +++++++++++++++++++ RtlSdrSource.h | 79 ++++++++ SoftFM.h | 34 ++++ main.cc | 472 ++++++++++++++++++++++++++++++++++++++++++++++++ 11 files changed, 2001 insertions(+) create mode 100644 AudioOutput.cc create mode 100644 AudioOutput.h create mode 100644 Filter.cc create mode 100644 Filter.h create mode 100644 FmDecode.cc create mode 100644 FmDecode.h create mode 100644 Makefile create mode 100644 RtlSdrSource.cc create mode 100644 RtlSdrSource.h create mode 100644 SoftFM.h create mode 100644 main.cc diff --git a/AudioOutput.cc b/AudioOutput.cc new file mode 100644 index 0000000..28d1cd1 --- /dev/null +++ b/AudioOutput.cc @@ -0,0 +1,151 @@ + +#include +#include +#include +#include +#include + +#include "SoftFM.h" +#include "AudioOutput.h" + +using namespace std; + + +/* **************** class AudioOutput **************** */ + +// Encode a list of samples as signed 16-bit little-endian integers. +void AudioOutput::samplesToInt16(const SampleVector& samples, + vector& bytes) +{ + bytes.resize(2 * samples.size()); + + SampleVector::const_iterator i = samples.begin(); + SampleVector::const_iterator n = samples.end(); + vector::iterator k = bytes.begin(); + + while (i != n) { + Sample s = *(i++); + s = max(Sample(-1.0), min(Sample(1.0), s)); + long v = lrint(s * 32767); + unsigned long u = v; + *(k++) = u & 0xff; + *(k++) = (u >> 8) & 0xff; + } +} + + +/* **************** class RawAudioOutput **************** */ + +// Construct raw audio writer. +RawAudioOutput::RawAudioOutput(const string& filename) +{ + if (filename == "-") { + + m_fd = STDOUT_FILENO; + + } else { + + m_fd = open(filename.c_str(), O_WRONLY | O_CREAT | O_TRUNC, 0666); + if (m_fd < 0) { + m_error = "can not open '" + filename + "' (" + + strerror(errno) + ")"; + m_zombie = true; + return; + } + + } +} + + +// Destructor. +RawAudioOutput::~RawAudioOutput() +{ + // Close file descriptor. + if (m_fd >= 0 && m_fd != STDOUT_FILENO) { + close(m_fd); + } +} + + +// Write audio data. +bool RawAudioOutput::write(const SampleVector& samples) +{ + if (m_fd < 0) + return -1; + + // Convert samples to bytes. + samplesToInt16(samples, m_bytebuf); + + // Write data. + size_t p = 0; + size_t n = m_bytebuf.size(); + while (p < n) { + + ssize_t k = ::write(m_fd, m_bytebuf.data() + p, n - p); + if (k <= 0) { + if (k == 0 || errno != EINTR) { + m_error = "write failed ("; + m_error += strerror(errno); + m_error += ")"; + return false; + } + } else { + p += k; + } + } + + return true; +} + + +#if 0 + +/** Write audio data as .WAV file. */ +class WavAudioOutput +{ +public: + + /** + * Construct .WAV writer. + * + * filename :: file name (including path) or "-" to write to stdout + * samplerate :: audio sample rate in Hz + * stereo :: true if the output stream contains stereo data + */ + WavAudioOutput(const std::string& filename, + unsigned int samplerate, + bool stereo); + + ~WavAudioOutput(); + bool write(const SampleVector& samples); + std::string error(); + +private: +// TODO +}; + + +/** Write audio data to ALSA device. */ +class AlsaAudioOutput +{ +public: + + /** + * Construct ALSA output stream. + * + * dename :: ALSA PCM device + * samplerate :: audio sample rate in Hz + * stereo :: true if the output stream contains stereo data + */ + AlsaAudioOutput(const std::string& devname, + unsigned int samplerate, + bool stereo); + + ~AlsaAudioOutput(); + bool write(const SampleVector& samples); + std::string error(); + +private: + // TODO +}; +#endif diff --git a/AudioOutput.h b/AudioOutput.h new file mode 100644 index 0000000..c85dd51 --- /dev/null +++ b/AudioOutput.h @@ -0,0 +1,125 @@ +#ifndef SOFTFM_AUDIOOUTPUT_H +#define SOFTFM_AUDIOOUTPUT_H + +#include +#include +#include +#include "SoftFM.h" + + +/** Base class for writing audio data to file or playback. */ +class AudioOutput +{ +public: + + /** Destructor. */ + virtual ~AudioOutput() { } + + /** + * Write audio data. + * + * Return true on success. + * Return false if an error occurs. + */ + virtual bool write(const SampleVector& samples) = 0; + + /** Return the last error, or return an empty string if there is no error. */ + std::string error() + { + std::string ret(m_error); + m_error.clear(); + return ret; + } + + /** Return true if the stream is OK, return false if there is an error. */ + operator bool() const + { + return (!m_zombie) && m_error.empty(); + } + +protected: + /** Constructor. */ + AudioOutput() : m_zombie(false) { } + + /** Encode a list of samples as signed 16-bit little-endian integers. */ + static void samplesToInt16(const SampleVector& samples, + std::vector& bytes); + + std::string m_error; + bool m_zombie; + +private: + AudioOutput(const AudioOutput&); // no copy constructor + AudioOutput& operator=(const AudioOutput&); // no assignment operator +}; + + +/** Write audio data as raw signed 16-bit little-endian data. */ +class RawAudioOutput : public AudioOutput +{ +public: + + /** + * Construct raw audio writer. + * + * filename :: file name (including path) or "-" to write to stdout + */ + RawAudioOutput(const std::string& filename); + + ~RawAudioOutput(); + bool write(const SampleVector& samples); + +private: + int m_fd; + std::vector m_bytebuf; +}; + + +/** Write audio data as .WAV file. */ +class WavAudioOutput : public AudioOutput +{ +public: + + /** + * Construct .WAV writer. + * + * filename :: file name (including path) or "-" to write to stdout + * samplerate :: audio sample rate in Hz + * stereo :: true if the output stream contains stereo data + */ + WavAudioOutput(const std::string& filename, + unsigned int samplerate, + bool stereo); + + ~WavAudioOutput(); + bool write(const SampleVector& samples); + +private: +// TODO +}; + + +/** Write audio data to ALSA device. */ +class AlsaAudioOutput : public AudioOutput +{ +public: + + /** + * Construct ALSA output stream. + * + * dename :: ALSA PCM device + * samplerate :: audio sample rate in Hz + * stereo :: true if the output stream contains stereo data + */ + AlsaAudioOutput(const std::string& devname, + unsigned int samplerate, + bool stereo); + + ~AlsaAudioOutput(); + bool write(const SampleVector& samples); + +private: + // TODO +}; + +#endif diff --git a/Filter.cc b/Filter.cc new file mode 100644 index 0000000..d0f94c2 --- /dev/null +++ b/Filter.cc @@ -0,0 +1,450 @@ + +#include +#include +#include +#include +#include + +#include "Filter.h" + +using namespace std; + + +/** Prepare Lanczos FIR filter coefficients. */ +static void make_lanczos_coeff(unsigned int filter_order, double cutoff, + SampleVector& coeff) +{ + coeff.resize(filter_order + 1); + + // Prepare Lanczos FIR filter. + // t[i] = (i - order/2) + // coeff[i] = Sinc(2 * cutoff * t[i]) * Sinc(t[i] / (order/2 + 1)) + // coeff /= sum(coeff) + + double ysum = 0.0; + + // Calculate filter kernel. + for (int i = 0; i <= (int)filter_order; i++) { + int t2 = 2 * i - filter_order; + + double y; + if (t2 == 0) { + y = 1.0; + } else { + double x1 = cutoff * t2; + double x2 = t2 / double(filter_order + 2); + y = ( sin(M_PI * x1) / M_PI / x1 ) * + ( sin(M_PI * x2) / M_PI / x2 ); + } + + coeff[i] = y; + ysum += y; + } + + // Apply correction factor to ensure unit gain at DC. + for (unsigned i = 0; i <= filter_order; i++) { + coeff[i] /= ysum; + } +} + + +/* **************** class FineTuner **************** */ + +// Construct finetuner. +FineTuner::FineTuner(unsigned int table_size, int freq_shift) + : m_index(0) + , m_table(table_size) +{ + double phase_step = 2.0 * M_PI / double(table_size); + for (unsigned int i = 0; i < table_size; i++) { + double phi = (((int64_t)freq_shift * i) % table_size) * phase_step; + double pcos = cos(phi); + double psin = sin(phi); + m_table[i] = IQSample(pcos, psin); + } +} + + +// Process samples. +void FineTuner::process(const IQSampleVector& samples_in, + IQSampleVector& samples_out) +{ + unsigned int tblidx = m_index; + unsigned int tblsiz = m_table.size(); + unsigned int n = samples_in.size(); + + samples_out.resize(n); + + for (unsigned int i = 0; i < n; i++) { + samples_out[i] = samples_in[i] * m_table[tblidx]; + tblidx++; + if (tblidx == tblsiz) + tblidx = 0; + } + + m_index = tblidx; +} + + +/* **************** class LowPassFilterFirIQ **************** */ + +// Construct low-pass filter. +LowPassFilterFirIQ::LowPassFilterFirIQ(unsigned int filter_order, double cutoff) + : m_state(filter_order) +{ + make_lanczos_coeff(filter_order, cutoff, m_coeff); +} + + +// Process samples. +void LowPassFilterFirIQ::process(const IQSampleVector& samples_in, + IQSampleVector& samples_out) +{ + unsigned int order = m_state.size(); + unsigned int n = samples_in.size(); + + samples_out.resize(n); + + if (n == 0) + return; + + // The first few samples need data from m_state. + unsigned int i = 0; + for (; i < n && i < order; i++) { + IQSample y(0); + for (unsigned int j = 0; j <= i; j++) + y += samples_in[i-j] * m_coeff[j]; + for (unsigned int j = i + 1; j <= order; j++) + y += m_state[order + i - j] * m_coeff[j]; + samples_out[i] = y; + } + + // Remaining samples only need data from samples_in. + for (; i < n; i++) { + IQSample y(0); + for (unsigned int j = 0; j <= order; j++) + y += samples_in[i-j] * m_coeff[j]; + samples_out[i] = y; + } + + // Update m_state. + if (n < order) { + copy(m_state.begin() + n, m_state.end(), m_state.begin()); + copy(samples_in.begin(), samples_in.end(), m_state.end() - n); + } else { + copy(samples_in.end() - order, samples_in.end(), m_state.begin()); + } +} + + +/* **************** class DownsampleFilter **************** */ + +// Construct low-pass filter with optional downsampling. +DownsampleFilter::DownsampleFilter(unsigned int filter_order, double cutoff, + double downsample, bool integer_factor) + : m_downsample(downsample) + , m_downsample_int(integer_factor ? lrint(downsample) : 0) + , m_pos_int(0) + , m_pos_frac(0) + , m_state(filter_order) +{ + assert(downsample >= 1); + assert(filter_order > 1); + + // Force the first coefficient to zero and append an extra zero at the + // end of the array. This ensures we can always obtain (filter_order+1) + // coefficients by linear interpolation between adjacent array elements. + make_lanczos_coeff(filter_order - 1, cutoff, m_coeff); + m_coeff.insert(m_coeff.begin(), 0); + m_coeff.push_back(0); +} + + +// Process samples. +void DownsampleFilter::process(const SampleVector& samples_in, + SampleVector& samples_out) +{ + unsigned int order = m_state.size(); + unsigned int n = samples_in.size(); + + if (m_downsample_int != 0) { + + // Integer downsample factor, no linear interpolation. + // This is relatively simple. + + unsigned int p = m_pos_int; + unsigned int pstep = m_downsample_int; + + samples_out.resize((n - p + pstep - 1) / pstep); + + // The first few samples need data from m_state. + unsigned int i = 0; + for (; p < n && p < order; p += pstep, i++) { + Sample y = 0; + for (unsigned int j = 0; j <= p; j++) + y += samples_in[p-j] * m_coeff[j]; + for (unsigned int j = p + 1; j <= order; j++) + y += m_state[order + p - j] * m_coeff[j]; + samples_out[i] = y; + } + + // Remaining samples only need data from samples_in. + for (; p < n; p += pstep, i++) { + Sample y = 0; + for (unsigned int j = 0; j <= order; j++) + y += samples_in[p-j] * m_coeff[j]; + samples_out[i] = y; + } + + assert(i == samples_out.size()); + + // Update index of start position in text sample block. + m_pos_int = p - n; + + } else { + + // Fractional downsample factor via linear interpolation of + // the FIR coefficient table. This is a bitch. + + // Estimate number of output samples we can produce in this run. + Coeff p = m_pos_frac; + Coeff pstep = m_downsample; + unsigned int n_out = int(2 + n / pstep); + + samples_out.resize(n_out); + + // Produce output samples. + unsigned int i = 0; + Coeff pf = p; + unsigned int pi = int(pf); + while (pi < n) { + Coeff k1 = pf - pi; + Coeff k0 = 1 - k1; + + Sample y = 0; + for (unsigned int j = 0; j <= order; j++) { + Coeff k = m_coeff[j] * k0 + m_coeff[j+1] * k1; + Sample s = (j <= pi) ? samples_in[pi-j] : m_state[order+pi-j]; + y += k * s; + } + samples_out[i] = y; + + i++; + pf = p + i * pstep; + pi = int(pf); + } + + // We may overestimate the number of samples by 1 or 2. + assert(i <= n_out && i + 2 >= n_out); + samples_out.resize(i); + + // Update fractional index of start position in text sample block. + // Limit to 0 to avoid catastrophic results of rounding errors. + m_pos_frac = pf - n; + if (m_pos_frac < 0) + m_pos_frac = 0; + } + + // Update m_state. + if (n < order) { + copy(m_state.begin() + n, m_state.end(), m_state.begin()); + copy(samples_in.begin(), samples_in.end(), m_state.end() - n); + } else { + copy(samples_in.end() - order, samples_in.end(), m_state.begin()); + } + +} + + +/* **************** class LowPassFilterRC **************** */ + +// Construct 1st order low-pass IIR filter. +LowPassFilterRC::LowPassFilterRC(double timeconst) + : m_timeconst(timeconst) + , m_y1(0) +{ +} + + +// Process samples. +void LowPassFilterRC::process(const SampleVector& samples_in, + SampleVector& samples_out) +{ + /* + * Continuous domain: + * H(s) = 1 / (1 - s * timeconst) + * + * Discrete domain: + * H(z) = (1 - exp(-1/timeconst)) / (1 - exp(-1/timeconst) / z)a + */ + Coeff a1 = - exp(-1/m_timeconst);; + Coeff b0 = 1 + a1; + + unsigned int n = samples_in.size(); + samples_out.resize(n); + + Sample y = m_y1; + for (unsigned int i = 0; i < n; i++) { + Sample x = samples_in[i]; + y = b0 * x - a1 * y; + samples_out[i] = y; + } + + m_y1 = y; +} + + +// Process samples in-place. +void LowPassFilterRC::processInPlace(SampleVector& samples) +{ + Coeff a1 = - exp(-1/m_timeconst);; + Coeff b0 = 1 + a1; + + unsigned int n = samples.size(); + + Sample y = m_y1; + for (unsigned int i = 0; i < n; i++) { + Sample x = samples[i]; + y = b0 * x - a1 * y; + samples[i] = y; + } + + m_y1 = y; +} + + +/* **************** class LowPassFilterIir **************** */ + +// Construct 4th order low-pass IIR filter. +LowPassFilterIir::LowPassFilterIir(double cutoff) + : y1(0), y2(0), y3(0), y4(0) +{ + typedef complex CDbl; + + // Angular cutoff frequency. + double w = 2 * M_PI * cutoff; + + // Poles 1 and 4 are a conjugate pair, and poles 2 and 3 are another pair. + // Continuous domain: + // p_k = w * exp( (2*k + n - 1) / (2*n) * pi * j) + CDbl p1s = w * exp((2*1 + 4 - 1) / double(2 * 4) * CDbl(0, M_PI)); + CDbl p2s = w * exp((2*2 + 4 - 1) / double(2 * 4) * CDbl(0, M_PI)); + + // Map poles to discrete-domain via matched Z transform. + CDbl p1z = exp(p1s); + CDbl p2z = exp(p2s); + + // Discrete-domain transfer function: + // H(z) = b0 / ( (1 - p1/z) * (1 - p4/z) * (1 - p2/z) * (1 - p3/z) ) + // = b0 / ( (1 - (p1+p4)/z + p1*p4/z**2) * + // (1 - (p2+p3)/z + p2*p3/z**2) ) + // = b0 / (1 - (p1 + p4 + p2 + p3)/z + // + (p1*p4 + p2*p3 + (p1+p4)*(p2+p3))/z**2 + // - ((p1+p4)*p2*p3 + (p2+p3)*p1*p4)/z**3 + // + p1*p4*p2*p3/z**4 + // + // Note that p3 = conj(p2), p4 = conj(p1) + // Therefore p1+p4 == 2*real(p1), p1*p4 == abs(p1*p1) + // + a1 = - (2*real(p1z) + 2*real(p2z)); + a2 = (abs(p1z*p1z) + abs(p2z*p2z) + 2*real(p1z) * 2*real(p2z)); + a3 = - (2*real(p1z) * abs(p2z*p2z) + 2*real(p2z) * abs(p1z*p1z)); + a4 = abs(p1z*p1z) * abs(p2z*p2z); + + // Choose b0 to get unit DC gain. + b0 = 1 + a1 + a2 + a3 + a4; +} + + +// Process samples. +void LowPassFilterIir::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++) { + Sample x = samples_in[i]; + Sample y = b0 * x - a1 * y1 - a2 * y2 - a3 * y3 - a4 * y4; + y4 = y3; y3 = y2; y2 = y1; y1 = y; + samples_out[i] = y; + } +} + + +/* **************** class HighPassFilterIir **************** */ + +// Construct 2nd order high-pass IIR filter. +HighPassFilterIir::HighPassFilterIir(double cutoff) + : x1(0), x2(0), y1(0), y2(0) +{ + typedef complex CDbl; + + // Angular cutoff frequency. + double w = 2 * M_PI * cutoff; + + // Poles 1 and 2 are a conjugate pair. + // Continuous-domain: + // p_k = w / exp( (2*k + n - 1) / (2*n) * pi * j) + CDbl p1s = w / exp((2*1 + 2 - 1) / double(2 * 2) * CDbl(0, M_PI)); + + // Map poles to discrete-domain via matched Z transform. + CDbl p1z = exp(p1s); + + // Both zeros are located in s = 0, z = 1. + + // Discrete-domain transfer function: + // H(z) = g * (1 - 1/z) * (1 - 1/z) / ( (1 - p1/z) * (1 - p2/z) ) + // = g * (1 - 2/z + 1/z**2) / (1 - (p1+p2)/z + (p1*p2)/z**2) + // + // Note that z2 = conj(z1). + // Therefore p1+p2 == 2*real(p1), p1*2 == abs(p1*p1), z4 = conj(z1) + // + b0 = 1; + b1 = -2; + b2 = 1; + a1 = -2 * real(p1z); + a2 = abs(p1z*p1z); + + // Adjust b coefficients to get unit gain at Nyquist frequency (z=-1). + double g = (b0 - b1 + b2) / (1 - a1 + a2); + b0 /= g; + b1 /= g; + b2 /= g; +} + + +// Process samples. +void HighPassFilterIir::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++) { + Sample x = samples_in[i]; + Sample y = b0 * x + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2; + x2 = x1; x1 = x; + y2 = y1; y1 = y; + samples_out[i] = y; + } +} + + +// Process samples in-place. +void HighPassFilterIir::processInPlace(SampleVector& samples) +{ + unsigned int n = samples.size(); + + for (unsigned int i = 0; i < n; i++) { + Sample x = samples[i]; + Sample y = b0 * x + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2; + x2 = x1; x1 = x; + y2 = y1; y1 = y; + samples[i] = y; + } +} + +/* end */ diff --git a/Filter.h b/Filter.h new file mode 100644 index 0000000..75f6fe4 --- /dev/null +++ b/Filter.h @@ -0,0 +1,164 @@ +#ifndef SOFTFM_FILTER_H +#define SOFTFM_FILTER_H + +#include +#include "SoftFM.h" + + +/** Fine tuner which shifts the frequency of an IQ signal by a fixed offset. */ +class FineTuner +{ +public: + + /** + * Construct fine tuner. + * + * table_size :: Size of internal sin/cos tables, determines the resolution + * of the frequency shift. + * + * freq_shift :: Frequency shift. Signal frequency will be shifted by + * (sample_rate * freq_shift / table_size). + */ + FineTuner(unsigned int table_size, int freq_shift); + + /** Process samples. */ + void process(const IQSampleVector& samples_in, IQSampleVector& samples_out); + +private: + unsigned int m_index; + IQSampleVector m_table; +}; + + +/** Low-pass filter for IQ samples, based on Lanczos FIR filter. */ +class LowPassFilterFirIQ +{ +public: + + /** + * Construct low-pass filter. + * + * filter_order :: FIR filter order. + * cutoff :: Cutoff frequency relative to the full sample rate + * (valid range 0.0 ... 0.5). + */ + LowPassFilterFirIQ(unsigned int filter_order, double cutoff); + + /** Process samples. */ + void process(const IQSampleVector& samples_in, IQSampleVector& samples_out); + +private: + CoeffVector m_coeff; + IQSampleVector m_state; +}; + + +/** + * Downsampler with low-pass FIR filter for real-valued signals. + * + * Step 1: Low-pass filter based on Lanczos FIR filter + * Step 2: (optional) Decimation by an arbitrary factor (integer or float) + */ +class DownsampleFilter +{ +public: + + /** + * Construct low-pass filter with optional downsampling. + * + * filter_order :: FIR filter order + * cutoff :: Cutoff frequency relative to the full input sample rate + * (valid range 0.0 .. 0.5) + * downsample :: Decimation factor (>= 1) or 1 to disable + * integer_factor :: Enables a faster and more precise algorithm that + * only works for integer downsample factors. + * + * The output sample rate is (input_sample_rate / downsample) + */ + DownsampleFilter(unsigned int filter_order, double cutoff, + double downsample=1, bool integer_factor=true); + + /** Process samples. */ + void process(const SampleVector& samples_in, SampleVector& samples_out); + +private: + double m_downsample; + unsigned int m_downsample_int; + unsigned int m_pos_int; + Coeff m_pos_frac; + CoeffVector m_coeff; + SampleVector m_state; +}; + + +/** First order low-pass IIR filter for real-valued signals. */ +class LowPassFilterRC +{ +public: + + /** + * Construct 1st order low-pass IIR filter. + * + * timeconst :: RC time constant in seconds (1 / (2 * PI * cutoff_freq) + */ + LowPassFilterRC(double timeconst); + + /** Process samples. */ + void process(const SampleVector& samples_in, SampleVector& samples_out); + + /** Process samples in-place. */ + void processInPlace(SampleVector& samples); + +private: + double m_timeconst; + Sample m_y1; +}; + + +/** Low-pass filter for real-valued signals based on Butterworth IIR filter. */ +class LowPassFilterIir +{ +public: + + /** + * Construct 4th order low-pass IIR filter. + * + * cutoff :: Low-pass cutoff relative to the sample frequency + * (valid range 0.0 .. 0.5, 0.5 = Nyquist) + */ + LowPassFilterIir(double cutoff); + + /** Process samples. */ + void process(const SampleVector& samples_in, SampleVector& samples_out); + +private: + Coeff b0, a1, a2, a3, a4; + Sample y1, y2, y3, y4; +}; + + +/** High-pass filter for real-valued signals based on Butterworth IIR filter. */ +class HighPassFilterIir +{ +public: + + /** + * Construct 2nd order high-pass IIR filter. + * + * cutoff :: High-pass cutoff relative to the sample frequency + * (valid range 0.0 .. 0.5, 0.5 = Nyquist) + */ + HighPassFilterIir(double cutoff); + + /** Process samples. */ + void process(const SampleVector& samples_in, SampleVector& samples_out); + + /** Process samples in-place. */ + void processInPlace(SampleVector& samples); + +private: + Coeff b0, b1, b2, a1, a2; + Sample x1, x2, y1, y2; +}; + +#endif diff --git a/FmDecode.cc b/FmDecode.cc new file mode 100644 index 0000000..fa17e28 --- /dev/null +++ b/FmDecode.cc @@ -0,0 +1,159 @@ + +#include + +#include "FmDecode.h" + +using namespace std; + + +/** Fast approximation of atan function. */ +static inline Sample fast_atan(Sample x) +{ + // http://stackoverflow.com/questions/7378187/approximating-inverse-trigonometric-funcions + + Sample y = 1; + Sample p = 0; + + if (x < 0) { + x = -x; + y = -1; + } + + if (x > 1) { + p = y; + y = -y; + x = 1 / x; + } + + const Sample b = 0.596227; + y *= (b*x + x*x) / (1 + 2*b*x + x*x); + + return (y + p) * Sample(M_PI_2); +} + + +/** Compute RMS level over a small prefix of the specified sample vector. */ +static Sample rms_level_approx(const IQSampleVector& samples) +{ + unsigned int n = samples.size(); + n = (n + 63) / 64; + + Sample level = 0; + for (unsigned int i = 0; i < n; i++) { + const IQSample& s = samples[i]; + IQSample::value_type re = s.real(), im = s.imag(); + level += re * re + im * im; + } + + return sqrt(level / n); +} + + +/* **************** class PhaseDiscriminator **************** */ + +// Construct phase discriminator. +PhaseDiscriminator::PhaseDiscriminator(double max_freq_dev) + : m_freq_scale_factor(1.0 / (max_freq_dev * 2.0 * M_PI)) +{ } + + +// Process samples. +void PhaseDiscriminator::process(const IQSampleVector& samples_in, + SampleVector& samples_out) +{ + unsigned int n = samples_in.size(); + IQSample s0 = m_last_sample; + + samples_out.resize(n); + + for (unsigned int i = 0; i < n; i++) { + IQSample s1(samples_in[i]); + IQSample d(conj(s0) * s1); +// TODO : implement fast approximation of atan2 + Sample w = atan2(d.imag(), d.real()); + samples_out[i] = w * m_freq_scale_factor; + s0 = s1; + } + + m_last_sample = s0; +} + + +/* **************** class FmDecoder **************** */ + +FmDecoder::FmDecoder(double sample_rate_if, + double tuning_offset, + double sample_rate_pcm, + bool stereo, + double deemphasis, + double bandwidth_if, + double freq_dev, + double bandwidth_pcm, + unsigned int downsample) + : m_sample_rate_if(sample_rate_if) + , m_tuning_table_size(64) + , m_tuning_shift(lrint(-64.0 * tuning_offset / sample_rate_if)) + , m_freq_dev(freq_dev) + , m_downsample(downsample) + , m_stereo_enabled(stereo) + , m_stereo_detected(false) + , m_if_level(0) + , m_baseband_mean(0) + , m_baseband_level(0) + , m_finetuner(m_tuning_table_size, m_tuning_shift) + , m_iffilter(10, bandwidth_if / sample_rate_if) + , m_phasedisc(freq_dev / sample_rate_if) + , m_resample_baseband(6 * downsample, + 0.5 / downsample, + downsample, true) + , m_resample_mono(int(15 * sample_rate_if / downsample / bandwidth_pcm), + bandwidth_pcm * downsample / sample_rate_if, + sample_rate_if / downsample / sample_rate_pcm, false) + , m_dcblock_mono(30.0 / sample_rate_pcm) + , m_deemph_mono((deemphasis == 0) ? 1.0 : (deemphasis * sample_rate_pcm * 1.0e-6)) +{ +} + + +void FmDecoder::process(const IQSampleVector& samples_in, + SampleVector& audio) +{ + // Fine tuning. + m_finetuner.process(samples_in, m_buf_iftuned); + + // Low pass filter to isolate station. + m_iffilter.process(m_buf_iftuned, m_buf_iffiltered); + + // Measure IF level. + Sample if_rms = rms_level_approx(m_buf_iffiltered); + m_if_level = 0.95 * m_if_level + 0.05 * if_rms; + + // Extract carrier frequency. + m_phasedisc.process(m_buf_iffiltered, m_buf_baseband); + + // Downsample baseband signal to reduce processing. + if (m_downsample > 1) { + SampleVector tmp(move(m_buf_baseband)); + m_resample_baseband.process(tmp, m_buf_baseband); + } + + // Measure baseband level. + Sample 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; + +// TODO : stereo decoding + + // Extract mono audio signal. + m_resample_mono.process(m_buf_baseband, m_buf_mono); + + // DC blocking and de-emphasis. + m_dcblock_mono.processInPlace(m_buf_mono); + m_deemph_mono.processInPlace(m_buf_mono); + +// TODO : stereo mixing + audio = move(m_buf_mono); +} + +/* end */ diff --git a/FmDecode.h b/FmDecode.h new file mode 100644 index 0000000..2363192 --- /dev/null +++ b/FmDecode.h @@ -0,0 +1,141 @@ +#ifndef SOFTFM_FMDECODE_H +#define SOFTFM_FMDECODE_H + +#include "SoftFM.h" +#include "Filter.h" + + +/* Detect frequency by phase discrimination between successive samples. */ +class PhaseDiscriminator +{ +public: + + /** + * Construct phase discriminator. + * + * max_freq_dev :: Full scale frequency deviation relative to the + * full sample frequency. + */ + PhaseDiscriminator(double max_freq_dev); + + /** + * Process samples. + * Output is a sequence of frequency estimates, scaled such that + * output value +/- 1.0 represents the maximum frequency deviation. + */ + void process(const IQSampleVector& samples_in, SampleVector& samples_out); + +private: + const Sample m_freq_scale_factor; + IQSample m_last_sample; +}; + + +// TODO : maybe CIC downsampling from 1 MS/s to ~ 250 kS/s + + +/** Complete decoder for FM broadcast signal. */ +class FmDecoder +{ +public: + static constexpr double default_deemphasis = 50; + static constexpr double default_bandwidth_if = 115000; + static constexpr double default_freq_dev = 75000; + static constexpr double default_bandwidth_pcm = 15000; + + /** + * Construct FM decoder. + * + * sample_rate_if :: IQ sample rate in Hz. + * tuning_offset :: Frequency offset in Hz of radio station with respect + * to receiver LO frequency (positive value means + * station is at higher frequency than LO). + * sample_rate_pcm :: Audio sample rate. + * stereo :: True to enable stereo decoding. + * deemphasis :: Time constant of de-emphasis filter in microseconds + * (50 us for broadcast FM, 0 to disable de-emphasis). + * bandwidth_if :: Half bandwidth of IF signal in Hz + * (~ 100 kHz for broadcast FM) + * freq_dev :: Full scale carrier frequency deviation + * (75 kHz for broadcast FM) + * bandwidth_pcm :: Half bandwidth of audio signal in Hz + * (15 kHz for broadcast FM) + * downsample :: Downsampling factor to apply after FM demodulation. + * Set to 1 to disable. + */ + FmDecoder(double sample_rate_if, + double tuning_offset, + double sample_rate_pcm, + bool stereo=true, + double deemphasis=50, + double bandwidth_if=default_bandwidth_if, + double freq_dev=default_freq_dev, + double bandwidth_pcm=default_bandwidth_pcm, + unsigned int downsample=1); + + /** + * Process IQ samples and return audio samples. + * + * If the decoder is set in stereo mode, samples for left and right + * channels are interleaved in the output vector (even if no stereo + * signal is detected). If the decoder is set in mono mode, the output + * vector only contains samples for one channel. + */ + void process(const IQSampleVector& samples_in, + SampleVector& audio); + + /** Return true if a stereo signal is detected. */ + bool stereo_detected() const + { + return m_stereo_detected; + } + + /** Return actual frequency offset in Hz with respect to receiver LO. */ + double get_tuning_offset() const + { + double tuned = - m_tuning_shift * m_sample_rate_if / + double(m_tuning_table_size); + return tuned + m_baseband_mean * m_freq_dev; + } + + /** Return RMS IF level (where full scale IQ signal is 1.0). */ + double get_if_level() const + { + return m_if_level; + } + + /** Return RMS baseband signal level (where nominal level is 0.707). */ + double get_baseband_level() const + { + return m_baseband_level; + } + +// TODO : stuff for stereo pilot locking + +private: + const double m_sample_rate_if; + const int m_tuning_table_size; + const int m_tuning_shift; + const double m_freq_dev; + 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; + + IQSampleVector m_buf_iftuned; + IQSampleVector m_buf_iffiltered; + SampleVector m_buf_baseband; + SampleVector m_buf_mono; + + FineTuner m_finetuner; + LowPassFilterFirIQ m_iffilter; + PhaseDiscriminator m_phasedisc; + DownsampleFilter m_resample_baseband; + DownsampleFilter m_resample_mono; + HighPassFilterIir m_dcblock_mono; + LowPassFilterRC m_deemph_mono; +}; + +#endif diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..7eafffb --- /dev/null +++ b/Makefile @@ -0,0 +1,41 @@ +# Makefile for SoftFM + +# ----- Tweak these settings to configure for your system + +# TODO : -D_FILE_OFFSET_BITS=64 + +CROSS = +CFLAGS_OPT = -O2 -ffast-math -ftree-vectorize +CFLAGS_DEBUG = -g +CFLAGS_ARCH = +CFLAGS_PATH = -I/home/joris/test/rtl-sdr/inst/include +CFLAGS_EXTRA = +LDFLAGS_PATH = -L/home/joris/test/rtl-sdr/inst/lib +LDFLAGS_EXTRA = +LIBS_RTLSDR = /home/joris/test/rtl-sdr/inst/lib/librtlsdr.a -lusb-1.0 +LIBS_EXTRA = + +# ----- end tweakable settings + + +CXX = $(CROSS)g++ +CXXFLAGS = -std=c++11 -Wall $(CFLAGS_OPT) $(CFLAGS_DEBUG) \ + $(CFLAGS_ARCH) $(CFLAGS_PATH) $(CFLAGS_EXTRA) +LDFLAGS = $(LDFLAGS_PATH) $(LDFLAGS_EXTRA) +LDLIBS = $(LIBS_RTLSDR) $(LIBS_EXTRA) + +OBJS = RtlSdrSource.o Filter.o FmDecode.o AudioOutput.o main.o + +softfm : $(OBJS) + $(CXX) $(LDFLAGS) -o $@ $(OBJS) $(LDLIBS) + +RtlSdrSource.o : RtlSdrSource.cc RtlSdrSource.h SoftFM.h +Filter.o : Filter.cc Filter.h SoftFM.h +FmDecode.o : FmDecode.cc FmDecode.h SoftFM.h Filter.h +AudioOutput.o : AudioOutput.cc AudioOutput.h SoftFM.h +main.o : main.cc SoftFM.h RtlSdrSource.h Filter.h FmDecode.h AudioOutput.h + +.PHONY: clean +clean: + $(RM) softfm *.o + diff --git a/RtlSdrSource.cc b/RtlSdrSource.cc new file mode 100644 index 0000000..bbbf7ff --- /dev/null +++ b/RtlSdrSource.cc @@ -0,0 +1,185 @@ + +#include +#include + +#include "RtlSdrSource.h" + +using namespace std; + + +// Open RTL-SDR device. +RtlSdrSource::RtlSdrSource(int dev_index) + : m_dev(0) + , m_block_length(default_block_length) +{ + int r; + + r = rtlsdr_open(&m_dev, dev_index); + if (r < 0) { + m_error = "Failed to open RTL-SDR device ("; + m_error += strerror(-r); + m_error += ")"; + } +} + + +// Close RTL-SDR device. +RtlSdrSource::~RtlSdrSource() +{ + if (m_dev) + rtlsdr_close(m_dev); +} + + +// Configure RTL-SDR tuner and prepare for streaming. +bool RtlSdrSource::configure(uint32_t sample_rate, + uint32_t frequency, + int tuner_gain, + int block_length) +{ + int r; + + if (!m_dev) + return false; + + r = rtlsdr_set_sample_rate(m_dev, sample_rate); + if (r < 0) { + m_error = "rtlsdr_set_sample_rate failed"; + return false; + } + + r = rtlsdr_set_center_freq(m_dev, frequency); + if (r < 0) { + m_error = "rtlsdr_set_center_freq failed"; + return false; + } + + if (tuner_gain < 0) { + r = rtlsdr_set_tuner_gain_mode(m_dev, 0); + if (r < 0) { + m_error = "rtlsdr_set_tuner_gain_mode could not set automatic gain"; + return false; + } + } else { + r = rtlsdr_set_tuner_gain_mode(m_dev, 1); + if (r < 0) { + m_error = "rtlsdr_set_tuner_gain_mode could not set manual gain"; + return false; + } + + r = rtlsdr_set_tuner_gain(m_dev, tuner_gain); + if (r < 0) { + m_error = "rtlsdr_set_tuner_gain failed"; + return false; + } + } + + // set block length + m_block_length = (block_length < 4096) ? 4096 : + (block_length > 1024 * 1024) ? 1024 * 1024 : + block_length; + m_block_length -= m_block_length % 4096; + + // reset buffer to start streaming + if (rtlsdr_reset_buffer(m_dev) < 0) { + m_error = "rtlsdr_reset_buffer failed"; + return false; + } + + return true; +} + + +// Return current sample frequency in Hz. +uint32_t RtlSdrSource::get_sample_rate() +{ + return rtlsdr_get_sample_rate(m_dev); +} + + +// Return current center frequency in Hz. +uint32_t RtlSdrSource::get_frequency() +{ + return rtlsdr_get_center_freq(m_dev); +} + + +// Return current tuner gain in dB. +double RtlSdrSource::get_tuner_gain() +{ + return 0.1 * rtlsdr_get_tuner_gain(m_dev); +} + + +// Return a list of supported tuner gain settings in dB. +vector RtlSdrSource::get_tuner_gains() +{ + vector result; + + int num_gains = rtlsdr_get_tuner_gains(m_dev, NULL); + if (num_gains <= 0) + return result; + + int gains[num_gains]; + if (rtlsdr_get_tuner_gains(m_dev, gains) != num_gains) + return result; + + result.reserve(num_gains); + for (int i = 0; i < num_gains; i++) + result.push_back(0.1 * gains[i]); + + return result; +} + + +// Fetch a bunch of samples from the device. +bool RtlSdrSource::get_samples(IQSampleVector& samples) +{ + int r, n_read; + + if (!m_dev) + return false; + + vector buf(2 * m_block_length); + + r = rtlsdr_read_sync(m_dev, buf.data(), 2 * m_block_length, &n_read); + if (r < 0) { + m_error = "rtlsdr_read_sync failed"; + return false; + } + + if (n_read != 2 * m_block_length) { + m_error = "short read, samples lost"; + return false; + } + + samples.resize(m_block_length); + for (int i = 0; i < m_block_length; i++) { + int32_t re = buf[2*i]; + int32_t im = buf[2*i+1]; + samples[i] = IQSample( (re - 128) / IQSample::value_type(128), + (im - 128) / IQSample::value_type(128) ); + } + + return true; +} + + +// Return a list of supported devices. +vector RtlSdrSource::get_device_names() +{ + vector result; + + int device_count = rtlsdr_get_device_count(); + if (device_count <= 0) + return result; + + result.reserve(device_count); + for (int i = 0; i < device_count; i++) { + result.push_back(string(rtlsdr_get_device_name(i))); + } + + return result; +} + +/* end */ diff --git a/RtlSdrSource.h b/RtlSdrSource.h new file mode 100644 index 0000000..08c31f5 --- /dev/null +++ b/RtlSdrSource.h @@ -0,0 +1,79 @@ +#ifndef SOFTFM_RTLSDRSOURCE_H +#define SOFTFM_RTLSDRSOURCE_H + +#include +#include +#include + +#include "SoftFM.h" + + +class RtlSdrSource +{ +public: + + static const int default_block_length = 65536; + + /** Open RTL-SDR device. */ + RtlSdrSource(int dev_index); + + /** Close RTL-SDR device. */ + ~RtlSdrSource(); + + /** + * Configure RTL-SDR tuner and prepare for streaming. + * + * sample_rate :: desired sample rate in Hz. + * frequency :: desired center frequency in Hz. + * gain :: desired tuner gain index, or -1 for auto-gain. + * block_length :: preferred number of samples per block. + * + * Return true for success, false if an error occurred. + */ + bool configure(uint32_t sample_rate, uint32_t frequency, int tuner_gain, + int block_length=default_block_length); + + /** Return current sample frequency in Hz. */ + uint32_t get_sample_rate(); + + /** Return current center frequency in Hz. */ + uint32_t get_frequency(); + + /** Return current tuner gain in dB. */ + double get_tuner_gain(); + + /** Return a list of supported tuner gain settings in dB. */ + std::vector get_tuner_gains(); + + /** + * Fetch a bunch of samples from the device. + * + * This function must be called regularly to maintain streaming. + * Return true for success, false if an error occurred. + */ + bool get_samples(IQSampleVector& samples); + + /** Return the last error, or return an empty string if there is no error. */ + std::string error() + { + std::string ret(m_error); + m_error.clear(); + return ret; + } + + /** Return true if the device is OK, return false if there is an error. */ + operator bool() const + { + return m_dev && m_error.empty(); + } + + /** Return a list of supported devices. */ + static std::vector get_device_names(); + +private: + struct rtlsdr_dev * m_dev; + int m_block_length; + std::string m_error; +}; + +#endif diff --git a/SoftFM.h b/SoftFM.h new file mode 100644 index 0000000..a9a27d2 --- /dev/null +++ b/SoftFM.h @@ -0,0 +1,34 @@ +#ifndef SOFTFM_H +#define SOFTFM_H + +#include +#include + +typedef std::complex IQSample; +typedef std::vector IQSampleVector; + +typedef float Sample; +typedef std::vector SampleVector; + +typedef float 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) +{ + Sample vsum = 0; + Sample vsumsq = 0; + + unsigned int n = samples.size(); + for (unsigned int i = 0; i < n; i++) { + Sample v = samples[i]; + vsum += v; + vsumsq += v * v; + } + + mean = vsum / n; + rms = sqrt(vsumsq / n); +} + +#endif diff --git a/main.cc b/main.cc new file mode 100644 index 0000000..666c855 --- /dev/null +++ b/main.cc @@ -0,0 +1,472 @@ +/* + * SoftFM - Software decoder for FM broadcast radio with RTL-SDR + * + * Joris van Rantwijk, 2013. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "SoftFM.h" +#include "RtlSdrSource.h" +#include "FmDecode.h" +#include "AudioOutput.h" + +using namespace std; + + +/** Flag is set on SIGINT / SIGTERM. */ +static atomic_bool stop_flag(false); + + +/** Buffer to move sample data between threads. */ +template +class DataBuffer +{ +public: + /** Constructor. */ + DataBuffer() + : m_qlen(0) + , m_end_marked(false) + { } + + /** Add samples to the queue. */ + void push(vector&& samples) + { + if (!samples.empty()) { + unique_lock lock(m_mutex); + m_qlen += samples.size(); + m_queue.push(move(samples)); + lock.unlock(); + m_cond.notify_all(); + } + } + + /** Mark the end of the data stream. */ + void push_end() + { + unique_lock lock(m_mutex); + m_end_marked = true; + lock.unlock(); + m_cond.notify_all(); + } + + /** Return number of samples in queue. */ + size_t queued_samples() + { + unique_lock lock(m_mutex); + return m_qlen; + } + + /** + * If the queue is non-empty, remove a block from the queue and + * return the samples. If the end marker has been reached, return + * an empty vector. If the queue is empty, wait until more data is pushed + * or until the end marker is pushed. + */ + vector pull() + { + vector ret; + unique_lock lock(m_mutex); + while (m_queue.empty() && !m_end_marked) + m_cond.wait(lock); + if (!m_queue.empty()) { + m_qlen -= m_queue.front().size(); + swap(ret, m_queue.front()); + m_queue.pop(); + } + return ret; + } + + /** Return true if the end has been reached at the Pull side. */ + bool pull_end_reached() + { + unique_lock lock(m_mutex); + return m_qlen == 0 && m_end_marked; + } + + /** Wait until the buffer contains minfill samples or an end marker. */ + void wait_buffer_fill(size_t minfill) + { + unique_lock lock(m_mutex); + while (m_qlen < minfill && !m_end_marked) + m_cond.wait(lock); + } + +private: + size_t m_qlen; + bool m_end_marked; + queue> m_queue; + mutex m_mutex; + condition_variable m_cond; +}; + + +/** Simple linear gain adjustment. */ +void adjust_gain(SampleVector& samples, double gain) +{ + for (unsigned int i = 0, n = samples.size(); i < n; i++) { + samples[i] *= gain; + } +} + + +/** + * Read data from source device and put it in a buffer. + * + * This code runs in a separate thread. + * The RTL-SDR library is not capable of buffering large amounts of data. + * Running this in a background thread ensures that the time between calls + * to RtlSdrSource::get_samples() is very short. + */ +void read_source_data(RtlSdrSource *rtlsdr, DataBuffer *buf) +{ + IQSampleVector iqsamples; + + while (!stop_flag.load()) { + + if (!rtlsdr->get_samples(iqsamples)) { + fprintf(stderr, "ERROR: RtlSdr: %s\n", rtlsdr->error().c_str()); + exit(1); + } + + buf->push(move(iqsamples)); + } + + buf->push_end(); +} + + +/** + * Get data from output buffer and write to output stream. + * + * This code runs in a separate thread. + */ +void write_output_data(AudioOutput *output, DataBuffer *buf, + unsigned int buf_minfill) +{ + while (!stop_flag.load()) { + + if (buf->queued_samples() == 0) { + // The buffer is empty. Perhaps the output stream is consuming + // samples faster than we can produce them. Wait until the buffer + // is back at its nominal level to make sure this does not happen + // too often. + buf->wait_buffer_fill(buf_minfill); + } + + if (buf->pull_end_reached()) { + // Reached end of stream. + break; + } + + // Get samples from buffer and write to output. + SampleVector samples = buf->pull(); + output->write(samples); + } +} + + +void usage() +{ + fprintf(stderr, + "Usage: softfm -f freq [options]\n" + " -f freq Frequency of radio station in Hz\n" + " -d devidx RTL-SDR device index (default 0)\n" + " -s ifrate IF sample rate in Hz (default 1000000)\n" + " -r pcmrate Audio sample rate in Hz (default 48000 Hz)\n" + " -M Disable stereo decoding\n" + " -R filename Write audio data as raw S16_LE samples\n" + " use filename '-' to write to stdout\n" + " -W filename Write audio data to .WAV file\n" + " -P [device] Play audio via ALSA device (default 'default')\n" + " -b seconds Set audio buffer size in seconds\n" + "\n"); +} + + +void badarg(const char *label) +{ + usage(); + fprintf(stderr, "ERROR: Invalid argument for %s\n", label); + exit(1); +} + + +bool parse_opt(const char *s, int& v) +{ + char *endp; + long t = strtol(s, &endp, 10); + if (endp == s || *endp != '\0' || t < INT_MIN || t > INT_MAX) + return false; + v = t; + return true; +} + + +bool parse_opt(const char *s, double& v) +{ + char *endp; + v = strtod(s, &endp); + return (endp != s && *endp == '\0'); +} + + +int main(int argc, char **argv) +{ + int c; + double freq = -1; + int devidx = 0; + double ifrate = 1.0e6; + int pcmrate = 48000; + int stereo = 1; + enum OutputMode { MODE_RAW, MODE_WAV, MODE_ALSA }; + OutputMode outmode = MODE_ALSA; + string filename; + string devname("default"); + double bufsecs = -1; + + fprintf(stderr, + "SoftFM - Software decoder for FM broadcast radio with RTL-SDR\n"); + + while ((c = getopt(argc, argv, "f:d:s:r:MR:W:P::b:")) >= 0) { + switch (c) { + case 'f': + if (!parse_opt(optarg, freq) || freq <= 0) { + badarg("-f"); + } + break; + case 'd': + if (!parse_opt(optarg, devidx) || devidx < 0) { + badarg("-d"); + } + break; + case 's': + if (!parse_opt(optarg, ifrate) || ifrate <= 0) { + badarg("-s"); + } + break; + case 'r': + if (!parse_opt(optarg, pcmrate) || pcmrate < 1) { + badarg("-r"); + } + break; + case 'M': + stereo = 0; + break; + case 'R': + outmode = MODE_RAW; + filename = optarg; + break; + case 'W': + outmode = MODE_WAV; + filename = optarg; + break; + case 'P': + outmode = MODE_ALSA; + if (optarg != NULL) + devname = optarg; + break; + case 'b': + if (!parse_opt(optarg, bufsecs) || bufsecs < 0) { + badarg("-b"); + } + default: + usage(); + fprintf(stderr, "ERROR: Unknown option\n"); + exit(1); + } + } + + if (freq <= 0) { + usage(); + fprintf(stderr, "ERROR: Specify a tuning frequency\n"); + exit(1); + } + + if (3 * FmDecoder::default_bandwidth_if > ifrate) { + fprintf(stderr, "ERROR: IF sample rate must be at least %.0f Hz\n", + 3 * FmDecoder::default_bandwidth_if); + exit(1); + } + +// TODO : catch Ctrl-C + + // Intentionally tune at a higher frequency to avoid DC offset. + double tuner_freq = freq; + if (ifrate >= 5 * FmDecoder::default_bandwidth_if) { + tuner_freq += 0.25 * ifrate; + } + + // Open RTL-SDR device. + RtlSdrSource rtlsdr(devidx); + if (!rtlsdr) { + fprintf(stderr, "ERROR: RtlSdr: %s\n", rtlsdr.error().c_str()); + exit(1); + } + + // Configure RTL-SDR device and start streaming. + rtlsdr.configure(ifrate, tuner_freq, -1); + if (!rtlsdr) { + fprintf(stderr, "ERROR: RtlSdr: %s\n", rtlsdr.error().c_str()); + exit(1); + } + + tuner_freq = rtlsdr.get_frequency(); + fprintf(stderr, "device tuned for %.6f MHz\n", tuner_freq * 1.0e-6); + + ifrate = rtlsdr.get_sample_rate(); + fprintf(stderr, "IF sample rate %.0f Hz\n", ifrate); + + // Create source data queue. + DataBuffer source_buffer; + + // Start reading from device in separate thread. + thread source_thread(read_source_data, &rtlsdr, &source_buffer); + + // The baseband signal is empty above 100 kHz, so we can + // downsample to ~ 200 kS/s without loss of information. + // This will speed up later processing stages. + unsigned int downsample = max(1, int(ifrate / 215.0e3)); + fprintf(stderr, "baseband downsampling factor %u\n", downsample); + + // Prevent aliasing at very low output sample rates. + double bandwidth_pcm = min(FmDecoder::default_bandwidth_pcm, + 0.45 * pcmrate); + fprintf(stderr, "audio sample rate %u Hz\n", pcmrate); + fprintf(stderr, "audio bandwidth %.3f kHz\n", bandwidth_pcm * 1.0e-3); + + // Prepare decoder. + FmDecoder fm(ifrate, // sample_rate_if + freq - tuner_freq, // tuning_offset + pcmrate, // sample_rate_pcm + stereo, // stereo + FmDecoder::default_deemphasis, // deemphasis, + FmDecoder::default_bandwidth_if, // bandwidth_if + FmDecoder::default_freq_dev, // freq_dev + bandwidth_pcm, // bandwidth_pcm + downsample); // downsample + + // Calculate number of samples in audio buffer. + unsigned int outputbuf_samples = 0; + if (bufsecs < 0 && + (outmode == MODE_ALSA || (outmode == MODE_RAW && filename == "-"))) { + // Set default buffer to 1 second for interactive output streams. + outputbuf_samples = pcmrate; + } else if (bufsecs > 0) { + // Calculate nr of samples for configured buffer length. + outputbuf_samples = (unsigned int)(bufsecs * pcmrate); + } + if (outputbuf_samples > 0) { + fprintf(stderr, "output buffer %.1f seconds\n", + outputbuf_samples / double(pcmrate)); + } + + // Prepare output writer. + unique_ptr audio_output; + switch (outmode) { + case MODE_RAW: + audio_output.reset(new RawAudioOutput(filename)); + break; + case MODE_WAV: + case MODE_ALSA: + // TODO + abort(); + } + + // If buffering enabled, start background output thread. + DataBuffer output_buffer; + thread output_thread; + if (outputbuf_samples > 0) { + unsigned int nchannel = stereo ? 2 : 1; + output_thread = thread(write_output_data, + audio_output.get(), + &output_buffer, + outputbuf_samples * nchannel); + } + + SampleVector audiosamples; + bool inbuf_length_warning = false; + double audio_level = 0; + + // Main loop. + for (unsigned int block = 0; ; block++) { + + // Check for overflow of source buffer. + if (!inbuf_length_warning && + source_buffer.queued_samples() > 10 * ifrate) { + fprintf(stderr, + "\nWARNING: Input buffer is growing (system too slow)\n"); + inbuf_length_warning = true; + } + + // Pull next block from source buffer. + IQSampleVector iqsamples = source_buffer.pull(); + + // Decode FM signal. + fm.process(iqsamples, audiosamples); + + // Measure audio level. + Sample audio_mean, audio_rms; + samples_mean_rms(audiosamples, audio_mean, audio_rms); + audio_level = 0.95 * audio_level + 0.05 * audio_rms; + + adjust_gain(audiosamples, 0.5); + +// TODO : investigate I/Q imbalance to fix Radio4 noise +// TODO : investigate if PLL receiver is better than phase discriminator at weak reception + + +// TODO : show mono/stereo + fprintf(stderr, + "\rblk=%6d freq=%8.4fMHz IF=%+5.1fdB BB=%+5.1fdB audio=%+5.1fdB ", + block, + (tuner_freq + fm.get_tuning_offset()) * 1.0e-6, + 20*log10(fm.get_if_level()), + 20*log10(fm.get_baseband_level()) + 3.01, + 20*log10(audio_level) + 3.01); + if (outputbuf_samples > 0) { + unsigned int nchannel = stereo ? 2 : 1; + size_t buflen = output_buffer.queued_samples(); + fprintf(stderr, + " buf=%.1fs ", + buflen / nchannel / double(pcmrate)); + } + fflush(stderr); + + // Throw away first block. It is noisy because IF filters + // are still starting up. + if (block > 0) { + + // Write samples to output. + if (outputbuf_samples > 0) { + // Buffered write. + output_buffer.push(move(audiosamples)); + } else { + // Direct write. + audio_output->write(audiosamples); + } + } + + } + + // Join background threads. + source_thread.join(); + if (outputbuf_samples > 0) { + output_thread.join(); + } + + // TODO : cleanup + + return 0; +} + +/* end */