joris
/
SoftFM
1
0
Fork 0
Code Pull Requests Activity

Clean exit on Ctrl-C or SIGTERM. 

Use double instead of float for real-valued samples.
Start implementation of stereo pilot PLL.
This commit is contained in:
Joris van Rantwijk 2013-12-29 00:34:13 +01:00
parent 0ec6475902
commit 70ba5520ca
6 changed files with 201 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Filter.cc
View File

@ -11,8 +11,9 @@ using namespace std;
/** Prepare Lanczos FIR filter coefficients. */
template <class T>
static void make_lanczos_coeff(unsigned int filter_order, double cutoff,
SampleVector& coeff)
vector<T>& coeff)
{
coeff.resize(filter_order + 1);

2
Filter.h
View File

@ -48,7 +48,7 @@ public:
void process(const IQSampleVector& samples_in, IQSampleVector& samples_out);
private:
CoeffVector m_coeff;
std::vector<IQSample::value_type> m_coeff;
IQSampleVector m_state;
};

122
FmDecode.cc
View File

@ -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;

46
FmDecode.h
View File

@ -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;

6
SoftFM.h
View File

@ -7,15 +7,15 @@
typedef std::complex<float> IQSample;
typedef std::vector<IQSample> IQSampleVector;
typedef float Sample;
typedef double Sample;
typedef std::vector<Sample> SampleVector;
typedef float Coeff;
typedef double Coeff;
typedef std::vector<Coeff> 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;

40
main.cc
View File

@ -5,14 +5,18 @@
*/
#include <cstdlib>
#include <cstdio>
#include <climits>
#include <cmath>
#include <csignal>
#include <cstring>
#include <atomic>
#include <condition_variable>
#include <memory>
#include <mutex>
#include <queue>
#include <thread>
#include <unistd.h>
#include <getopt.h>
#include "SoftFM.h"
@ -178,6 +182,20 @@ void write_output_data(AudioOutput *output, DataBuffer<Sample> *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) {