more work on DAB decoding

decoder_modules/dab_decoder/src/dab_dsp.h

@@ -5,139 +5,139 @@
 #include "dab_phase_sym.h"
 
 namespace dab {
-    class CyclicSync : public dsp::Processor<dsp::complex_t, dsp::complex_t> {
+    // class CyclicSync : public dsp::Processor<dsp::complex_t, dsp::complex_t> {
-        using base_type = dsp::Processor<dsp::complex_t, dsp::complex_t>;
+    //     using base_type = dsp::Processor<dsp::complex_t, dsp::complex_t>;
-    public:
+    // public:
-        CyclicSync() {}
+    //     CyclicSync() {}
 
-        // TODO: The default AGC rate is probably way too fast, plot out the avgCorr to see how much it moves
+    //     // TODO: The default AGC rate is probably way too fast, plot out the avgCorr to see how much it moves
-        CyclicSync(dsp::stream<dsp::complex_t>* in, double symbolLength, double cyclicPrefixLength, double samplerate, float agcRate = 1e-3) { init(in, symbolLength, cyclicPrefixLength, samplerate, agcRate); }
+    //     CyclicSync(dsp::stream<dsp::complex_t>* in, double symbolLength, double cyclicPrefixLength, double samplerate, float agcRate = 1e-3) { init(in, symbolLength, cyclicPrefixLength, samplerate, agcRate); }
 
-        void init(dsp::stream<dsp::complex_t>* in, double symbolLength, double cyclicPrefixLength, double samplerate, float agcRate = 1e-3) {
+    //     void init(dsp::stream<dsp::complex_t>* in, double symbolLength, double cyclicPrefixLength, double samplerate, float agcRate = 1e-3) {
-            // Computer the number of samples for the symbol and its cyclic prefix
+    //         // Computer the number of samples for the symbol and its cyclic prefix
-            symbolSamps = round(samplerate * symbolLength);
+    //         symbolSamps = round(samplerate * symbolLength);
-            prefixSamps = round(samplerate * cyclicPrefixLength);
+    //         prefixSamps = round(samplerate * cyclicPrefixLength);
 
-            // Allocate and clear the delay buffer
+    //         // Allocate and clear the delay buffer
-            delayBuf = dsp::buffer::alloc<dsp::complex_t>(STREAM_BUFFER_SIZE + 64000);
+    //         delayBuf = dsp::buffer::alloc<dsp::complex_t>(STREAM_BUFFER_SIZE + 64000);
-            dsp::buffer::clear(delayBuf, symbolSamps);
+    //         dsp::buffer::clear(delayBuf, symbolSamps);
 
-            // Allocate and clear the history buffer
+    //         // Allocate and clear the history buffer
-            histBuf = dsp::buffer::alloc<dsp::complex_t>(prefixSamps);
+    //         histBuf = dsp::buffer::alloc<dsp::complex_t>(prefixSamps);
-            dsp::buffer::clear(histBuf, prefixSamps);
+    //         dsp::buffer::clear(histBuf, prefixSamps);
 
-            // Compute the delay input addresses
+    //         // Compute the delay input addresses
-            delayBufInput = &delayBuf[symbolSamps];
+    //         delayBufInput = &delayBuf[symbolSamps];
 
-            // Compute the correlation AGC configuration
+    //         // Compute the correlation AGC configuration
-            this->agcRate = agcRate;
+    //         this->agcRate = agcRate;
-            agcRateInv = 1.0f - agcRate;
+    //         agcRateInv = 1.0f - agcRate;
             
-            base_type::init(in);
+    //         base_type::init(in);
-        }
+    //     }
 
-        void reset() {
+    //     void reset() {
-            assert(base_type::_block_init);
+    //         assert(base_type::_block_init);
-            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+    //         std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
-            base_type::tempStop();
+    //         base_type::tempStop();
             
-            base_type::tempStart();
+    //         base_type::tempStart();
-        }
+    //     }
 
-        int run() {
+    //     int run() {
-            int count = base_type::_in->read();
+    //         int count = base_type::_in->read();
-            if (count < 0) { return -1; }
+    //         if (count < 0) { return -1; }
 
-            // Copy the data into the normal delay buffer
+    //         // Copy the data into the normal delay buffer
-            memcpy(delayBufInput, base_type::_in->readBuf, count * sizeof(dsp::complex_t));
+    //         memcpy(delayBufInput, base_type::_in->readBuf, count * sizeof(dsp::complex_t));
 
-            // Flush the input stream
+    //         // Flush the input stream
-            base_type::_in->flush();
+    //         base_type::_in->flush();
 
-            // Do cross-correlation
+    //         // Do cross-correlation
-            for (int i = 0; i < count; i++) {
+    //         for (int i = 0; i < count; i++) {
-                // Get the current history slot
+    //             // Get the current history slot
-                dsp::complex_t* slot = &histBuf[histId++];
+    //             dsp::complex_t* slot = &histBuf[histId++];
 
-                // Wrap around the history slot index (TODO: Check that the history buffer's length is correct)
+    //             // Wrap around the history slot index (TODO: Check that the history buffer's length is correct)
-                histId %= prefixSamps;
+    //             histId %= prefixSamps;
 
-                // Kick out last value from the correlation
+    //             // Kick out last value from the correlation
-                corr -= *slot;
+    //             corr -= *slot;
 
-                // Save input value and compute the new prodct
+    //             // Save input value and compute the new prodct
-                dsp::complex_t val = delayBuf[i];
+    //             dsp::complex_t val = delayBuf[i];
-                dsp::complex_t prod = val.conj()*delayBuf[i+symbolSamps];
+    //             dsp::complex_t prod = val.conj()*delayBuf[i+symbolSamps];
 
-                // Add the new value to the correlation
+    //             // Add the new value to the correlation
-                *slot = prod;
+    //             *slot = prod;
 
-                // Add the new value to the history buffer
+    //             // Add the new value to the history buffer
-                corr += prod;
+    //             corr += prod;
 
-                // Compute sample amplitude
+    //             // Compute sample amplitude
-                float rcorr = corr.amplitude();
+    //             float rcorr = corr.amplitude();
 
-                // If a high enough peak is reached, reset the symbol counter
+    //             // If a high enough peak is reached, reset the symbol counter
-                if (rcorr > avgCorr && rcorr > peakCorr) { // Note keeping an average level might not be needed
+    //             if (rcorr > avgCorr && rcorr > peakCorr) { // Note keeping an average level might not be needed
-                    peakCorr = rcorr;
+    //                 peakCorr = rcorr;
-                    peakLCorr = lastCorr;
+    //                 peakLCorr = lastCorr;
-                    samplesSincePeak = 0;
+    //                 samplesSincePeak = 0;
-                }
+    //             }
 
-                // If this is the sample right after the peak, save it
+    //             // If this is the sample right after the peak, save it
-                if (samplesSincePeak == 1) {
+    //             if (samplesSincePeak == 1) {
-                    peakRCorr = rcorr;
+    //                 peakRCorr = rcorr;
-                }
+    //             }
 
-                // Write the sample to the output
+    //             // Write the sample to the output
-                out.writeBuf[samplesSincePeak++] = val;
+    //             out.writeBuf[samplesSincePeak++] = val;
 
-                // If the end of the symbol is reached, send it off
+    //             // If the end of the symbol is reached, send it off
-                if (samplesSincePeak >= symbolSamps) {
+    //             if (samplesSincePeak >= symbolSamps) {
-                    if (!out.swap(symbolSamps)) {
+    //                 if (!out.swap(symbolSamps)) {
-                        return -1;
+    //                     return -1;
-                    }
+    //                 }
-                    samplesSincePeak = 0;
+    //                 samplesSincePeak = 0;
-                    peakCorr = 0;
+    //                 peakCorr = 0;
-                }
+    //             }
 
-                // Update the average correlation
+    //             // Update the average correlation
-                lastCorr = rcorr;
+    //             lastCorr = rcorr;
 
-                // Update the average correlation value
+    //             // Update the average correlation value
-                avgCorr = agcRate*rcorr + agcRateInv*avgCorr;
+    //             avgCorr = agcRate*rcorr + agcRateInv*avgCorr;
-            }
+    //         }
 
-            // Move unused data
+    //         // Move unused data
-            memmove(delayBuf, &delayBuf[count], symbolSamps * sizeof(dsp::complex_t));
+    //         memmove(delayBuf, &delayBuf[count], symbolSamps * sizeof(dsp::complex_t));
 
-            return count;
+    //         return count;
-        }
+    //     }
 
-    protected:
+    // protected:
-        int symbolSamps;
+    //     int symbolSamps;
-        int prefixSamps;
+    //     int prefixSamps;
 
-        int histId = 0;
+    //     int histId = 0;
-        dsp::complex_t* histBuf;
+    //     dsp::complex_t* histBuf;
 
-        dsp::complex_t* delayBuf;
+    //     dsp::complex_t* delayBuf;
-        dsp::complex_t* delayBufInput;
+    //     dsp::complex_t* delayBufInput;
 
-        dsp::complex_t corr = { 0.0f, 0.0f };
+    //     dsp::complex_t corr = { 0.0f, 0.0f };
 
-        int samplesSincePeak = 0;
+    //     int samplesSincePeak = 0;
-        float lastCorr = 0.0f;
+    //     float lastCorr = 0.0f;
-        float peakCorr = 0.0f;
+    //     float peakCorr = 0.0f;
-        float peakLCorr = 0.0f;
+    //     float peakLCorr = 0.0f;
-        float peakRCorr = 0.0f;
+    //     float peakRCorr = 0.0f;
 
-        // Note only required for DAB
+    //     // Note only required for DAB
-        float avgCorr = 0.0f;
+    //     float avgCorr = 0.0f;
-        float agcRate;
+    //     float agcRate;
-        float agcRateInv;
+    //     float agcRateInv;
-    };
+    // };
 
     class FrameFreqSync : public dsp::Processor<dsp::complex_t, dsp::complex_t> {
         using base_type = dsp::Processor<dsp::complex_t, dsp::complex_t>;

decoder_modules/dab_decoder/src/main.cpp

@@ -14,6 +14,7 @@
 #include <chrono>
 #include "dab_dsp.h"
 #include <gui/widgets/constellation_diagram.h>
+#include "ofdm.h"
 
 #define CONCAT(a, b) ((std::string(a) + b).c_str())
 
@@ -35,7 +36,7 @@ public:
     M17DecoderModule(std::string name)  {
         this->name = name;
 
-        file = std::ofstream("sync4.f32", std::ios::out | std::ios::binary);
+        file = std::ofstream("sync5.f32", std::ios::out | std::ios::binary);
 
         // Load config
         config.acquire();
@@ -47,7 +48,7 @@ public:
         vfo->setSnapInterval(250);
 
         // Initialize DSP here
-        csync.init(vfo->output, 1e-3, 246e-6, INPUT_SAMPLE_RATE);
+        csync.init(vfo->output, 2048, 504, 1e-3, INPUT_SAMPLE_RATE, 1e-6, 0.01, 0.005);
         ffsync.init(&csync.out);
         ns.init(&ffsync.out, handler, this);
 
@@ -131,8 +132,9 @@ private:
     std::string name;
     bool enabled = true;
 
-    dab::CyclicSync csync;
+    //dab::CyclicSync csync;
     dab::FrameFreqSync ffsync;
+    dsp::ofdm::CyclicTimeSync csync;
     dsp::sink::Handler<dsp::complex_t> ns;
 
     ImGui::ConstellationDiagram constDiagram;

decoder_modules/dab_decoder/src/ofdm.h

@@ -11,67 +11,93 @@ namespace dsp::ofdm {
     public:
         CyclicTimeSync() {}
 
-        CyclicTimeSync(stream<complex_t>* in, int fftSize, double usefulSymbolTime, double cyclicPrefixRatio, double samplerate,
+        CyclicTimeSync(stream<complex_t>* in, int fftSize, int cpSize, double usefulSymbolTime, double samplerate,
                         double omegaGain, double muGain, double omegaRelLimit, int interpPhaseCount = 128, int interpTapCount = 8) {
-            init(in, fftSize, usefulSymbolTime, cyclicPrefixRatio, samplerate, omegaGain, muGain, omegaRelLimit, interpPhaseCount, interpTapCount);
+            init(in, fftSize, cpSize, usefulSymbolTime, samplerate, omegaGain, muGain, omegaRelLimit, interpPhaseCount, interpTapCount);
         }
 
         ~CyclicTimeSync() {
             if (!base_type::_block_init) { return; }
             base_type::stop();
             dsp::multirate::freePolyphaseBank(interpBank);
+            buffer::free(corrSampCache);
+            buffer::free(corrProdCache);
             buffer::free(buffer);
         }
 
-        void init(stream<complex_t>* in, int fftSize, double usefulSymbolTime, double cyclicPrefixRatio, double samplerate,
+        void init(stream<complex_t>* in, int fftSize, int cpSize, double usefulSymbolTime, double samplerate,
                     double omegaGain, double muGain, double omegaRelLimit, int interpPhaseCount = 128, int interpTapCount = 8) {
-            omega = 0; // TODO
+            // Save parameters
-            _omegaGain = omegaGain;
+            this->fftSize = fftSize;
-            _muGain = muGain;
+            this->cpSize = cpSize;
-            _omegaRelLimit = omegaRelLimit;
+            period = fftSize + cpSize;
-            _interpPhaseCount = interpPhaseCount;
+            
-            _interpTapCount = interpTapCount;
+            // Compute the interpolator settings
+            omega = (usefulSymbolTime * samplerate) / (double)fftSize;
+            this->omegaGain = omegaGain;
+            this->muGain = muGain;
+            this->omegaRelLimit = omegaRelLimit;
+            this->interpPhaseCount = interpPhaseCount;
+            this->interpTapCount = interpTapCount;
 
-            pcl.init(_muGain, _omegaGain, 0.0, 0.0, 1.0, omega, omega * (1.0 - omegaRelLimit), omega * (1.0 + omegaRelLimit));
+            // Compute the correlator AGC settings
+            // TODO: Compute it using he FFT and CP sizes
+            this->corrAgcRate = 1e-4;
+            corrAgcInvRate = 1.0f - corrAgcRate;
+
+            // Initialize the control loop
+            pcl.init(muGain, omegaGain, 0.0, 0.0, 1.0, omega, omega * (1.0 - omegaRelLimit), omega * (1.0 + omegaRelLimit));
+
+            // Generate the interpolation taps
             generateInterpTaps();
-            buffer = buffer::alloc<complex_t>(STREAM_BUFFER_SIZE + _interpTapCount);
+
-            bufStart = &buffer[_interpTapCount - 1];
+            // Allocate the buffers
+            corrSampCache = buffer::alloc<complex_t>(fftSize);
+            corrProdCache = buffer::alloc<complex_t>(cpSize);
+            buffer = buffer::alloc<complex_t>(STREAM_BUFFER_SIZE + interpTapCount);
+            bufStart = &buffer[interpTapCount - 1];
+
+            // Clear the buffers
+            buffer::clear(corrSampCache, fftSize);
+            buffer::clear(corrProdCache, cpSize);
+            buffer::clear(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;
+            this->omegaGain = omegaGain;
-            pcl.setCoefficients(_muGain, _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;
+            this->muGain = muGain;
-            pcl.setCoefficients(_muGain, _omegaGain);
+            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;
+            this->omegaRelLimit = omegaRelLimit;
-            pcl.setFreqLimits(omega * (1.0 - _omegaRelLimit), omega * (1.0 + _omegaRelLimit));
+            pcl.setFreqLimits(omega * (1.0 - omegaRelLimit), omega * (1.0 + omegaRelLimit));
         }
 
         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;
+            this->interpPhaseCount = interpPhaseCount;
-            _interpTapCount = interpTapCount;
+            this->interpTapCount = interpTapCount;
             dsp::multirate::freePolyphaseBank(interpBank);
             buffer::free(buffer);
             generateInterpTaps();
-            buffer = buffer::alloc<complex_t>(STREAM_BUFFER_SIZE + _interpTapCount);
+            buffer = buffer::alloc<complex_t>(STREAM_BUFFER_SIZE + interpTapCount);
-            bufStart = &buffer[_interpTapCount - 1];
+            bufStart = &buffer[interpTapCount - 1];
             base_type::tempStart();
         }
 
@@ -82,59 +108,139 @@ namespace dsp::ofdm {
             offset = 0;
             pcl.phase = 0.0f;
             pcl.freq = omega;
+            // TODO: The rest
             base_type::tempStart();
         }
 
-        inline int process(int count, const complex_t* in, complex_t* out) {
+        int run() {
+            int count = base_type::_in->read();
+            if (count < 0) { return -1; }
+
             // Copy data to work buffer
-            memcpy(bufStart, in, count * sizeof(complex_t));
+            memcpy(bufStart, base_type::_in->readBuf, count * sizeof(complex_t));
 
             // Process all samples
-            int outCount = 0;
             while (offset < count) {
+                // Get the cache slots
+                complex_t* sampSlot = &corrSampCache[corrSampCacheId++];
+                complex_t* prodSlot = &corrProdCache[corrProdCacheId++];
+                corrSampCacheId %= fftSize;
+                corrProdCacheId %= cpSize;
+
                 // Compute the interpolated sample
                 complex_t sample;
-                int phase = std::clamp<int>(floorf(pcl.phase * (float)_interpPhaseCount), 0, _interpPhaseCount - 1);
+                int phase = std::clamp<int>(floorf(pcl.phase * (float)interpPhaseCount), 0, interpPhaseCount - 1);
-                volk_32fc_32f_dot_prod_32fc((lv_32fc_t*)&sample, (lv_32fc_t*)&buffer[offset], interpBank.phases[phase], _interpTapCount);
+                volk_32fc_32f_dot_prod_32fc((lv_32fc_t*)&sample, (lv_32fc_t*)&buffer[offset], interpBank.phases[phase], interpTapCount);
 
-                // Update autocorrelation
+                // Write the sample to the output
+                if (outCount >= cpSize) {
+                    out.writeBuf[outCount - cpSize] = sample;
+                }
+                
+                // Send out a symbol when it's fully received
+                if ((++outCount) >= fftSize+cpSize) {
+                    if (!out.swap(outCount)) { break; }
+                    outCount = 0;
+                }
+
+                // Run autocorrelation
+                complex_t prod = sample.conj()*(*sampSlot);
+                corr += prod;
+                corr -= *prodSlot;
+
+                // Write back the new sample and product value to the cache
+                *sampSlot = sample;
+                *prodSlot = prod;
 
                 // Compute the correlation level
                 float corrLvl = corr.amplitude();
 
-                // Detect peak in autocorrelation
+                // Detect peak in autocorrelation (TODO: level check maybe not needed now that corrPeak is reset to corrLvl)
-                if (0/*TODO*/) {
+                if (corrLvl > corrAvg && corrLvl > corrPeak) {
                     // Save the current correlation as the peak
                     corrPeak = corrLvl;
 
                     // Save the value of the previous correlation as the left side of the peak
-                    corrPeakL = corrLastLvl;
+                    corrPeakL = corrLast;
 
-                    // Save the symbol period
+                    // Reset the peak distance counter
-                    measuredSymbolPeriod = sampCount;
+                    sincePeak = 0;
-
-                    // Reset the sample count
-                    sampCount = 0;
-
-                    // TODO: Maybe save the error to apply it at the end of the frame? (will  cause issues with the longer null symbol in DAB)
                 }
 
-                // Write the sample to the frame if within it
+                // The first sample after a peak is the right-side sample
-                if (sampCount < symbolSize) {
+                if (sincePeak == 1) {
-                    symbol[sampCount++] = sample;
+                    corrPeakR = corrLvl;
                 }
-
+                else if (sincePeak == cpSize) {
-                // When the end of the symbol is reached
+                    // Start the useful symbol counter
-                if (sampCount == symbolSize) {
+                    sinceCp = 0;
-                    // Send out the symbol
+                    
-                    // TODO
+                    // Compute the fractional error (TODO: Probably very inaccurate with noise, use real slopes instead)
+                    if (corrPeakL > corrPeakR) {
+                        float maxSlope = corrPeakR - corrPeak;
+                        float slope = corrPeak - corrPeakL;
+                        fracErr = std::clamp<float>(0.5f * (1.0f + slope / maxSlope), -0.5f, 0.5f);
+                    }
+                    else {
+                        float maxSlope = corrPeak - corrPeakL;
+                        float slope = corrPeakR - corrPeak;
+                        fracErr = std::clamp<float>(-0.5f * (1.0f + slope / maxSlope), -0.5f, 0.5f);
+                    }
                 }
+                else if (sincePeak == fftSize) {
+                    // Reset the peak detector
+                    corrPeak = corrAvg;
+                }
+                // NOTE: THIS IS ONLY NEEDED FOR DAB
+                // Detect a wider-than-normal distance to adapt the output counter
+                else if (sincePeak == 2656) {
+                    // Reset the output counter
+                    outCount = 50;
+                }
+                
+                // Last sample of useful symbol
+                if (sinceCp == fftSize) {
+                    // If the fractional error is valid, run closed-loop
+                    float err = 0.0f;
+                    if (!std::isnan(fracErr)) {
+                        // Compute the measured period using the distance to the last symbol
+                        float measuredPeriod = (float)sinceLastSym - fracErr;
+                        // NOTE: THIS IS ONLY NEEDED FOR DAB
+                        if (measuredPeriod > 3828.0f) {
+                            // Null symbol
+                            err = measuredPeriod - (2552.0f+2656.0f);
+                        }
+                        else {
+                            // Regular symbol
+                            err = measuredPeriod - period;
+                        }
 
-                // TODO: limit how much the sample count can grow otherwise otherflows will trigger a false frame detection
+                        err = std::clamp<float>(err, -10.0f, 10.0f);
 
-                // Run the control loop
+                        // Run the control loop in closed-loop mode
-                //pcl.advance(error); // TODO
+                        pcl.advance(err);
-                pcl.advancePhase();
+                    }
+                    else {
+                        // Otherwise, run open-loop
+                        pcl.advancePhase();
+                    }
+                    
+                    // printf("%d\n", outCount);
+
+                    // Nudge the symbol window if it's too out of sync
+                    if (outCount > 100) {
+                        // TODO: MOVE THE LAST SAMPLES OR THE SYMBOL WILL BE CORRUPTED!
+                        outCount = 50;
+                        flog::debug("NUDGE!");
+                    }
+
+                    // Reset the period counter
+                    sinceLastSym = 0;
+                }
+                else {
+                    // Run the control loop in open-loop mode
+                    pcl.advancePhase();
+                }
 
                 // Update the offset and phase
                 float delta = floorf(pcl.phase);
@@ -142,44 +248,45 @@ namespace dsp::ofdm {
                 pcl.phase -= delta;
 
                 // Update the last correlation level
-                corrLastLvl = corrLvl;
+                corrLast = corrLvl;
+
+                // Update correlation AGC
+                corrAvg = corrAvg*corrAgcInvRate + corrLvl*corrAgcRate;
+
+                // Increment the distance counters (TODO: Check if they happen at the right point, eg. after being reset to zero)
+                sincePeak++;
+                sinceLastSym++;
+                sinceCp++;
             }
 
             // Prepare offset for next buffer of samples
             offset -= count;
 
             // Update delay buffer
-            memmove(buffer, &buffer[count], (_interpTapCount - 1) * sizeof(complex_t));
+            memmove(buffer, &buffer[count], (interpTapCount - 1) * sizeof(complex_t));
-
-            return outCount;
-        }
-
-        int run() {
-            int count = base_type::_in->read();
-            if (count < 0) { return -1; }
-
-            int outCount = process(count, base_type::_in->readBuf, base_type::out.writeBuf);
 
             // Swap if some data was generated
             base_type::_in->flush();
-            if (outCount) {
+            return count;
-                if (!base_type::out.swap(outCount)) { return -1; }
-            }
-            return outCount;
         }
 
     protected:
         void generateInterpTaps() {
-            double bw = 0.5 / (double)_interpPhaseCount;
+            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);
+            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);
+            interpBank = dsp::multirate::buildPolyphaseBank<float>(interpPhaseCount, lp);
             taps::free(lp);
         }
 
+        // OFDM Configuration
+        int fftSize;
+        int cpSize;
+        float period;
+
         // Interpolator
         dsp::multirate::PolyphaseBank<float> interpBank;
-        int _interpPhaseCount;
+        int interpPhaseCount;
-        int _interpTapCount;
+        int interpTapCount;
         int offset = 0;
         complex_t* buffer = NULL;
         complex_t* bufStart;
@@ -187,24 +294,31 @@ namespace dsp::ofdm {
         // Control loop
         loop::PhaseControlLoop<float, false> pcl;
         double omega;
-        double _omegaGain;
+        double omegaGain;
-        double _muGain;
+        double muGain;
-        double _omegaRelLimit;
+        double omegaRelLimit;
+        float fracErr = 0.0f;
         
         // Autocorrelator
-        complex_t corr;
+        complex_t corr = {0.0f, 0.0f};
-        complex_t* corrProducts = NULL;
+        complex_t* corrSampCache = NULL;
+        complex_t* corrProdCache = NULL;
+        int corrSampCacheId = 0;
+        int corrProdCacheId = 0;
         float corrAgcRate;
         float corrAgcInvRate;
-        float corrLastLvl = 0;
+        float corrAvg = 0;
+        float corrLast = 0;
         float corrPeakR = 0;
         float corrPeak = 0;
         float corrPeakL = 0;
 
-        // Symbol
+        // Peak detection
-        complex_t* symbol; // TODO: Will use output stream buffer instead
+        int sincePeak = 0;
-        int symbolSize;
+        int sinceLastSym = 0;
-        int sampCount = 0;
+        int sinceCp = 0;
-        int measuredSymbolPeriod = 0;
+        
+        // Other shit to categorize
+        int outCount = 0;
     };
 };

decoder_modules/dab_decoder/src/optimized_algo.txt

@@ -1,34 +0,0 @@
-0123456789
----  ---
-
-0*4
-1*5
-2*6
-
-1*5
-2*6 = L + 3*7 - 0*4
-3*7
-
-2*6
-3*7 = L + 4*8 - 1*5
-4*8
-
-3*7
-4*8 = L + 5*9 - 2*6
-5*9
-
-
-
-0*5
-1*6
-2*7
-
-1*6
-2*7
-3*8
-
-2*7
-3*8
-4*9
-
-=> Use same technique to cache the interpolation results

decoder_modules/dab_decoder/src/optmized.txt

@@ -0,0 +1,31 @@
+cyclicLen = 4
+usefulLen = 12
+
+A =  0*12 +  1*13 +  2*14 +  3*15
+B =  1*13 +  2*14 +  3*15 +  4*16   = A -  0*12 +  4*16
+C =  2*14 +  3*15 +  4*16 +  5*17   = B -  1*13 +  5*17
+D =  3*15 +  4*16 +  5*17 +  6*18   = C -  2*14 +  6*18
+E =  4*16 +  5*17 +  6*18 +  7*19   = D -  3*15 +  7*19
+F =  5*17 +  6*18 +  7*19 +  8*20   = E -  4*16 +  8*20
+G =  6*18 +  7*19 +  8*20 +  9*21   = F -  5*17 +  9*21
+H =  7*19 +  8*20 +  9*21 + 10*22   = G -  6*18 + 10*22
+I =  8*20 +  9*21 + 10*22 + 11*23   = H -  7*19 + 11*23
+J =  9*21 + 10*22 + 11*23 + 12*24   = I -  8*20 + 12*24
+K = 10*22 + 11*23 + 12*24 + 13*25   = J -  9*21 + 13*25
+L = 11*23 + 12*24 + 13*25 + 14*26   = K - 10*22 + 14*26
+M = 12*24 + 13*25 + 14*26 + 15*27   = L - 11*23 + 15*27
+N = 13*25 + 14*26 + 15*27 + 16*28   = M - 12*24 + 16*28
+O = 14*26 + 15*27 + 16*28 + 17*29   = N - 13*25 + 17*29
+P = 15*27 + 16*28 + 17*29 + 18*30   = O - 14*26 + 18*30
+Q = 16*28 + 17*29 + 18*30 + 19*31   = P - 15*27 + 19*31
+R = 17*29 + 18*30 + 19*31 + 20*32   = Q - 16*28 + 20*32
+S = 18*30 + 19*31 + 20*32 + 21*33   = R - 17*29 + 21*33
+T = 19*31 + 20*32 + 21*33 + 22*34   = S - 18*30 + 22*34
+U = 20*32 + 21*33 + 22*34 + 23*35   = T - 19*31 + 23*35
+
+Conclusion:
+sampCacheLen = usefulLen
+prodCacheLen = cyclicLen
+
+Peak correlation occurs when the current interpolated value is the last FFT sample
+

decoder_modules/dab_decoder/src/test.txt

@@ -0,0 +1,397 @@
+5209
+2553
+2551
+2551
+2552
+2551
+2553
+2555
+2549
+2552
+2552
+2554
+2550
+2553
+2553
+2550
+2553
+2552
+2550
+2552
+2548
+2558
+2551
+2546
+2559
+2551
+2552
+2552
+2551
+2554
+2555
+2549
+2550
+2549
+2557
+2551
+2554
+2550
+2552
+2546
+2559
+2551
+2554
+2550
+2550
+2557
+2550
+2556
+2543
+2556
+2551
+2554
+2551
+2553
+2554
+2550
+2551
+2552
+2552
+2553
+2550
+2553
+2546
+2558
+2552
+2552
+2552
+2550
+2555
+2551
+2551
+2554
+2553
+2553
+2549
+2552
+5208
+2554
+2550
+2552
+2551
+2553
+2551
+2553
+2551
+2553
+2550
+2554
+2553
+2551
+2553
+2552
+2546
+2557
+2551
+2553
+2552
+2551
+2553
+2551
+2548
+2557
+2553
+2551
+2562
+2539
+2559
+2548
+2551
+2551
+2550
+2556
+2551
+2554
+2550
+2552
+2552
+2548
+2556
+2550
+2554
+2553
+2553
+2550
+2552
+2552
+2550
+2555
+2558
+2545
+2552
+2553
+2548
+2555
+2552
+2552
+2557
+2550
+2548
+2553
+2554
+2550
+2552
+2547
+2558
+2551
+2552
+2552
+2552
+2555
+2549
+2552
+5208
+2552
+2552
+2552
+2550
+2555
+2550
+2553
+2553
+2551
+2552
+2547
+2557
+2552
+2552
+2552
+2552
+2552
+2552
+2552
+2546
+2558
+2551
+2552
+2553
+2551
+2553
+2553
+2552
+2551
+2552
+2552
+2551
+2553
+2552
+2552
+2552
+2552
+2552
+2552
+2556
+2548
+2551
+2555
+2550
+2552
+2555
+2549
+2552
+2552
+2550
+2553
+2551
+2554
+2552
+2551
+2553
+2553
+2551
+2548
+2550
+2555
+2554
+2553
+2556
+2547
+2552
+2554
+2552
+2550
+2552
+2552
+2553
+2552
+2551
+2552
+5209
+2553
+2551
+2551
+2547
+2558
+2550
+2553
+2552
+2553
+2552
+2549
+2551
+2557
+2549
+2554
+2551
+2554
+2550
+2553
+2553
+2551
+2551
+2548
+2559
+2549
+2552
+2552
+2552
+2551
+2551
+2550
+2557
+2551
+2552
+2555
+2549
+2557
+2549
+2550
+2553
+2549
+2554
+2553
+2553
+2550
+2552
+2553
+2550
+2553
+2553
+2551
+2547
+2557
+2552
+2552
+2551
+2553
+2549
+2555
+2556
+2550
+2550
+2553
+2551
+2552
+2553
+2551
+2552
+2552
+2552
+2553
+2551
+2541
+2563
+2552
+5210
+2550
+2551
+2552
+2553
+2553
+2551
+2552
+2552
+2552
+2552
+2558
+2547
+2551
+2552
+2552
+2552
+2552
+2550
+2554
+2551
+2553
+2552
+2551
+2553
+2551
+2553
+2553
+2550
+2551
+2555
+2550
+2552
+2552
+2553
+2553
+2550
+2552
+2553
+2550
+2553
+2558
+2546
+2553
+2554
+2550
+2552
+2553
+2548
+2554
+2552
+2552
+2552
+2551
+2554
+2550
+2553
+2553
+2550
+2551
+2552
+2554
+2553
+2551
+2551
+2555
+2550
+2553
+2552
+2550
+2554
+2550
+2553
+2553
+2553
+2550
+5210
+2553
+2549
+2553
+2551
+2552
+2552
+2556
+2548
+2549
+2555
+2551
+2557
+2548
+2552
+2552
+2552