From 1a9dca1632d5a383e65fd73b2c86ea1ea66e7fe7 Mon Sep 17 00:00:00 2001 From: f4exb Date: Sat, 12 Dec 2015 15:14:26 +0100 Subject: [PATCH] BFM demod: working RDS demod --- plugins/channel/bfm/bfmdemod.cpp | 26 ++-- plugins/channel/bfm/bfmdemod.h | 6 + plugins/channel/bfm/rdsdemod.cpp | 196 +++++++++++++++++++------------ plugins/channel/bfm/rdsdemod.h | 71 +++++++---- 4 files changed, 194 insertions(+), 105 deletions(-) diff --git a/plugins/channel/bfm/bfmdemod.cpp b/plugins/channel/bfm/bfmdemod.cpp index e4e00ca4f..f532ab313 100644 --- a/plugins/channel/bfm/bfmdemod.cpp +++ b/plugins/channel/bfm/bfmdemod.cpp @@ -92,7 +92,7 @@ void BFMDemod::configure(MessageQueue* messageQueue, void BFMDemod::feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end, bool firstOfBurst) { - Complex ci, cs; + Complex ci, cs, cr; fftfilt::cmplx *rf; int rf_out; Real msq, demod; @@ -136,8 +136,15 @@ void BFMDemod::feed(const SampleVector::const_iterator& begin, const SampleVecto if (m_running.m_rdsActive) { - m_rdsDemod.process(demod * 2.0 * cos(3.0 * m_pilotPLLSamples[2]), m_pilotPLLSamples[2]); - //m_rdsDemod.process(demod, m_pilotPLLSamples[2]); + //Complex r(demod * 2.0 * std::cos(3.0 * m_pilotPLLSamples[2]), 0.0); + Complex r(demod * 2.0 * std::cos(3.0 * m_pilotPLLSamples[2]), m_pilotPLLSamples[2]); + + if (m_interpolatorRDS.interpolate(&m_interpolatorRDSDistanceRemain, r, &cr)) + { + m_rdsDemod.process(cr.real(), cr.imag()); + m_interpolatorRDSDistanceRemain += m_interpolatorRDSDistance; + } + } Real sampleStereo; @@ -158,6 +165,7 @@ void BFMDemod::feed(const SampleVector::const_iterator& begin, const SampleVecto if (m_interpolatorStereo.interpolate(&m_interpolatorStereoDistanceRemain, s, &cs)) { sampleStereo = cs.real(); + m_interpolatorStereoDistanceRemain += m_interpolatorStereoDistance; } } @@ -291,11 +299,6 @@ bool BFMDemod::handleMessage(const Message& cmd) void BFMDemod::apply() { - if (m_config.m_inputSampleRate != m_running.m_inputSampleRate) - { - m_rdsDemod.setSampleRate(m_config.m_inputSampleRate); - } - if ((m_config.m_inputSampleRate != m_running.m_inputSampleRate) || (m_config.m_audioStereo && (m_config.m_audioStereo != m_running.m_audioStereo))) { @@ -314,12 +317,19 @@ void BFMDemod::apply() { m_settingsMutex.lock(); qDebug() << "BFMDemod::handleMessage: m_interpolator.create"; + m_interpolator.create(16, m_config.m_inputSampleRate, m_config.m_afBandwidth); m_interpolatorDistanceRemain = (Real) m_config.m_inputSampleRate / m_config.m_audioSampleRate; m_interpolatorDistance = (Real) m_config.m_inputSampleRate / (Real) m_config.m_audioSampleRate; + m_interpolatorStereo.create(16, m_config.m_inputSampleRate, m_config.m_afBandwidth); m_interpolatorStereoDistanceRemain = (Real) m_config.m_inputSampleRate / m_config.m_audioSampleRate; m_interpolatorStereoDistance = (Real) m_config.m_inputSampleRate / (Real) m_config.m_audioSampleRate; + + m_interpolatorRDS.create(4, m_config.m_inputSampleRate, 600.0); + m_interpolatorRDSDistanceRemain = (Real) m_config.m_inputSampleRate / 250000.0; + m_interpolatorRDSDistance = (Real) m_config.m_inputSampleRate / 250000.0; + m_settingsMutex.unlock(); } diff --git a/plugins/channel/bfm/bfmdemod.h b/plugins/channel/bfm/bfmdemod.h index cbe5a9f2e..eb5c0a8c5 100644 --- a/plugins/channel/bfm/bfmdemod.h +++ b/plugins/channel/bfm/bfmdemod.h @@ -159,9 +159,15 @@ private: Interpolator m_interpolator; //!< Interpolator between fixed demod bandwidth and audio bandwidth (rational) Real m_interpolatorDistance; Real m_interpolatorDistanceRemain; + Interpolator m_interpolatorStereo; //!< Twin Interpolator for stereo subcarrier Real m_interpolatorStereoDistance; Real m_interpolatorStereoDistanceRemain; + + Interpolator m_interpolatorRDS; //!< Twin Interpolator for stereo subcarrier + Real m_interpolatorRDSDistance; + Real m_interpolatorRDSDistanceRemain; + Lowpass m_lowpass; fftfilt* m_rfFilter; diff --git a/plugins/channel/bfm/rdsdemod.cpp b/plugins/channel/bfm/rdsdemod.cpp index 070fce839..5599d6432 100644 --- a/plugins/channel/bfm/rdsdemod.cpp +++ b/plugins/channel/bfm/rdsdemod.cpp @@ -23,77 +23,164 @@ #include "rdsdemod.h" const Real RDSDemod::m_pllBeta = 50; +const int RDSDemod::m_udpSize = 1472; RDSDemod::RDSDemod() { m_srate = 250000; - m_fsc = 57000.0; - m_subcarrPhi_1 = 0; - m_dPhiSc = 0; - m_subcarrBB_1 = 0.0; - m_rdsClockPhase = 0.0; - m_rdsClockPhase_1 = 0.0; - m_rdsClockOffset = 0.0; - m_rdsClockLO = 0.0; - m_rdsClockLO_1 = 0.0; - m_numSamples = 0; - m_acc = 0.0; - m_acc_1 = 0.0; - m_counter = 0; - m_readingFrame = 0; - m_totErrors[0] = 0; - m_totErrors[1] = 0; - m_dbit = 0; + + m_parms.subcarr_phi = 0; + m_parms.clock_offset = 0; + m_parms.clock_phi = 0; + m_parms.prev_clock_phi = 0; + m_parms.lo_clock = 0; + m_parms.prevclock = 0; + m_parms.prev_bb = 0; + m_parms.d_phi_sc = 0; + m_parms.d_cphi = 0; + m_parms.acc = 0; + m_parms.subcarr_sample = 0; + m_parms.c = 0; + m_parms.fmfreq = 0; + m_parms.bytesread; + m_parms.numsamples = 0; + m_parms.loop_out = 0; + m_parms.prev_loop = 0; + + m_parms.prev_acc = 0; + m_parms.counter = 0; + m_parms.reading_frame = 0; + + m_socket = new QUdpSocket(this); + m_sampleBufferIndex = 0; } RDSDemod::~RDSDemod() { + delete m_socket; } void RDSDemod::setSampleRate(int srate) { m_srate = srate; - m_fsc = 57000.0; } -void RDSDemod::process(Real demod, Real pilotPhaseSample) +void RDSDemod::process(Real demod, Real pilot) { - pilotPhaseSample = fmod(pilotPhaseSample, M_PI); - m_dPhiSc = pilotPhaseSample - m_subcarrPhi_1; + double fsc = 57000; - if (m_dPhiSc < 0) + Real dPilot = pilot - m_pilotPrev; + + if (dPilot < 0) { - m_dPhiSc += M_PI; + dPilot += 2 * M_PI; } - m_subcarrBB[0] = filter_lp_2400_iq(demod, 0); // working on real part only + if (m_sampleBufferIndex < m_udpSize) + { + m_sampleBufferIndex++; + } + else + { + m_socket->writeDatagram((const char*)&m_sampleBuffer[0], (qint64 ) (m_udpSize * sizeof(Real)), QHostAddress::LocalHost, 9995); + m_sampleBufferIndex = 0; + } - // 1187.5 Hz clock from 19 kHz pilot + // Subcarrier downmix & phase recovery - m_rdsClockPhase += (m_dPhiSc / 16.0) + m_rdsClockOffset; - m_rdsClockPhase = fmod(m_rdsClockPhase, M_PI); - m_rdsClockLO = (m_rdsClockPhase < 0.0 ? -1.0 : 1.0); + m_parms.subcarr_phi += 2 * M_PI * fsc * (1.0 / m_srate); + + /* + if (m_parms.subcarr_phi > 48 * 2 * M_PI) + { + m_parms.subcarr_phi -= 48 * 2 * M_PI; + }*/ + + m_parms.subcarr_bb[0] = filter_lp_2400_iq(demod, 0); + //m_parms.subcarr_bb[1] = filter_lp_2400_iq(demod * std::sin(m_parms.subcarr_phi), 1); + + m_parms.clock_phi += dPilot / 16.0; + m_parms.clock_phi = std::fmod(m_parms.clock_phi, 2 * M_PI); + m_parms.lo_clock = (m_parms.clock_phi < M_PI ? 1 : -1); + + m_sampleBuffer[m_sampleBufferIndex] = m_parms.lo_clock * m_parms.subcarr_bb[0]; // UDP debug + + // 1187.5 Hz clock + + m_parms.clock_phi = (m_parms.subcarr_phi / 48.0) + m_parms.clock_offset; // Clock phase recovery - if (sign(m_subcarrBB_1) != sign(m_subcarrBB[0])) + if (sign(m_parms.prev_bb) != sign(m_parms.subcarr_bb[0])) { - Real d_cphi = m_rdsClockPhase; + m_parms.d_cphi = std::fmod(m_parms.clock_phi, M_PI); - if (d_cphi >= M_PI_2) + if (m_parms.d_cphi >= M_PI_2) { - d_cphi -= M_PI; + m_parms.d_cphi -= M_PI; } - m_rdsClockOffset -= 0.005 * d_cphi; + m_parms.clock_offset -= 0.005 * m_parms.d_cphi; } - biphase(m_acc, m_rdsClockPhase - m_rdsClockPhase_1); + /* Decimate band-limited signal */ + if (m_parms.numsamples % 8 == 0) + { + /* biphase symbol integrate & dump */ + m_parms.acc += m_parms.subcarr_bb[0] * m_parms.lo_clock; - m_numSamples++; - m_subcarrPhi_1 = pilotPhaseSample; - m_rdsClockPhase_1 = m_rdsClockPhase; + if (sign(m_parms.lo_clock) != sign(m_parms.prevclock)) + { + biphase(m_parms.acc, m_parms.clock_phi - m_parms.prev_clock_phi); + m_parms.acc = 0; + } + + m_parms.prevclock = m_parms.lo_clock; + } + + m_parms.numsamples++; + m_parms.prev_bb = m_parms.subcarr_bb[0]; + m_parms.prev_clock_phi = m_parms.clock_phi; + m_prev = demod; + m_pilotPrev - pilot; +} + +void RDSDemod::biphase(Real acc, Real d_cphi) +{ + + if (sign(acc) != sign(m_parms.prev_acc)) // two successive of different sign: error detected + { + m_parms.tot_errs[m_parms.counter % 2]++; + } + + if (m_parms.counter % 2 == m_parms.reading_frame) // two successive of the same sing: OK + { + // TODO: take action print_delta(sign(acc + prev_acc)); + } + + if (m_parms.counter == 0) + { + if (m_parms.tot_errs[1 - m_parms.reading_frame] < m_parms.tot_errs[m_parms.reading_frame]) + { + m_parms.reading_frame = 1 - m_parms.reading_frame; + } + + Real qua = (1.0 * abs(m_parms.tot_errs[0] - m_parms.tot_errs[1]) / (m_parms.tot_errs[0] + m_parms.tot_errs[1])) * 100; + qDebug("RDSDemod::biphase: frame: %d acc: %+6.3f errs: %3d %3d qual: %3.0f%% clk: %7.2f\n", + m_parms.reading_frame, + acc, + m_parms.tot_errs[0], + m_parms.tot_errs[1], + qua, + (d_cphi / (2 * M_PI)) * m_srate); + + m_parms.tot_errs[0] = 0; + m_parms.tot_errs[1] = 0; + } + + m_parms.prev_acc = acc; // memorize (z^-1) + m_parms.counter = (m_parms.counter + 1) % 800; } Real RDSDemod::filter_lp_2400_iq(Real input, int iqIndex) @@ -125,38 +212,3 @@ int RDSDemod::sign(Real a) { return (a >= 0 ? 1 : 0); } - -void RDSDemod::biphase(Real acc, Real clockDPhi) -{ - if (clockDPhi < 0) - { - clockDPhi += M_PI; - } - - double fsc = (m_dPhiSc / (2.0 * M_PI)) * m_srate; - double fclk = (clockDPhi / (2.0 * M_PI)) * m_srate; - - if (m_counter == 0) - { - qDebug("RDSDemod::biphase: frame: %d pll: %.3f (ppm: %+8.3f) clk: %8.1f", - m_readingFrame, - fsc, - ((fsc - 19000.0) / 19000.0) * 1000000, - fclk); - } - - m_counter = (m_counter + 1) % 800; -} - -void RDSDemod::print_delta(char b) -{ - output_bit(b ^ m_dbit); - m_dbit = b; -} - -void RDSDemod::output_bit(char b) -{ - // TODO: return value instead of spitting out - //printf("%d", b); -} - diff --git a/plugins/channel/bfm/rdsdemod.h b/plugins/channel/bfm/rdsdemod.h index 4d4a29dd4..9a30f2306 100644 --- a/plugins/channel/bfm/rdsdemod.h +++ b/plugins/channel/bfm/rdsdemod.h @@ -19,49 +19,70 @@ #ifndef PLUGINS_CHANNEL_BFM_RDSDEMOD_H_ #define PLUGINS_CHANNEL_BFM_RDSDEMOD_H_ +#include +#include +#include + #include "dsp/dsptypes.h" -class RDSDemod +class RDSDemod : public QObject { + Q_OBJECT public: RDSDemod(); ~RDSDemod(); void setSampleRate(int srate); - void process(Real rdsSample, Real pilotPhaseSample); + void process(Real rdsSample, Real pilotSample); protected: + void biphase(Real acc, Real d_cphi); Real filter_lp_2400_iq(Real in, int iqIndex); Real filter_lp_pll(Real input); int sign(Real a); - void biphase(Real acc, Real dPhiClock); - void print_delta(char b); - void output_bit(char b); private: - int m_srate; - Real m_fsc; + struct + { + double subcarr_phi; + double subcarr_bb[2]; + double clock_offset; + double clock_phi; + double prev_clock_phi; + double lo_clock; + double prevclock; + double prev_bb; + double d_phi_sc; + double d_cphi; + double acc; + double subcarr_sample; + int c; + int fmfreq; + int bytesread; + int numsamples; + double loop_out; + double prev_loop; + double prev_acc; + int counter; + int reading_frame; + int tot_errs[2]; + } m_parms; + Real m_xv[2][2+1]; Real m_yv[2][2+1]; - Real m_xw[2]; - Real m_yw[2]; - Real m_subcarrPhi_1; - Real m_subcarrBB[2]; - Real m_dPhiSc; - Real m_subcarrBB_1; - Real m_rdsClockPhase; - Real m_rdsClockPhase_1; - Real m_rdsClockOffset; - Real m_rdsClockLO; - Real m_rdsClockLO_1; - int m_numSamples; - Real m_acc; - Real m_acc_1; - int m_counter; - int m_readingFrame; - int m_totErrors[2]; - int m_dbit; + Real m_xw[1+1]; + Real m_yw[1+1]; + Real m_prev; + Real m_pilotPrev; + + int m_srate; + + QUdpSocket *m_socket; + Real m_sampleBuffer[1<<12]; + int m_sampleBufferIndex; + + static const int m_udpSize; static const Real m_pllBeta; };