mirror of
https://github.com/f4exb/sdrangel.git
synced 2024-10-01 09:16:39 -04:00
BFM demod: working RDS demod
This commit is contained in:
f4exb
parent
8fa44a4ddb
commit
1a9dca1632
@ -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();
|
||||
}
|
||||
|
||||
|
||||
@ -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<Real> m_lowpass;
|
||||
fftfilt* m_rfFilter;
|
||||
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -19,49 +19,70 @@
|
||||
#ifndef PLUGINS_CHANNEL_BFM_RDSDEMOD_H_
|
||||
#define PLUGINS_CHANNEL_BFM_RDSDEMOD_H_
|
||||
|
||||
#include <QObject>
|
||||
#include <QHostAddress>
|
||||
#include <QUdpSocket>
|
||||
|
||||
#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;
|
||||
};
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user