BFM demod: RDS demod completed

plugins/channel/bfm/CMakeLists.txt

@@ -4,12 +4,14 @@ set(bfm_SOURCES
 	bfmdemod.cpp
 	bfmdemodgui.cpp
 	bfmplugin.cpp
+	rdsdemod.cpp
 )
 
 set(bfm_HEADERS
 	bfmdemod.h
 	bfmdemodgui.h
 	bfmplugin.h
+	rdsdemod.h
 )
 
 set(bfm_FORMS

plugins/channel/bfm/rdsdemod.cpp

@@ -16,6 +16,9 @@
 ///////////////////////////////////////////////////////////////////////////////////
 
 #include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
 #include "rdsdemod.h"
 
 RDSDemod::RDSDemod()
@@ -25,6 +28,15 @@ RDSDemod::RDSDemod()
 	m_rdsClockPhase = 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;
 }
 
 RDSDemod::~RDSDemod()
@@ -38,6 +50,34 @@ void RDSDemod::process(Real rdsSample, Real pilotSample)
 	// 1187.5 Hz clock
 	m_rdsClockPhase = (pilotSample / 48.0) + m_rdsClockOffset;
 	m_rdsClockLO = (fmod(m_rdsClockPhase, 2 * M_PI) < M_PI ? 1 : -1);
+
+	// Clock phase recovery
+	if (sign(m_rdsBB_1) != sign(m_rdsBB))
+	{
+		Real d_cphi = fmod(m_rdsClockPhase, M_PI);
+
+		if (d_cphi >= M_PI_2)
+		{
+			d_cphi -= M_PI;
+		}
+
+		m_rdsClockOffset -= 0.005 * d_cphi;
+	}
+
+	// Decimate band-limited signal
+	if (m_numSamples % 8 == 0)
+	{
+		/* biphase symbol integrate & dump */
+		m_acc += m_rdsBB * m_rdsClockLO;
+
+		if (sign(m_rdsClockLO) != sign(m_rdsClockLO_1))
+		{
+			biphase(m_acc);
+			m_acc = 0;
+		}
+
+		m_rdsClockLO_1 = m_rdsClockLO;
+	}
 }
 
 Real RDSDemod::filter_lp_2400_iq(Real input, int iqIndex)
@@ -55,3 +95,48 @@ Real RDSDemod::filter_lp_2400_iq(Real input, int iqIndex)
 	return m_yv[iqIndex][2];
 }
 
+int RDSDemod::sign(Real a)
+{
+	return (a >= 0 ? 1 : 0);
+}
+
+void RDSDemod::biphase(Real acc)
+{
+	static int reading_frame = 0;
+
+	if (sign(acc) != sign(m_acc_1)) // two successive of different sign: error detected
+	{
+		m_totErrors[m_counter % 2]++;
+	}
+
+	if (m_counter % 2 == reading_frame) // two successive of the same sing: OK
+	{
+		print_delta(sign(acc + m_acc_1));
+	}
+
+	if (m_counter == 0)
+	{
+		if (m_totErrors[1 - reading_frame] < m_totErrors[reading_frame])
+		{
+			reading_frame = 1 - reading_frame;
+		}
+
+		m_totErrors[0] = 0;
+		m_totErrors[1] = 0;
+	}
+
+	m_acc_1 = acc; // memorize (z^-1)
+	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)
+{
+	printf("%d", b);
+}
+

plugins/channel/bfm/rdsdemod.h

@@ -19,6 +19,8 @@
 #ifndef PLUGINS_CHANNEL_BFM_RDSDEMOD_H_
 #define PLUGINS_CHANNEL_BFM_RDSDEMOD_H_
 
+#include "dsp/dsptypes.h"
+
 class RDSDemod
 {
 public:
@@ -26,7 +28,13 @@ public:
 	~RDSDemod();
 
 	void process(Real rdsSample, Real pilotSample);
+
+protected:
 	Real filter_lp_2400_iq(Real in, int iqIndex);
+	int sign(Real a);
+	void biphase(Real acc);
+	void print_delta(char b);
+	void output_bit(char b);
 
 private:
 	Real m_xv[2][2+1];
@@ -36,6 +44,14 @@ private:
 	Real m_rdsClockPhase;
 	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;
 };
 
 #endif /* PLUGINS_CHANNEL_BFM_RDSDEMOD_H_ */