Completely redid the RDS demod

core/src/dsp/demod/broadcast_fm.h

@@ -49,6 +49,7 @@ namespace dsp::demod {
             audioFirTaps = taps::lowPass(15000.0, 4000.0, _samplerate);
             alFir.init(NULL, audioFirTaps);
             arFir.init(NULL, audioFirTaps);
+            xlator.init(NULL, -57000.0, samplerate);
             rdsResamp.init(NULL, samplerate, 5000.0);
 
             lmr = buffer::alloc<float>(STREAM_BUFFER_SIZE);
@@ -56,9 +57,9 @@ namespace dsp::demod {
             r = buffer::alloc<float>(STREAM_BUFFER_SIZE);
 
             lprDelay.out.free();
-            lmrDelay.out.free();
             arFir.out.free();
             alFir.out.free();
+            xlator.out.free();
             rdsResamp.out.free();
 
             base_type::init(in);
@@ -92,6 +93,7 @@ namespace dsp::demod {
             alFir.setTaps(audioFirTaps);
             arFir.setTaps(audioFirTaps);
 
+            xlator.setOffset(-57000.0, samplerate);
             rdsResamp.setInSamplerate(samplerate);
 
             reset();
@@ -139,7 +141,7 @@ namespace dsp::demod {
             base_type::tempStart();
         }
 
-        inline int process(int count, complex_t* in, stereo_t* out, int& rdsOutCount, float* rdsout = NULL) {
+        inline int process(int count, complex_t* in, stereo_t* out, int& rdsOutCount, complex_t* rdsout = NULL) {
             // Demodulate
             demod.process(count, in, demod.out.writeBuf);
             if (_stereo) {
@@ -152,24 +154,24 @@ namespace dsp::demod {
 
                 // Delay
                 lprDelay.process(count, demod.out.writeBuf, demod.out.writeBuf);
-                lmrDelay.process(count, rtoc.out.writeBuf, rtoc.out.writeBuf);
+                lmrDelay.process(count, rtoc.out.writeBuf, lmrDelay.out.writeBuf);
                 
                 // conjugate PLL output to down convert twice the L-R signal
                 math::Conjugate::process(count, pilotPLL.out.writeBuf, pilotPLL.out.writeBuf);
-                math::Multiply<dsp::complex_t>::process(count, rtoc.out.writeBuf, pilotPLL.out.writeBuf, rtoc.out.writeBuf);
+                math::Multiply<dsp::complex_t>::process(count, lmrDelay.out.writeBuf, pilotPLL.out.writeBuf, lmrDelay.out.writeBuf);
-                math::Multiply<dsp::complex_t>::process(count, rtoc.out.writeBuf, pilotPLL.out.writeBuf, rtoc.out.writeBuf);
+                math::Multiply<dsp::complex_t>::process(count, lmrDelay.out.writeBuf, pilotPLL.out.writeBuf, lmrDelay.out.writeBuf);
 
                 // Do RDS demod
                 if (_rdsOut) {
-                    // Since the PLL output is no longer needed after this, use it as the output
+                    // Translate to 0Hz
-                    math::Multiply<dsp::complex_t>::process(count, rtoc.out.writeBuf, pilotPLL.out.writeBuf, pilotPLL.out.writeBuf);
+                    xlator.process(count, rtoc.out.writeBuf, rtoc.out.writeBuf);
-                    convert::ComplexToReal::process(count, pilotPLL.out.writeBuf, rdsout);
+
-                    volk_32f_s32f_multiply_32f(rdsout, rdsout, 100.0, count);
+                    // Resample to the output samplerate
-                    rdsOutCount = rdsResamp.process(count, rdsout, rdsout);
+                    rdsOutCount = rdsResamp.process(count, rtoc.out.writeBuf, rdsout);
                 }
 
                 // Convert output back to real for further processing
-                convert::ComplexToReal::process(count, rtoc.out.writeBuf, lmr);
+                convert::ComplexToReal::process(count, lmrDelay.out.writeBuf, lmr);
 
                 // Amplify by 2x
                 volk_32f_s32f_multiply_32f(lmr, lmr, 2.0f, count);
@@ -193,24 +195,11 @@ namespace dsp::demod {
                     // Convert to complex
                     rtoc.process(count, demod.out.writeBuf, rtoc.out.writeBuf);
 
-                    // Filter out pilot and run through PLL
+                    // Translate to 0Hz
-                    pilotFir.process(count, rtoc.out.writeBuf, pilotFir.out.writeBuf);
+                    xlator.process(count, rtoc.out.writeBuf, rtoc.out.writeBuf);
-                    pilotPLL.process(count, pilotFir.out.writeBuf, pilotPLL.out.writeBuf);
 
-                    // Delay
+                    // Resample to the output samplerate
-                    lprDelay.process(count, demod.out.writeBuf, demod.out.writeBuf);
+                    rdsOutCount = rdsResamp.process(count, rtoc.out.writeBuf, rdsout);
-                    lmrDelay.process(count, rtoc.out.writeBuf, rtoc.out.writeBuf);
-                    
-                    // conjugate PLL output to down convert twice the L-R signal
-                    math::Conjugate::process(count, pilotPLL.out.writeBuf, pilotPLL.out.writeBuf);
-                    math::Multiply<dsp::complex_t>::process(count, rtoc.out.writeBuf, pilotPLL.out.writeBuf, rtoc.out.writeBuf);
-                    math::Multiply<dsp::complex_t>::process(count, rtoc.out.writeBuf, pilotPLL.out.writeBuf, rtoc.out.writeBuf);
-
-                    // Since the PLL output is no longer needed after this, use it as the output
-                    math::Multiply<dsp::complex_t>::process(count, rtoc.out.writeBuf, pilotPLL.out.writeBuf, pilotPLL.out.writeBuf);
-                    convert::ComplexToReal::process(count, pilotPLL.out.writeBuf, rdsout);
-                    volk_32f_s32f_multiply_32f(rdsout, rdsout, 100.0, count);
-                    rdsOutCount = rdsResamp.process(count, rdsout, rdsout);
                 }
 
                 // Filter if needed
@@ -240,7 +229,7 @@ namespace dsp::demod {
             return count;
         }
 
-        stream<float> rdsOut;
+        stream<complex_t> rdsOut;
 
     protected:
         double _deviation;
@@ -253,13 +242,14 @@ namespace dsp::demod {
         tap<complex_t> pilotFirTaps;
         filter::FIR<complex_t, complex_t> pilotFir;
         convert::RealToComplex rtoc;
+        channel::FrequencyXlator xlator;
         loop::PLL pilotPLL;
         math::Delay<float> lprDelay;
         math::Delay<complex_t> lmrDelay;
         tap<float> audioFirTaps;
         filter::FIR<float, float> arFir;
         filter::FIR<float, float> alFir;
-        multirate::RationalResampler<float> rdsResamp;
+        multirate::RationalResampler<dsp::complex_t> rdsResamp;
 
         float* lmr;
         float* l;

core/src/dsp/loop/phase_control_loop.h

@@ -65,6 +65,11 @@ namespace dsp::loop {
             if constexpr(CLAMP_PHASE) { clampPhase(); }
         }
 
+        inline void advancePhase() {
+            phase += freq;
+            if constexpr(CLAMP_PHASE) { clampPhase(); }
+        }
+
         T freq;
         T phase;
 

core/src/utils/riff.cpp

@@ -7,6 +7,14 @@ namespace riff {
     const char* LIST_SIGNATURE      = "LIST";
     const size_t RIFF_LABEL_SIZE    = 4;
 
+    // Writer::Writer(const Writer&& b) {
+    //     //file = std::move(b.file);
+    // }
+
+    Writer::~Writer() {
+        close();
+    }
+
     bool Writer::open(std::string path, const char form[4]) {
         std::lock_guard<std::recursive_mutex> lck(mtx);
 

core/src/utils/riff.h

@@ -20,6 +20,10 @@ namespace riff {
 
     class Writer {
     public:
+        Writer() {}
+        // Writer(const Writer&& b);
+        ~Writer();
+
         bool open(std::string path, const char form[4]);
         bool isOpen();
         void close();
@@ -40,4 +44,23 @@ namespace riff {
         std::ofstream file;
         std::stack<ChunkDesc> chunks;
     };
+
+    // class Reader {
+    // public:
+    //     Reader();
+    //     Reader(const Reader&& b);
+    //     ~Reader();
+
+    //     bool open(std::string path);
+    //     bool isOpen();
+    //     void close();
+
+    //     const std::string& form();
+
+    // private:
+
+    //     std::string _form;
+    //     std::recursive_mutex mtx;
+    //     std::ofstream file;
+    // };
 }

decoder_modules/atv_decoder/src/chroma_pll.h

@@ -0,0 +1,63 @@
+#pragma once
+#include <dsp/loop/pll.h>
+#include "chrominance_filter.h"
+
+// TODO: Should be 60 but had to try something
+#define BURST_START (63+CHROMA_FIR_DELAY)
+#define BURST_END   (BURST_START+28)
+
+#define A_PHASE     ((135.0/180.0)*FL_M_PI)
+#define B_PHASE     ((-135.0/180.0)*FL_M_PI)
+
+namespace dsp::loop {
+    class ChromaPLL : public PLL {
+        using base_type = PLL;
+    public:
+        ChromaPLL() {}
+
+        ChromaPLL(stream<complex_t>* in, double bandwidth, double initPhase = 0.0, double initFreq = 0.0, double minFreq = -FL_M_PI, double maxFreq = FL_M_PI) {
+            base_type::init(in, bandwidth, initFreq, initPhase, minFreq, maxFreq);
+        }
+
+        inline int process(int count, complex_t* in, complex_t* out, bool aphase = false) {
+            // Process the pre-burst section
+            for (int i = 0; i < BURST_START; i++) {
+                out[i] = in[i] * math::phasor(-pcl.phase);
+                pcl.advancePhase();
+            }
+
+            // Process the burst itself
+            if (aphase) {
+                for (int i = BURST_START; i < BURST_END; i++) {
+                    complex_t outVal = in[i] * math::phasor(-pcl.phase);
+                    out[i] = outVal;
+                    pcl.advance(math::normalizePhase(outVal.phase() - A_PHASE));
+                }
+            }
+            else {
+                for (int i = BURST_START; i < BURST_END; i++) {
+                    complex_t outVal = in[i] * math::phasor(-pcl.phase);
+                    out[i] = outVal;
+                    pcl.advance(math::normalizePhase(outVal.phase() - B_PHASE));
+                }
+            }
+            
+            
+            // Process the post-burst section
+            for (int i = BURST_END; i < count; i++) {
+                out[i] = in[i] * math::phasor(-pcl.phase);
+                pcl.advancePhase();
+            }
+
+            return count;
+        }
+
+        inline int processBlank(int count, complex_t* in, complex_t* out) {
+            for (int i = 0; i < count; i++) {
+                out[i] = in[i] * math::phasor(-pcl.phase);
+                pcl.advancePhase();
+            }
+            return count;
+        }
+    };
+}

decoder_modules/atv_decoder/src/chrominance_filter.h

@@ -0,0 +1,239 @@
+#pragma once
+#include <dsp/types.h>
+
+inline const dsp::complex_t CHROMA_FIR[] = {
+    {-0.000005461290583903, -0.000011336784355655},
+    { 0.000020060944485414,  0.000009851315045203},
+    {-0.000034177222729438,  0.000007245841504981},
+    { 0.000027694034878705, -0.000033114740542635},
+    {-0.000001217597841648,  0.000039141482370942},
+    {-0.000008324593371228, -0.000011315001355976},
+    {-0.000038085228233509, -0.000010585909953738},
+    { 0.000114833396071141, -0.000047778708840608},
+    {-0.000115428390169113,  0.000205816198882814},
+    {-0.000055467806072871, -0.000356692479491626},
+    { 0.000349316846854190,  0.000326162940234916},
+    {-0.000558465829929114, -0.000048001521408724},
+    { 0.000488176200631416, -0.000319593757302922},
+    {-0.000169437838021935,  0.000501610900725908},
+    {-0.000131793335799502, -0.000373003580727547},
+    { 0.000166817395492786,  0.000105930895534474},
+    { 0.000030499908326112, -0.000003048682668943},
+    {-0.000174999505027919,  0.000168008090089458},
+    { 0.000054431163395030, -0.000385174790951272},
+    { 0.000215876012859739,  0.000372695852521209},
+    {-0.000325534912280750, -0.000130173041693966},
+    { 0.000154951430569290, -0.000045395998708328},
+    { 0.000054324657659002, -0.000076028700470037},
+    { 0.000015664427565764,  0.000348002612845696},
+    {-0.000345943017888332, -0.000402175417043307},
+    { 0.000568731727879741,  0.000112347863435682},
+    {-0.000416485880859085,  0.000211750352828909},
+    { 0.000087462353623011, -0.000188197153014309},
+    {-0.000032082305030264, -0.000136804226080664},
+    { 0.000379089999045955,  0.000303466839685362},
+    {-0.000726760198519770, -0.000007022279302816},
+    { 0.000619888661818195, -0.000476871323359809},
+    {-0.000151885493742993,  0.000595641190573181},
+    {-0.000100626407015494, -0.000227947144491108},
+    {-0.000201935458823941, -0.000107628631934340},
+    { 0.000680260922139900, -0.000120771182888852},
+    {-0.000666108629277491,  0.000744775901128973},
+    { 0.000067236591919755, -0.001044125966364420},
+    { 0.000447037274751822,  0.000651912509450913},
+    {-0.000262675893448686, -0.000082499729563337},
+    {-0.000349821460486320,  0.000132102793530818},
+    { 0.000507024815168287, -0.000837598610490618},
+    { 0.000163814255478652,  0.001346530693477834},
+    {-0.000970457632383793, -0.000968411010101160},
+    { 0.000974834882891140,  0.000116507082762032},
+    {-0.000225464280571542,  0.000137131865995708},
+    {-0.000211542240694642,  0.000563783548428947},
+    {-0.000414412310798766, -0.001309793399193736},
+    { 0.001497010004594478,  0.001021907858926259},
+    {-0.001752019159639658,  0.000116536066154131},
+    { 0.000872822027879430, -0.000783952720205569},
+    {-0.000032439446797970,  0.000184988059956734},
+    { 0.000446259382722895,  0.000833040920509238},
+    {-0.001741577737284306, -0.000764423771425237},
+    { 0.002306569133792772, -0.000593352416441601},
+    {-0.001336084746214192,  0.001744394557524181},
+    {-0.000015810020735495, -0.001342809547658260},
+    { 0.000007636494885364,  0.000009498318627546},
+    { 0.001403876768349702,  0.000326101441888391},
+    {-0.002351020828600226,  0.001098649819278302},
+    { 0.001389314639579544, -0.002746943712072884},
+    { 0.000526319899588909,  0.002635084366837732},
+    {-0.001109526585744687, -0.000950323796527721},
+    {-0.000307792427984886, -0.000013203419520794},
+    { 0.001737955094951111, -0.001247368808692850},
+    {-0.000974502437588420,  0.003352512117661680},
+    {-0.001462571137390936, -0.003635296917435679},
+    { 0.002783459090201693,  0.001604420226187745},
+    {-0.001471518558760170,  0.000211117948702137},
+    {-0.000575340825070194,  0.000601820846100026},
+    { 0.000302090333345692, -0.003088058972305493},
+    { 0.002496092353182990,  0.003912508340989065},
+    {-0.004645661091012423, -0.001630427298020200},
+    { 0.003556824805628799, -0.001209822327859352},
+    {-0.000744999556260706,  0.001143238699138109},
+    { 0.000144278726929409,  0.001638049051599065},
+    {-0.003025291044450178, -0.003226370992887968},
+    { 0.006047866290490120,  0.000927406808799887},
+    {-0.005338456415106141,  0.003008811999350399},
+    { 0.001642959659014839, -0.003972384205231079},
+    { 0.000273874932822212,  0.000977326273749033},
+    { 0.002315022846573390,  0.001695671268241410},
+    {-0.006240953957978884,  0.000207330368698293},
+    { 0.006164252120861735, -0.005177351717451013},
+    {-0.001560310257561104,  0.007437030759707700},
+    {-0.002131333814462852, -0.004317129694157112},
+    { 0.000280518918541908,  0.000134405998842553},
+    { 0.004612116481180659, -0.001024468120657814},
+    {-0.005599300279638699,  0.006828277067771868},
+    { 0.000228879728552504, -0.010675998154712657},
+    { 0.005692081512980654,  0.007582243186569848},
+    {-0.005100500569859509, -0.001364751685737153},
+    {-0.000902490398043454,  0.000385770160220703},
+    { 0.003673858819546609, -0.006701685283451640},
+    { 0.002079056046131593,  0.012568579063417429},
+    {-0.010730008156911677, -0.009826454574016218},
+    { 0.012092401380903161,  0.000921764172237851},
+    {-0.004714530989129091,  0.003151948807627123},
+    {-0.001055930168838909,  0.003228576712467020},
+    {-0.004343270165991213, -0.011924332879354394},
+    { 0.016499994418955999,  0.010255324919126899},
+    {-0.021047239750251585,  0.002309419513135448},
+    { 0.011855513874047341, -0.011604071033866310},
+    {-0.000777842281358575,  0.005916341648175263},
+    { 0.004380939277688377,  0.007397670455730446},
+    {-0.021891594662401131, -0.008509480947490166},
+    { 0.032787638290674201, -0.009950745850861956},
+    {-0.021022579272463194,  0.030030850567389102},
+    {-0.001508145650189953, -0.027571914870304640},
+    { 0.004056649693022923,  0.004624901687718579},
+    { 0.025728742586666287,  0.004824671348397606},
+    {-0.058002700931665603,  0.030198618296813803},
+    { 0.043631619628438784, -0.096308304333327280},
+    { 0.033451363423624300,  0.136687079396426990},
+    {-0.129387018420204200, -0.101540513046619400},
+    { 0.172881344826560730, -0.000000000000005297},
+    {-0.129387018420198010,  0.101540513046627330},
+    { 0.033451363423615862, -0.136687079396429050},
+    { 0.043631619628444723,  0.096308304333324601},
+    {-0.058002700931667456, -0.030198618296810247},
+    { 0.025728742586665992, -0.004824671348399184},
+    { 0.004056649693022639, -0.004624901687718827},
+    {-0.001508145650188251,  0.027571914870304734},
+    {-0.021022579272465047, -0.030030850567387805},
+    { 0.032787638290674812,  0.009950745850859947},
+    {-0.021891594662400610,  0.008509480947491507},
+    { 0.004380939277687923, -0.007397670455730714},
+    {-0.000777842281358940, -0.005916341648175215},
+    { 0.011855513874048058,  0.011604071033865578},
+    {-0.021047239750251731, -0.002309419513134139},
+    { 0.016499994418955360, -0.010255324919127926},
+    {-0.004343270165990471,  0.011924332879354665},
+    {-0.001055930168839110, -0.003228576712466955},
+    {-0.004714530989129287, -0.003151948807626830},
+    { 0.012092401380903103, -0.000921764172238603},
+    {-0.010730008156911072,  0.009826454574016881},
+    { 0.002079056046130817, -0.012568579063417559},
+    { 0.003673858819547020,  0.006701685283451416},
+    {-0.000902490398043478, -0.000385770160220647},
+    {-0.005100500569859424,  0.001364751685737466},
+    { 0.005692081512980187, -0.007582243186570198},
+    { 0.000228879728553163,  0.010675998154712643},
+    {-0.005599300279639117, -0.006828277067771524},
+    { 0.004612116481180722,  0.001024468120657532},
+    { 0.000280518918541900, -0.000134405998842571},
+    {-0.002131333814462586,  0.004317129694157243},
+    {-0.001560310257561563, -0.007437030759707604},
+    { 0.006164252120862052,  0.005177351717450635},
+    {-0.006240953957978898, -0.000207330368697911},
+    { 0.002315022846573286, -0.001695671268241552},
+    { 0.000273874932822152, -0.000977326273749050},
+    { 0.001642959659015084,  0.003972384205230976},
+    {-0.005338456415106324, -0.003008811999350072},
+    { 0.006047866290490063, -0.000927406808800258},
+    {-0.003025291044449980,  0.003226370992888153},
+    { 0.000144278726929308, -0.001638049051599074},
+    {-0.000744999556260777, -0.001143238699138063},
+    { 0.003556824805628873,  0.001209822327859134},
+    {-0.004645661091012323,  0.001630427298020484},
+    { 0.002496092353182751, -0.003912508340989219},
+    { 0.000302090333345882,  0.003088058972305475},
+    {-0.000575340825070231, -0.000601820846099991},
+    {-0.001471518558760183, -0.000211117948702046},
+    { 0.002783459090201593, -0.001604420226187919},
+    {-0.001462571137390710,  0.003635296917435769},
+    {-0.000974502437588628, -0.003352512117661619},
+    { 0.001737955094951189,  0.001247368808692742},
+    {-0.000307792427984885,  0.000013203419520814},
+    {-0.001109526585744628,  0.000950323796527789},
+    { 0.000526319899588746, -0.002635084366837765},
+    { 0.001389314639579712,  0.002746943712072799},
+    {-0.002351020828600294, -0.001098649819278158},
+    { 0.001403876768349682, -0.000326101441888477},
+    { 0.000007636494885364, -0.000009498318627546},
+    {-0.000015810020735412,  0.001342809547658261},
+    {-0.001336084746214299, -0.001744394557524099},
+    { 0.002306569133792808,  0.000593352416441460},
+    {-0.001741577737284259,  0.000764423771425344},
+    { 0.000446259382722843, -0.000833040920509266},
+    {-0.000032439446797982, -0.000184988059956732},
+    { 0.000872822027879478,  0.000783952720205515},
+    {-0.001752019159639665, -0.000116536066154024},
+    { 0.001497010004594416, -0.001021907858926351},
+    {-0.000414412310798685,  0.001309793399193761},
+    {-0.000211542240694677, -0.000563783548428934},
+    {-0.000225464280571550, -0.000137131865995694},
+    { 0.000974834882891133, -0.000116507082762092},
+    {-0.000970457632383734,  0.000968411010101219},
+    { 0.000163814255478569, -0.001346530693477844},
+    { 0.000507024815168339,  0.000837598610490586},
+    {-0.000349821460486328, -0.000132102793530797},
+    {-0.000262675893448681,  0.000082499729563353},
+    { 0.000447037274751782, -0.000651912509450940},
+    { 0.000067236591919819,  0.001044125966364416},
+    {-0.000666108629277537, -0.000744775901128932},
+    { 0.000680260922139908,  0.000120771182888810},
+    {-0.000201935458823935,  0.000107628631934352},
+    {-0.000100626407015480,  0.000227947144491114},
+    {-0.000151885493743030, -0.000595641190573172},
+    { 0.000619888661818225,  0.000476871323359771},
+    {-0.000726760198519770,  0.000007022279302861},
+    { 0.000379089999045936, -0.000303466839685386},
+    {-0.000032082305030256,  0.000136804226080666},
+    { 0.000087462353623023,  0.000188197153014303},
+    {-0.000416485880859098, -0.000211750352828883},
+    { 0.000568731727879734, -0.000112347863435717},
+    {-0.000345943017888307,  0.000402175417043329},
+    { 0.000015664427565742, -0.000348002612845697},
+    { 0.000054324657659007,  0.000076028700470034},
+    { 0.000154951430569292,  0.000045395998708319},
+    {-0.000325534912280742,  0.000130173041693986},
+    { 0.000215876012859716, -0.000372695852521222},
+    { 0.000054431163395054,  0.000385174790951269},
+    {-0.000174999505027930, -0.000168008090089447},
+    { 0.000030499908326113,  0.000003048682668941},
+    { 0.000166817395492779, -0.000105930895534485},
+    {-0.000131793335799479,  0.000373003580727555},
+    {-0.000169437838021966, -0.000501610900725898},
+    { 0.000488176200631435,  0.000319593757302892},
+    {-0.000558465829929111,  0.000048001521408758},
+    { 0.000349316846854170, -0.000326162940234938},
+    {-0.000055467806072849,  0.000356692479491629},
+    {-0.000115428390169126, -0.000205816198882806},
+    { 0.000114833396071144,  0.000047778708840601},
+    {-0.000038085228233508,  0.000010585909953741},
+    {-0.000008324593371228,  0.000011315001355977},
+    {-0.000001217597841650, -0.000039141482370942},
+    { 0.000027694034878707,  0.000033114740542633},
+    {-0.000034177222729439, -0.000007245841504979},
+    { 0.000020060944485413, -0.000009851315045204},
+    {-0.000005461290583903,  0.000011336784355656},
+};
+
+#define CHROMA_FIR_SIZE     (sizeof(CHROMA_FIR)/sizeof(dsp::complex_t))
+#define CHROMA_FIR_DELAY    ((CHROMA_FIR_SIZE-1)/2)

decoder_modules/atv_decoder/src/linesync.h

@@ -0,0 +1,193 @@
+#pragma once
+#include <dsp/processor.h>
+#include <dsp/loop/phase_control_loop.h>
+#include <dsp/taps/windowed_sinc.h>
+#include <dsp/multirate/polyphase_bank.h>
+#include <dsp/math/step.h>
+
+class LineSync : public dsp::Processor<float, float> {
+    using base_type = dsp::Processor<float, float>;
+public:
+    LineSync() {}
+
+    LineSync(dsp::stream<float>* in, double omega, double omegaGain, double muGain, double omegaRelLimit, int interpPhaseCount = 128, int interpTapCount = 8) { init(in, omega, omegaGain, muGain, omegaRelLimit, interpPhaseCount, interpTapCount); }
+
+    ~LineSync() {
+        if (!base_type::_block_init) { return; }
+        base_type::stop();
+        dsp::multirate::freePolyphaseBank(interpBank);
+        dsp::buffer::free(buffer);
+    }
+
+    void init(dsp::stream<float>* in, double omega, double omegaGain, double muGain, double omegaRelLimit, int interpPhaseCount = 128, int interpTapCount = 8) {
+        _omega = omega;
+        _omegaGain = omegaGain;
+        _muGain = muGain;
+        _omegaRelLimit = omegaRelLimit;
+        _interpPhaseCount = interpPhaseCount;
+        _interpTapCount = interpTapCount;
+
+        pcl.init(_muGain, _omegaGain, 0.0, 0.0, 1.0, _omega, _omega * (1.0 - omegaRelLimit), _omega * (1.0 + omegaRelLimit));
+        generateInterpTaps();
+        buffer = dsp::buffer::alloc<float>(STREAM_BUFFER_SIZE + _interpTapCount);
+        bufStart = &buffer[_interpTapCount - 1];
+    
+        base_type::init(in);
+    }
+
+    void setOmegaGain(double omegaGain) {
+        assert(base_type::_block_init);
+        std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+        _omegaGain = omegaGain;
+        pcl.setCoefficients(_muGain, _omegaGain);
+    }
+
+    void setMuGain(double muGain) {
+        assert(base_type::_block_init);
+        std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+        _muGain = muGain;
+        pcl.setCoefficients(_muGain, _omegaGain);
+    }
+
+    void setOmegaRelLimit(double omegaRelLimit) {
+        assert(base_type::_block_init);
+        std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+        _omegaRelLimit = omegaRelLimit;
+        pcl.setFreqLimits(_omega * (1.0 - _omegaRelLimit), _omega * (1.0 + _omegaRelLimit));
+    }
+
+    void setSyncLevel(float level) {
+        assert(base_type::_block_init);
+        std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+        syncLevel = level;
+    }
+
+    void setInterpParams(int interpPhaseCount, int interpTapCount) {
+        assert(base_type::_block_init);
+        std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+        base_type::tempStop();
+        _interpPhaseCount = interpPhaseCount;
+        _interpTapCount = interpTapCount;
+        dsp::multirate::freePolyphaseBank(interpBank);
+        dsp::buffer::free(buffer);
+        generateInterpTaps();
+        buffer = dsp::buffer::alloc<float>(STREAM_BUFFER_SIZE + _interpTapCount);
+        bufStart = &buffer[_interpTapCount - 1];
+        base_type::tempStart();
+    }
+
+    void reset() {
+        assert(base_type::_block_init);
+        std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+        base_type::tempStop();
+        offset = 0;
+        pcl.phase = 0.0f;
+        pcl.freq = _omega;
+        base_type::tempStart();
+    }
+
+    int run() {
+        int count = base_type::_in->read();
+        if (count < 0) { return -1; }
+
+        // Copy data to work buffer
+        memcpy(bufStart, base_type::_in->readBuf, count * sizeof(float));
+
+        if (test2) {
+            test2 = false;
+            offset += 5;
+        }
+
+        // Process all samples
+        while (offset < count) {
+            // Calculate new output value
+            int phase = std::clamp<int>(floorf(pcl.phase * (float)_interpPhaseCount), 0, _interpPhaseCount - 1);
+            float outVal;
+            volk_32f_x2_dot_prod_32f(&outVal, &buffer[offset], interpBank.phases[phase], _interpTapCount);
+            base_type::out.writeBuf[outCount++] = outVal;
+
+            // If the end of the line is reached, process it and determin error
+            float error = 0;
+            if (outCount >= 720) {
+                // Compute averages.
+                float left = 0.0f, right = 0.0f;
+                for (int i = (720-17); i < 720; i++) {
+                    left += base_type::out.writeBuf[i];
+                }
+                for (int i = 0; i < 27; i++) {
+                    left += base_type::out.writeBuf[i];
+                }
+                for (int i = 27; i < (54+17); i++) {
+                    right += base_type::out.writeBuf[i];
+                }
+                left *= (1.0f/44.0f);
+                right *= (1.0f/44.0f);
+
+                // If the sync is present, compute error
+                if ((left < syncLevel && right < syncLevel) && !forceLock) {
+                    error = (left + syncBias - right);
+                    locked = true;
+                }
+                else {
+                    locked = false;
+                }
+
+                if (++counter >= 100) {
+                    counter = 0;
+                    //flog::warn("Left: {}, Right: {}, Error: {}, Freq: {}, Phase: {}", left, right, error, pcl.freq, pcl.phase);
+                }
+
+                // Output line
+                if (!base_type::out.swap(outCount)) { break; }
+                outCount = 0;
+            }
+
+            // Advance symbol offset and phase
+            pcl.advance(error);
+            float delta = floorf(pcl.phase);
+            offset += delta;
+            pcl.phase -= delta;
+        }
+        offset -= count;
+
+        // Update delay buffer
+        memmove(buffer, &buffer[count], (_interpTapCount - 1) * sizeof(float));
+
+        // Swap if some data was generated
+        base_type::_in->flush();
+        return outCount;
+    }
+
+    bool locked = false;
+    bool test2 = false;
+
+    float syncBias = 0.0f;
+    bool forceLock = false;
+
+    int counter = 0;
+
+protected:
+    void generateInterpTaps() {
+        double bw = 0.5 / (double)_interpPhaseCount;
+        dsp::tap<float> lp = dsp::taps::windowedSinc<float>(_interpPhaseCount * _interpTapCount, dsp::math::hzToRads(bw, 1.0), dsp::window::nuttall, _interpPhaseCount);
+        interpBank = dsp::multirate::buildPolyphaseBank<float>(_interpPhaseCount, lp);
+        dsp::taps::free(lp);
+    }
+
+    dsp::multirate::PolyphaseBank<float> interpBank;
+    dsp::loop::PhaseControlLoop<double, false> pcl;
+
+    double _omega;
+    double _omegaGain;
+    double _muGain;
+    double _omegaRelLimit;
+    int _interpPhaseCount;
+    int _interpTapCount;
+
+    int offset = 0;
+    int outCount = 0;
+    float* buffer;
+    float* bufStart;
+    
+    float syncLevel = -0.03f;
+};

decoder_modules/atv_decoder/src/main.cpp

@@ -10,6 +10,15 @@
 
 #include <dsp/demod/quadrature.h>
 #include <dsp/sink/handler_sink.h>
+#include "linesync.h"
+#include <dsp/loop/pll.h>
+#include <dsp/convert/real_to_complex.h>
+#include <dsp/filter/fir.h>
+#include <dsp/taps/from_array.h>
+
+#include "chrominance_filter.h"
+
+#include "chroma_pll.h"
 
 #define CONCAT(a, b) ((std::string(a) + b).c_str())
 
@@ -17,7 +26,8 @@ SDRPP_MOD_INFO{/* Name:            */ "atv_decoder",
                /* Description:     */ "ATV decoder for SDR++",
                /* Author:          */ "Ryzerth",
                /* Version:         */ 0, 1, 0,
-               /* Max instances    */ -1};
+               /* Max instances    */ -1
+};
 
 #define SAMPLE_RATE (625.0f * 720.0f * 25.0f)
 
@@ -29,9 +39,16 @@ class ATVDecoderModule : public ModuleManager::Instance {
         vfo = sigpath::vfoManager.createVFO(name, ImGui::WaterfallVFO::REF_CENTER, 0, 8000000.0f, SAMPLE_RATE, SAMPLE_RATE, SAMPLE_RATE, true);
 
         demod.init(vfo->output, SAMPLE_RATE, SAMPLE_RATE / 2.0f);
-        sink.init(&demod.out, handler, this);
+        sync.init(&demod.out, 1.0f, 1e-6, 1.0, 0.05);
+        sink.init(&sync.out, handler, this);
+
+        r2c.init(NULL);
+        chromaTaps = dsp::taps::fromArray(CHROMA_FIR_SIZE, CHROMA_FIR);
+        fir.init(NULL, chromaTaps);
+        pll.init(NULL, 0.01, 0.0, dsp::math::hzToRads(4433618.75, SAMPLE_RATE), dsp::math::hzToRads(4433618.75*0.90, SAMPLE_RATE), dsp::math::hzToRads(4433618.75*1.1, SAMPLE_RATE));
 
         demod.start();
+        sync.start();
         sink.start();
 
         gui::menu.registerEntry(name, menuHandler, this, this);
@@ -47,9 +64,13 @@ class ATVDecoderModule : public ModuleManager::Instance {
 
     void postInit() {}
 
-    void enable() { enabled = true; }
+    void enable() {
+        enabled = true;
+    }
 
-    void disable() { enabled = false; }
+    void disable() {
+        enabled = false;
+    }
 
     bool isEnabled() { return enabled; }
 
@@ -61,6 +82,8 @@ class ATVDecoderModule : public ModuleManager::Instance {
             style::beginDisabled();
         }
 
+        // Ideal width for testing: 750pixels
+
         ImGui::FillWidth();
         _this->img.draw();
 
@@ -76,6 +99,28 @@ class ATVDecoderModule : public ModuleManager::Instance {
         ImGui::FillWidth();
         ImGui::SliderFloat("##spanLvl", &_this->spanLvl, 0, 1.0);
 
+        ImGui::LeftLabel("Sync Bias");
+        ImGui::FillWidth();
+        ImGui::SliderFloat("##syncBias", &_this->sync.syncBias,-0.1, 0.1);
+
+        if (ImGui::Button("Test2")) {
+            _this->sync.test2 = true;
+        }
+
+        if (ImGui::Button("Switch frame")) {
+            std::lock_guard<std::mutex> lck(_this->evenFrameMtx);
+            _this->evenFrame = !_this->evenFrame;
+        }
+
+        if (_this->sync.locked) {
+            ImGui::TextColored(ImVec4(0, 1, 0, 1), "Locked");
+        }
+        else {
+            ImGui::TextUnformatted("Not locked");
+        }
+
+        ImGui::Checkbox("Force Lock", &_this->sync.forceLock);
+
         if (!_this->enabled) {
             style::endDisabled();
         }
@@ -84,70 +129,66 @@ class ATVDecoderModule : public ModuleManager::Instance {
     static void handler(float *data, int count, void *ctx) {
         ATVDecoderModule *_this = (ATVDecoderModule *)ctx;
 
-        uint8_t *buf = (uint8_t *)_this->img.buffer;
+        // Convert line to complex
-        float val;
+        _this->r2c.process(720, data, _this->r2c.out.writeBuf);
-        float imval;
+
-        int pos = 0;
+        // Isolate the chroma subcarrier
+        _this->fir.process(720, _this->r2c.out.writeBuf, _this->fir.out.writeBuf);
+
+        // Run chroma carrier through the PLL
+        _this->pll.process(720, _this->fir.out.writeBuf, _this->pll.out.writeBuf, ((_this->ypos%2)==1) ^ _this->evenFrame);
+
+        // Render line to the image without color
+        //int lypos = _this->ypos - 1;
+        //if (lypos < 0) { lypos = 624; }
+        //uint32_t* lastLine = &((uint32_t *)_this->img.buffer)[(lypos < 313) ? (lypos*720*2) : ((((lypos - 313)*2)+1)*720) ];
+        //uint32_t* currentLine = &((uint32_t *)_this->img.buffer)[(_this->ypos < 313) ? (_this->ypos*720*2) : ((((_this->ypos - 313)*2)+1)*720) ];
+
+        uint32_t* currentLine = &((uint32_t *)_this->img.buffer)[_this->ypos*720];
+
         for (int i = 0; i < count; i++) {
-            val = data[i];
+            //float imval = std::clamp<float>((data[i] - _this->minLvl) * 255.0 / _this->spanLvl, 0, 255);
-            // Sync
+            uint32_t re = std::clamp<float>((_this->pll.out.writeBuf[i].re - _this->minLvl) * 255.0 / _this->spanLvl, 0, 255);
-            if (val < _this->sync_level) {
+            uint32_t im = std::clamp<float>((_this->pll.out.writeBuf[i].im - _this->minLvl) * 255.0 / _this->spanLvl, 0, 255);
-                _this->sync_count++;
+            currentLine[i] = 0xFF000000 | (im << 8) | re;
-            }
+        }
-            else {
+    
-                if (_this->sync_count >= 300) {
+        // Vertical scan logic
-                    _this->short_sync = 0;
+        _this->ypos++;
-                }
+        bool rollover = _this->ypos >= 625;
-                else if (_this->sync_count >= 33) {
+        if (rollover) {
-                    if (_this->short_sync == 5) {
+            {
-                        _this->even_field = false;
+                std::lock_guard<std::mutex> lck(_this->evenFrameMtx);
-                        _this->ypos = 0;
+                _this->evenFrame = !_this->evenFrame;
-                        _this->img.swap();
-                        buf = (uint8_t *)_this->img.buffer;
-                    }
-                    else if (_this->short_sync == 4) {
-                        _this->even_field = true;
-                        _this->ypos = 0;
-                    }
-                    _this->xpos = 0;
-                    _this->short_sync = 0;
-                }
-                else if (_this->sync_count >= 15) {
-                    _this->short_sync++;
-                }
-                _this->sync_count = 0;
             }
+            _this->ypos = 0;
+            _this->img.swap();
+        }
 
+        // Measure vsync levels
+        float sync0 = 0.0f, sync1 = 0.0f;
+        for (int i = 0; i < 306; i++) {
+            sync0 += data[i];
+        }
+        for (int i = (720/2); i < ((720/2)+306); i++) {
+            sync1 += data[i];
+        }
+        sync0 *= (1.0f/305.0f);
+        sync1 *= (1.0f/305.0f);
 
-            // Draw
+        // Save sync detection to history
-            imval = std::clamp<float>((val - _this->minLvl) * 255.0 / _this->spanLvl, 0, 255);
+        _this->syncHistory >>= 2;
-            if (_this->even_field) {
+        _this->syncHistory |= (((uint16_t)(sync1 < _this->sync_level)) << 9) | (((uint16_t)(sync0 < _this->sync_level)) << 8);
-                pos = ((720 * _this->ypos * 2) + _this->xpos) * 4;
-            }
-            else {
-                pos = ((720 * (_this->ypos * 2 + 1)) + _this->xpos) * 4;
-            }
 
-            buf[pos] = imval;
+        // Trigger vsync in case one is detected
-            buf[pos + 1] = imval;
+        // TODO: Also sync with odd field
-            buf[pos + 2] = imval;
+        if (!rollover && _this->syncHistory == 0b0000011111) {
-            buf[pos + 3] = imval;
+            {
-
+                std::lock_guard<std::mutex> lck(_this->evenFrameMtx);
-            // Image logic
+                _this->evenFrame = !_this->evenFrame;
-            _this->xpos++;
-            if (_this->xpos >= 720) {
-                _this->ypos++;
-                _this->xpos = 0;
-            }
-            if (_this->ypos >= 312) {
-                _this->ypos = 0;
-                _this->xpos = 0;
-                _this->even_field = !_this->even_field;
-                if (_this->even_field) {
-                    _this->img.swap();
-                    buf = (uint8_t *)_this->img.buffer;
-                }
             }
+            _this->ypos = 0;
+            _this->img.swap();
         }
     }
 
@@ -156,19 +197,27 @@ class ATVDecoderModule : public ModuleManager::Instance {
 
     VFOManager::VFO *vfo = NULL;
     dsp::demod::Quadrature demod;
+    LineSync sync;
     dsp::sink::Handler<float> sink;
-
+    dsp::convert::RealToComplex r2c;
-    int xpos = 0;
+    dsp::tap<dsp::complex_t> chromaTaps;
+    dsp::filter::FIR<dsp::complex_t, dsp::complex_t> fir;
+    dsp::loop::ChromaPLL pll;
     int ypos = 0;
-    bool even_field = false;
 
-    float sync_level = -0.3f;
+    bool evenFrame = false;
+    std::mutex evenFrameMtx;
+
+    float sync_level = -0.06f;
     int sync_count = 0;
     int short_sync = 0;
 
     float minLvl = 0.0f;
     float spanLvl = 1.0f;
 
+    bool lockedLines = 0;
+    uint16_t syncHistory = 0;
+
     ImGui::ImageDisplay img;
 };
 

decoder_modules/radio/src/demodulators/wfm.h

@@ -2,11 +2,13 @@
 #include "../demod.h"
 #include <dsp/demod/broadcast_fm.h>
 #include <dsp/clock_recovery/mm.h>
-#include <dsp/clock_recovery/fd.h>
+#include <dsp/loop/fast_agc.h>
+#include <dsp/loop/costas.h>
 #include <dsp/taps/root_raised_cosine.h>
 #include <dsp/digital/binary_slicer.h>
 #include <dsp/digital/manchester_decoder.h>
 #include <dsp/digital/differential_decoder.h>
+#include <dsp/routing/doubler.h>
 #include <gui/widgets/symbol_diagram.h>
 #include <fstream>
 #include <rds.h>
@@ -14,9 +16,9 @@
 namespace demod {
     class WFM : public Demodulator {
     public:
-        WFM() {}
+        WFM() : diag(0.5, 4096)  {}
 
-        WFM(std::string name, ConfigManager* config, dsp::stream<dsp::complex_t>* input, double bandwidth, double audioSR) {
+        WFM(std::string name, ConfigManager* config, dsp::stream<dsp::complex_t>* input, double bandwidth, double audioSR) : diag(0.5, 4096) {
             init(name, config, input, bandwidth, audioSR);
         }
 
@@ -45,33 +47,66 @@ namespace demod {
             if (config->conf[name][getName()].contains("rds")) {
                 _rds = config->conf[name][getName()]["rds"];
             }
+            if (config->conf[name][getName()].contains("rdsInfo")) {
+                _rdsInfo = config->conf[name][getName()]["rdsInfo"];
+            }
             _config->release(modified);
 
             // Define structure
             demod.init(input, bandwidth / 2.0f, getIFSampleRate(), _stereo, _lowPass, _rds);
-            recov.init(&demod.rdsOut, 5000.0 / 2375, omegaGain, muGain, 0.01);
+            agc.init(&demod.rdsOut, 1.0, 1e6, 0.1);
-            slice.init(&recov.out);
+            costas.init(&agc.out,  0.005f);
-            manch.init(&slice.out);
+
-            diff.init(&manch.out, 2);
+            taps = dsp::taps::bandPass<dsp::complex_t>(0, 2375, 100, 5000);
+            fir.init(&costas.out, taps);
+            double baudfreq = dsp::math::hzToRads(2375.0/2.0, 5000);
+            costas2.init(&fir.out, 0.01, 0.0, baudfreq, baudfreq - (baudfreq*0.1), baudfreq + (baudfreq*0.1));
+
+            c2r.init(&costas2.out);
+            recov.init(&c2r.out, 5000.0 / (2375.0 / 2.0), 1e-6, 0.01, 0.01);
+            slice.init(&doubler.outA);
+            diff.init(&slice.out, 2);
             hs.init(&diff.out, rdsHandler, this);
+
+            doubler.init(&recov.out);
+            reshape.init(&doubler.outB, 4096, (1187 / 30) - 4096);
+            diagHandler.init(&reshape.out, _diagHandler, this);
+            diag.lines.push_back(-0.8);
+            diag.lines.push_back(0.8);
         }
 
         void start() {
+            agc.start();
+            costas.start();
+            fir.start();
+            costas2.start();
+            c2r.start();
             demod.start();
             recov.start();
             slice.start();
-            manch.start();
             diff.start();
             hs.start();
+
+            doubler.start();
+            reshape.start();
+            diagHandler.start();
         }
 
         void stop() {
+            agc.stop();
+            costas.stop();
+            fir.stop();
+            costas2.stop();
+            c2r.stop();
             demod.stop();
             recov.stop();
             slice.stop();
-            manch.stop();
             diff.stop();
             hs.stop();
+
+            c2r.stop();
+            reshape.stop();
+            diagHandler.stop();
         }
 
         void showMenu() {
@@ -94,14 +129,105 @@ namespace demod {
                 _config->release(true);
             }
 
-            // if (_rds) {
+            // TODO: This will break when the entire radio module is disabled
-            //     if (rdsDecode.countryCodeValid()) { ImGui::Text("Country code: %d", rdsDecode.getCountryCode()); }
+            if (!_rds) { ImGui::BeginDisabled(); }
-            //     if (rdsDecode.programCoverageValid()) { ImGui::Text("Program coverage: %d", rdsDecode.getProgramCoverage()); }
+            if (ImGui::Checkbox(("Advanced RDS Info##_radio_wfm_rds_info_" + name).c_str(), &_rdsInfo)) {
-            //     if (rdsDecode.programRefNumberValid()) { ImGui::Text("Reference number: %d", rdsDecode.getProgramRefNumber()); }
+                _config->acquire();
-            //     if (rdsDecode.programTypeValid()) { ImGui::Text("Program type: %d", rdsDecode.getProgramType()); }
+                _config->conf[name][getName()]["rdsInfo"] = _rdsInfo;
-            //     if (rdsDecode.PSNameValid()) { ImGui::Text("Program name: [%s]", rdsDecode.getPSName().c_str()); }
+                _config->release(true);
-            //     if (rdsDecode.radioTextValid()) { ImGui::Text("Radiotext: [%s]", rdsDecode.getRadioText().c_str()); }
+            }
-            // }
+            if (!_rds) { ImGui::EndDisabled(); }
+
+            float menuWidth = ImGui::GetContentRegionAvail().x;
+
+            if (_rds && _rdsInfo) {
+                ImGui::BeginTable(("##radio_wfm_rds_info_tbl_" + name).c_str(), 2, ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_RowBg | ImGuiTableFlags_Borders);
+                if (rdsDecode.piCodeValid()) {
+                    ImGui::TableNextRow();
+                    ImGui::TableSetColumnIndex(0);
+                    ImGui::TextUnformatted("PI Code");
+                    ImGui::TableSetColumnIndex(1);
+                    ImGui::Text("0x%04X (%s)", rdsDecode.getPICode(), rdsDecode.getCallsign().c_str());
+                    
+                    ImGui::TableNextRow();
+                    ImGui::TableSetColumnIndex(0);
+                    ImGui::TextUnformatted("Country Code");
+                    ImGui::TableSetColumnIndex(1);
+                    ImGui::Text("%d", rdsDecode.getCountryCode());
+
+                    ImGui::TableNextRow();
+                    ImGui::TableSetColumnIndex(0);
+                    ImGui::TextUnformatted("Program Coverage");
+                    ImGui::TableSetColumnIndex(1);
+                    ImGui::Text("%s (%d)", rds::AREA_COVERAGE_TO_STR[rdsDecode.getProgramCoverage()], rdsDecode.getProgramCoverage());
+
+                    ImGui::TableNextRow();
+                    ImGui::TableSetColumnIndex(0);
+                    ImGui::TextUnformatted("Reference Number");
+                    ImGui::TableSetColumnIndex(1);
+                    ImGui::Text("%d", rdsDecode.getProgramRefNumber());
+                }
+                else {
+                    ImGui::TableNextRow();
+                    ImGui::TableSetColumnIndex(0);
+                    ImGui::TextUnformatted("PI Code");
+                    ImGui::TableSetColumnIndex(1);
+                    ImGui::TextUnformatted("0x---- (----)");
+                    
+                    ImGui::TableNextRow();
+                    ImGui::TableSetColumnIndex(0);
+                    ImGui::TextUnformatted("Country Code");
+                    ImGui::TableSetColumnIndex(1);
+                    ImGui::TextUnformatted("--");  // TODO: String
+
+                    ImGui::TableNextRow();
+                    ImGui::TableSetColumnIndex(0);
+                    ImGui::TextUnformatted("Program Coverage");
+                    ImGui::TableSetColumnIndex(1);
+                    ImGui::TextUnformatted("------- (--)");
+
+                    ImGui::TableNextRow();
+                    ImGui::TableSetColumnIndex(0);
+                    ImGui::TextUnformatted("Reference Number");
+                    ImGui::TableSetColumnIndex(1);
+                    ImGui::TextUnformatted("--");
+                }
+
+                if (rdsDecode.programTypeValid()) {
+                    ImGui::TableNextRow();
+                    ImGui::TableSetColumnIndex(0);
+                    ImGui::TextUnformatted("Program Type");
+                    ImGui::TableSetColumnIndex(1);
+                    ImGui::Text("%s (%d)", rds::PROGRAM_TYPE_US_TO_STR[rdsDecode.getProgramType()], rdsDecode.getProgramType());
+                }
+                else {
+                    ImGui::TableNextRow();
+                    ImGui::TableSetColumnIndex(0);
+                    ImGui::TextUnformatted("Program Type");
+                    ImGui::TableSetColumnIndex(1);
+                    ImGui::TextUnformatted("------- (--)");  // TODO: String
+                }
+
+                if (rdsDecode.musicValid()) {
+                    ImGui::TableNextRow();
+                    ImGui::TableSetColumnIndex(0);
+                    ImGui::TextUnformatted("Music");
+                    ImGui::TableSetColumnIndex(1);
+                    ImGui::Text("%s", rdsDecode.getMusic() ? "Yes":"No");
+                }
+                else {
+                    ImGui::TableNextRow();
+                    ImGui::TableSetColumnIndex(0);
+                    ImGui::TextUnformatted("Music");
+                    ImGui::TableSetColumnIndex(1);
+                    ImGui::TextUnformatted("---");
+                }
+
+                ImGui::EndTable();
+
+                ImGui::SetNextItemWidth(menuWidth);
+                diag.draw();
+            }
         }
 
         void setBandwidth(double bandwidth) {
@@ -145,6 +271,14 @@ namespace demod {
             _this->rdsDecode.process(data, count);
         }
 
+        // DEBUGGING ONLY
+        static void _diagHandler(float* data, int count, void* ctx) {
+            WFM* _this = (WFM*)ctx;
+            float* buf = _this->diag.acquireBuffer();
+            memcpy(buf, data, count * sizeof(float));
+            _this->diag.releaseBuffer();
+        }
+
         static void fftRedraw(ImGui::WaterFall::FFTRedrawArgs args, void* ctx) {
             WFM* _this = (WFM*)ctx;
             if (!_this->_rds) { return; }
@@ -186,13 +320,23 @@ namespace demod {
         }
 
         dsp::demod::BroadcastFM demod;
-        dsp::clock_recovery::FD recov;
+        dsp::loop::FastAGC<dsp::complex_t> agc;
+        dsp::loop::Costas<2> costas;
+        dsp::tap<dsp::complex_t> taps;
+        dsp::filter::FIR<dsp::complex_t, dsp::complex_t> fir;
+        dsp::loop::Costas<2> costas2;
+        dsp::convert::ComplexToReal c2r;
+        dsp::clock_recovery::MM<float> recov;
         dsp::digital::BinarySlicer slice;
-        dsp::digital::ManchesterDecoder manch;
         dsp::digital::DifferentialDecoder diff;
         dsp::sink::Handler<uint8_t> hs;
         EventHandler<ImGui::WaterFall::FFTRedrawArgs> fftRedrawHandler;
 
+        dsp::routing::Doubler<float> doubler;
+        dsp::buffer::Reshaper<float> reshape;
+        dsp::sink::Handler<float> diagHandler;
+        ImGui::SymbolDiagram diag;
+
         rds::RDSDecoder rdsDecode;
 
         ConfigManager* _config = NULL;
@@ -200,6 +344,7 @@ namespace demod {
         bool _stereo = false;
         bool _lowPass = true;
         bool _rds = false;
+        bool _rdsInfo = false;
         float muGain = 0.01;
         float omegaGain = (0.01*0.01)/4.0;
 

decoder_modules/radio/src/rds.cpp

@@ -3,6 +3,8 @@
 #include <map>
 #include <algorithm>
 
+#include <utils/flog.h>
+
 namespace rds {
     std::map<uint16_t, BlockType> SYNDROMES = {
         { 0b1111011000, BLOCK_TYPE_A  },
@@ -54,18 +56,26 @@ namespace rds {
                 type = (BlockType)((lastType + 1) % _BLOCK_TYPE_COUNT);
             }
 
-            // Save block while correcting errors (NOT YET)
+            // Save block while correcting errors (NOT YET) <- idk why the "not yet is here", TODO: find why
             blocks[type] = correctErrors(shiftReg, type, blockAvail[type]);
 
-            // Update continous group count
+            // If block type is A, decode it directly, otherwise, update continous count
-            if (type == BLOCK_TYPE_A) { contGroup = 1; }
+            if (type == BLOCK_TYPE_A) {
-            else if (type == BLOCK_TYPE_B && lastType == BLOCK_TYPE_A) { contGroup++; }
+                decodeBlockA();
+            }
+            else if (type == BLOCK_TYPE_B) { contGroup = 1; }
             else if ((type == BLOCK_TYPE_C || type == BLOCK_TYPE_CP) && lastType == BLOCK_TYPE_B) { contGroup++; }
             else if (type == BLOCK_TYPE_D && (lastType == BLOCK_TYPE_C || lastType == BLOCK_TYPE_CP)) { contGroup++; }
-            else { contGroup = 0; }
+            else {
+                // If block B is available, decode it alone.
+                if (contGroup == 1) {
+                    decodeBlockB();
+                }
+                contGroup = 0;
+            }
 
             // If we've got an entire group, process it
-            if (contGroup >= 4) {
+            if (contGroup >= 3) {
                 contGroup = 0;
                 decodeGroup();
             }
@@ -124,26 +134,45 @@ namespace rds {
         return out;
     }
 
-    void RDSDecoder::decodeGroup() {
+    void RDSDecoder::decodeBlockA() {
+        // If it didn't decode properly return
+        if (!blockAvail[BLOCK_TYPE_A]) { return; }
+
+        // Update timeout
         std::lock_guard<std::mutex> lck(groupMtx);
         auto now = std::chrono::high_resolution_clock::now();
-        anyGroupLastUpdate = now;
+        blockALastUpdate = now;
-
-        // Make sure blocks A and B are available
-        if (!blockAvail[BLOCK_TYPE_A] || !blockAvail[BLOCK_TYPE_B]) { return; }
 
         // Decode PI code
+        piCode = (blocks[BLOCK_TYPE_A] >> 10) & 0xFFFF;
         countryCode = (blocks[BLOCK_TYPE_A] >> 22) & 0xF;
         programCoverage = (AreaCoverage)((blocks[BLOCK_TYPE_A] >> 18) & 0xF);
         programRefNumber = (blocks[BLOCK_TYPE_A] >> 10) & 0xFF;
+        decodeCallsign();
+    }
+
+    void RDSDecoder::decodeBlockB() {
+        // If it didn't decode properly return
+        if (!blockAvail[BLOCK_TYPE_B]) { return; }
 
         // Decode group type and version
-        uint8_t groupType = (blocks[BLOCK_TYPE_B] >> 22) & 0xF;
+        groupType = (blocks[BLOCK_TYPE_B] >> 22) & 0xF;
-        GroupVersion groupVer = (GroupVersion)((blocks[BLOCK_TYPE_B] >> 21) & 1);
+        groupVer = (GroupVersion)((blocks[BLOCK_TYPE_B] >> 21) & 1);
 
         // Decode traffic program and program type
         trafficProgram = (blocks[BLOCK_TYPE_B] >> 20) & 1;
         programType = (ProgramType)((blocks[BLOCK_TYPE_B] >> 15) & 0x1F);
+    }
+
+    void RDSDecoder::decodeGroup() {
+        std::lock_guard<std::mutex> lck(groupMtx);
+        auto now = std::chrono::high_resolution_clock::now();
+
+        // Make sure blocks B is available
+        if (!blockAvail[BLOCK_TYPE_B]) { return; }
+
+        // Decode block B
+        decodeBlockB();
         
         if (groupType == 0) {
             group0LastUpdate = now;
@@ -202,9 +231,33 @@ namespace rds {
         }
     }
 
-    bool RDSDecoder::anyGroupValid() {
+    void RDSDecoder::decodeCallsign() {
+        // Determin first better based on offset
+        bool w = (piCode >= 21672);
+        callsign =  w ? 'W' : 'K';
+
+        // Base25 decode the rest
+        std::string restStr;
+        int rest = piCode - (w ? 21672 : 4096);
+        while (rest) {
+            restStr += 'A' + (rest % 26);
+            rest /= 26;
+        }
+
+        // Reorder chars
+        for (int i = restStr.size() - 1; i >= 0; i--) {
+            callsign += restStr[i];
+        }
+    }
+
+    bool RDSDecoder::blockAValid() {
         auto now = std::chrono::high_resolution_clock::now();
-        return (std::chrono::duration_cast<std::chrono::milliseconds>(now - anyGroupLastUpdate)).count() < 5000.0;
+        return (std::chrono::duration_cast<std::chrono::milliseconds>(now - blockALastUpdate)).count() < 5000.0;
+    }
+
+    bool RDSDecoder::blockBValid() {
+        auto now = std::chrono::high_resolution_clock::now();
+        return (std::chrono::duration_cast<std::chrono::milliseconds>(now - blockALastUpdate)).count() < 5000.0;
     }
 
     bool RDSDecoder::group0Valid() {

decoder_modules/radio/src/rds.h

@@ -20,22 +20,42 @@ namespace rds {
     };
 
     enum AreaCoverage {
-        AREA_COVERAGE_LOCAL,
+        AREA_COVERAGE_INVALID           = -1,
-        AREA_COVERAGE_INTERNATIONAL,
+        AREA_COVERAGE_LOCAL             = 0,
-        AREA_COVERAGE_NATIONAL,
+        AREA_COVERAGE_INTERNATIONAL     = 1,
-        AREA_COVERAGE_SUPRA_NATIONAL,
+        AREA_COVERAGE_NATIONAL          = 2,
-        AREA_COVERAGE_REGIONAL1,
+        AREA_COVERAGE_SUPRA_NATIONAL    = 3,
-        AREA_COVERAGE_REGIONAL2,
+        AREA_COVERAGE_REGIONAL1         = 4,
-        AREA_COVERAGE_REGIONAL3,
+        AREA_COVERAGE_REGIONAL2         = 5,
-        AREA_COVERAGE_REGIONAL4,
+        AREA_COVERAGE_REGIONAL3         = 6,
-        AREA_COVERAGE_REGIONAL5,
+        AREA_COVERAGE_REGIONAL4         = 7,
-        AREA_COVERAGE_REGIONAL6,
+        AREA_COVERAGE_REGIONAL5         = 8,
-        AREA_COVERAGE_REGIONAL7,
+        AREA_COVERAGE_REGIONAL6         = 9,
-        AREA_COVERAGE_REGIONAL8,
+        AREA_COVERAGE_REGIONAL7         = 10,
-        AREA_COVERAGE_REGIONAL9,
+        AREA_COVERAGE_REGIONAL8         = 11,
-        AREA_COVERAGE_REGIONAL10,
+        AREA_COVERAGE_REGIONAL9         = 12,
-        AREA_COVERAGE_REGIONAL11,
+        AREA_COVERAGE_REGIONAL10        = 13,
-        AREA_COVERAGE_REGIONAL12
+        AREA_COVERAGE_REGIONAL11        = 14,
+        AREA_COVERAGE_REGIONAL12        = 15  
+    };
+
+    inline const char* AREA_COVERAGE_TO_STR[] = {
+        "Local",
+        "International",
+        "National",
+        "Supra-National",
+        "Regional 1",
+        "Regional 2",
+        "Regional 3",
+        "Regional 4",
+        "Regional 5",
+        "Regional 6",
+        "Regional 7",
+        "Regional 8",
+        "Regional 9",
+        "Regional 10",
+        "Regional 11",
+        "Regional 12",
     };
 
     enum ProgramType {
@@ -108,6 +128,76 @@ namespace rds {
         PROGRAM_TYPE_EU_ALARM = 31
     };
 
+    inline const char* PROGRAM_TYPE_EU_TO_STR[] = {
+        "None",
+        "News",
+        "Current Affairs",
+        "Information",
+        "Sports",
+        "Education",
+        "Drama",
+        "Culture",
+        "Science",
+        "Varied",
+        "Pop Music",
+        "Rock Music",
+        "Easy Listening Music",
+        "Light Classical",
+        "Serious Classical",
+        "Other Music",
+        "Weather",
+        "Finance",
+        "Children Program",
+        "Social Affairs",
+        "Religion",
+        "Phone-in",
+        "Travel",
+        "Leisure",
+        "Jazz Music",
+        "Country Music",
+        "National Music",
+        "Oldies Music",
+        "Folk Music",
+        "Documentary",
+        "Alarm Test",
+        "Alarm",
+    };
+
+    inline const char* PROGRAM_TYPE_US_TO_STR[] = {
+        "None",
+        "News",
+        "Information",
+        "Sports",
+        "Talk",
+        "Rock",
+        "Classic Rock",
+        "Adult Hits",
+        "Soft Rock",
+        "Top 40",
+        "Country",
+        "Oldies",
+        "Soft",
+        "Nostalgia",
+        "Jazz",
+        "Classical",
+        "Rythm and Blues",
+        "Soft Rythm and Blues",
+        "Foreign Language",
+        "Religious Music",
+        "Religious Talk",
+        "Personality",
+        "Public",
+        "College",
+        "Unassigned",
+        "Unassigned",
+        "Unassigned",
+        "Unassigned",
+        "Unassigned",
+        "Weather",
+        "Emergency Test",
+        "Emergency",
+    };
+
     enum DecoderIdentification {
         DECODER_IDENT_STEREO = (1 << 0),
         DECODER_IDENT_ARTIFICIAL_HEAD = (1 << 1),
@@ -119,13 +209,14 @@ namespace rds {
     public:
         void process(uint8_t* symbols, int count);
 
-        bool countryCodeValid() { std::lock_guard<std::mutex> lck(groupMtx); return anyGroupValid(); }
+        bool piCodeValid() { std::lock_guard<std::mutex> lck(groupMtx); return blockAValid(); }
+        uint16_t getPICode() { std::lock_guard<std::mutex> lck(groupMtx); return piCode; }
         uint8_t getCountryCode() { std::lock_guard<std::mutex> lck(groupMtx); return countryCode; }
-        bool programCoverageValid() { std::lock_guard<std::mutex> lck(groupMtx); return anyGroupValid(); }
         uint8_t getProgramCoverage() { std::lock_guard<std::mutex> lck(groupMtx); return programCoverage; }
-        bool programRefNumberValid() { std::lock_guard<std::mutex> lck(groupMtx); return anyGroupValid(); }
         uint8_t getProgramRefNumber() { std::lock_guard<std::mutex> lck(groupMtx); return programRefNumber; }
-        bool programTypeValid() { std::lock_guard<std::mutex> lck(groupMtx); return anyGroupValid(); }
+        std::string getCallsign() { std::lock_guard<std::mutex> lck(groupMtx); return callsign; }
+        
+        bool programTypeValid() { std::lock_guard<std::mutex> lck(groupMtx); return blockBValid(); }
         ProgramType getProgramType() { std::lock_guard<std::mutex> lck(groupMtx); return programType; }
 
         bool musicValid() { std::lock_guard<std::mutex> lck(groupMtx); return group0Valid(); }
@@ -139,9 +230,14 @@ namespace rds {
     private:
         static uint16_t calcSyndrome(uint32_t block);
         static uint32_t correctErrors(uint32_t block, BlockType type, bool& recovered);
+        void decodeBlockA();
+        void decodeBlockB();
         void decodeGroup();
 
-        bool anyGroupValid();
+        void decodeCallsign();
+
+        bool blockAValid();
+        bool blockBValid();
         bool group0Valid();
         bool group2Valid();
 
@@ -154,17 +250,24 @@ namespace rds {
         uint32_t blocks[_BLOCK_TYPE_COUNT];
         bool blockAvail[_BLOCK_TYPE_COUNT];
 
-        // All groups
+        // Block A (All groups)
         std::mutex groupMtx;
-        std::chrono::time_point<std::chrono::high_resolution_clock> anyGroupLastUpdate;
+        std::chrono::time_point<std::chrono::high_resolution_clock> blockALastUpdate{};  // 1970-01-01
+        uint16_t piCode;
         uint8_t countryCode;
         AreaCoverage programCoverage;
         uint8_t programRefNumber;
+        std::string callsign;
+
+        // Block B (All groups)
+        std::chrono::time_point<std::chrono::high_resolution_clock> blockBLastUpdate{};  // 1970-01-01
+        uint8_t groupType;
+        GroupVersion groupVer;
         bool trafficProgram;
         ProgramType programType;
 
         // Group type 0
-        std::chrono::time_point<std::chrono::high_resolution_clock> group0LastUpdate;
+        std::chrono::time_point<std::chrono::high_resolution_clock> group0LastUpdate{};  // 1970-01-01
         bool trafficAnnouncement;
         bool music;
         uint8_t decoderIdent;
@@ -172,7 +275,7 @@ namespace rds {
         std::string programServiceName = "        ";
 
         // Group type 2
-        std::chrono::time_point<std::chrono::high_resolution_clock> group2LastUpdate;
+        std::chrono::time_point<std::chrono::high_resolution_clock> group2LastUpdate{};  // 1970-01-01
         bool rtAB = false;
         std::string radioText = "                                                                ";
 

source_modules/bladerf_source/src/main.cpp

@@ -347,7 +347,7 @@ private:
     static void start(void* ctx) {
         BladeRFSourceModule* _this = (BladeRFSourceModule*)ctx;
         if (_this->running) { return; }
-        if (_this->devCount == 0) { return; }
+        if (_this->devCount <= 0) { return; }
 
         // Open device
         bladerf_devinfo info = _this->devInfoList[_this->devId];