work on ATV demod

decoder_modules/atv_decoder/src/linesync.h

@@ -5,6 +5,25 @@
 #include <dsp/multirate/polyphase_bank.h>
 #include <dsp/math/step.h>
 
+#define LINE_SIZE       945
+
+#define SYNC_LEN        70
+#define SYNC_SIDE_LEN   17
+#define SYNC_L_START    (LINE_SIZE - SYNC_SIDE_LEN)
+#define SYNC_R_START    (SYNC_LEN/2)
+#define SYNC_R_END      (SYNC_R_START + (SYNC_LEN/2) + SYNC_SIDE_LEN)
+#define SYNC_HALF_LEN   ((SYNC_LEN/2) + SYNC_SIDE_LEN)
+
+#define EQUAL_LEN       35
+
+#define HBLANK_START    SYNC_LEN
+#define HBLANK_END      155
+#define HBLANK_LEN      (HBLANK_END - HBLANK_START + 1)
+
+#define SYNC_LEVEL      (-0.428)
+
+#define MAX_LOCK    1000
+
 class LineSync : public dsp::Processor<float, float> {
     using base_type = dsp::Processor<float, float>;
 public:
@@ -27,41 +46,17 @@ public:
         _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];
+
+        // TODO: Needs tuning, so do the gains
+        maxPeriod = (int32_t)(1.0001 * (float)(1 << 30));
+        minPeriod = (int32_t)(0.9999 * (float)(1 << 30));
     
         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);
@@ -81,8 +76,7 @@ public:
         std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
         base_type::tempStop();
         offset = 0;
-        pcl.phase = 0.0f;
-        pcl.freq = _omega;
+        phase = 0;
         base_type::tempStart();
     }
 
@@ -93,61 +87,120 @@ public:
         // Copy data to work buffer
         memcpy(bufStart, base_type::_in->readBuf, count * sizeof(float));
 
-        if (test2) {
-            test2 = false;
-            offset += 5;
-        }
-
-        // Process all samples
+        // Process samples while they are available
         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;
+            // While the offset is negative, out put zeros
+            while (offset < 0 && pixel < LINE_SIZE) {
+                // Output a zero
+                base_type::out.writeBuf[pixel++] = 0.0f;
 
-            // If the end of the line is reached, process it and determin error
-            float error = 0;
-            if (outCount >= 720) {
-                // Compute averages.
+                // Increment the phase
+                phase += period;
+                offset += (phase >> 30);
+                phase &= 0x3FFFFFFF;
+            }
+
+            // Process as much of a line as possible
+            while (offset < count && pixel < LINE_SIZE) {
+                // Compute the output sample
+                volk_32f_x2_dot_prod_32f(&base_type::out.writeBuf[pixel++], &buffer[offset], interpBank.phases[(phase >> 23) & 0x7F], _interpTapCount);
+                
+                // Increment the phase
+                phase += period;
+                offset += (phase >> 30);
+                phase &= 0x3FFFFFFF;
+            }
+
+            // If the line is done, process it
+            if (pixel == LINE_SIZE) {
+                // Compute averages. (TODO: Try faster method)
                 float left = 0.0f, right = 0.0f;
-                for (int i = (720-17); i < 720; i++) {
+                int lc = 0, rc = 0;
+                for (int i = SYNC_L_START; i < LINE_SIZE; i++) {
                     left += base_type::out.writeBuf[i];
+                    lc++;
                 }
-                for (int i = 0; i < 27; i++) {
+                for (int i = 0; i < SYNC_R_START; i++) {
                     left += base_type::out.writeBuf[i];
+                    lc++;
                 }
-                for (int i = 27; i < (54+17); i++) {
+                for (int i = SYNC_R_START; i < SYNC_R_END; 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;
+                    rc++;
                 }
 
-                if (++counter >= 100) {
-                    counter = 0;
-                    //flog::warn("Left: {}, Right: {}, Error: {}, Freq: {}, Phase: {}", left, right, error, pcl.freq, pcl.phase);
+                // Compute the error
+                float error = (left - right) * (1.0f/((float)SYNC_HALF_LEN));
+
+                // Compute the change in phase and frequency due to the error
+                float periodDelta = error * _omegaGain;
+                float phaseDelta = error * _muGain;
+
+                // Normalize the phase delta (TODO: Make faster)
+                while (phaseDelta <= -1.0f) {
+                    phaseDelta += 1.0f;
+                    offset--;
+                }
+                while (phaseDelta >= 1.0f) {
+                    phaseDelta -= 1.0f;
+                    offset++;
+                }
+
+                // Update the period (TODO: Clamp error*omegaGain to prevent weird shit with corrupt samples)
+                period += (int32_t)(periodDelta * (float)(1 << 30));
+                period = std::clamp<uint32_t>(period, minPeriod, maxPeriod);
+
+                // Update the phase
+                phase += (int32_t)(phaseDelta * (float)(1 << 30));
+
+                // Normalize the phase
+                uint32_t overflow = phase >> 30;
+                if (overflow) {
+                    if (error < 0) {
+                        offset -= 4 - overflow;
+                    }
+                    else {
+                        offset += overflow;
+                    }
+                }
+                phase &= 0x3FFFFFFF;
+
+                // Find the lowest value
+                float lowest = INFINITY;
+                int lowestId = -1;
+                for (int i = 0; i < LINE_SIZE; i++) {
+                    float val =  base_type::out.writeBuf[i];
+                    if (val < lowest) {
+                        lowest = val;
+                        lowestId = i;
+                    }
+                }
+
+                // Check the the line is in lock
+                bool lineLocked = (lowestId < SYNC_R_END || lowestId >= SYNC_L_START);
+
+                // Update the lock status based on the line lock
+                if (!lineLocked && locked) {
+                    locked--;
+                }
+                else if (lineLocked && locked < MAX_LOCK) {
+                    locked++;
+                }
+
+                // If not locked, attempt to lock by forcing the sync to happen at the right spot
+                // TODO: This triggers waaaay too easily at low SNR
+                if (!locked && fastLock) {
+                    offset += lowestId - SYNC_R_START;
+                    locked = MAX_LOCK / 2;
                 }
 
                 // Output line
-                if (!base_type::out.swap(outCount)) { break; }
-                outCount = 0;
+                if (!base_type::out.swap(LINE_SIZE)) { break; }
+                pixel = 0;
             }
-
-            // Advance symbol offset and phase
-            pcl.advance(error);
-            float delta = floorf(pcl.phase);
-            offset += delta;
-            pcl.phase -= delta;
         }
+
+        // Get the offset ready for the next buffer
         offset -= count;
 
         // Update delay buffer
@@ -155,16 +208,15 @@ public:
 
         // Swap if some data was generated
         base_type::_in->flush();
-        return outCount;
+        return 0;
     }
 
-    bool locked = false;
-    bool test2 = false;
+    float syncBias = 0;
 
-    float syncBias = 0.0f;
-    bool forceLock = false;
+    uint32_t period = (0x800072F3 >> 1);//(1 << 31) + 1;
 
-    int counter = 0;
+    int locked = 0;
+    bool fastLock = true;
 
 protected:
     void generateInterpTaps() {
@@ -175,7 +227,6 @@ protected:
     }
 
     dsp::multirate::PolyphaseBank<float> interpBank;
-    dsp::loop::PhaseControlLoop<double, false> pcl;
 
     double _omega;
     double _omegaGain;
@@ -183,11 +234,14 @@ protected:
     double _omegaRelLimit;
     int _interpPhaseCount;
     int _interpTapCount;
-
-    int offset = 0;
-    int outCount = 0;
     float* buffer;
     float* bufStart;
-    
+
+    uint32_t phase = 0;
+    uint32_t maxPeriod;
+    uint32_t minPeriod;
     float syncLevel = -0.03f;
+
+    int offset = 0;
+    int pixel = 0;
 };

decoder_modules/atv_decoder/src/main.cpp

@@ -20,6 +20,11 @@
 
 #include "chroma_pll.h"
 
+#include "linesync_old.h"
+#include "amplitude.h"
+#include <dsp/demod/am.h>
+#include <dsp/loop/fast_agc.h>
+
 #define CONCAT(a, b) ((std::string(a) + b).c_str())
 
 SDRPP_MOD_INFO{/* Name:            */ "atv_decoder",
@@ -29,24 +34,24 @@ SDRPP_MOD_INFO{/* Name:            */ "atv_decoder",
                /* Max instances    */ -1
 };
 
-#define SAMPLE_RATE (625.0f * 720.0f * 25.0f)
+#define SAMPLE_RATE (625.0f * (float)LINE_SIZE * 25.0f)
 
 class ATVDecoderModule : public ModuleManager::Instance {
   public:
-    ATVDecoderModule(std::string name) : img(720, 625) {
+    ATVDecoderModule(std::string name) : img(768, 576) {
         this->name = name;
 
-        vfo = sigpath::vfoManager.createVFO(name, ImGui::WaterfallVFO::REF_CENTER, 0, 8000000.0f, SAMPLE_RATE, SAMPLE_RATE, SAMPLE_RATE, true);
+        vfo = sigpath::vfoManager.createVFO(name, ImGui::WaterfallVFO::REF_CENTER, 0, 7000000.0f, SAMPLE_RATE, SAMPLE_RATE, SAMPLE_RATE, true);
 
-        demod.init(vfo->output, SAMPLE_RATE, SAMPLE_RATE / 2.0f);
+        agc.init(vfo->output, 1.0f, 1e6, 0.001f, 1.0f);
+        demod.init(&agc.out, SAMPLE_RATE, SAMPLE_RATE / 2.0f);
+        //demod.init(vfo->output, dsp::demod::AM<float>::CARRIER, 8000000.0f, 50.0 / SAMPLE_RATE, 50.0 / SAMPLE_RATE, 0.0f, SAMPLE_RATE);
         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));
 
+        agc.start();
         demod.start();
         sync.start();
         sink.start();
@@ -58,7 +63,10 @@ class ATVDecoderModule : public ModuleManager::Instance {
         if (vfo) {
             sigpath::vfoManager.deleteVFO(vfo);
         }
+        agc.stop();
         demod.stop();
+        sync.stop();
+        sink.stop();
         gui::menu.removeEntry(name);
     }
 
@@ -82,142 +90,146 @@ class ATVDecoderModule : public ModuleManager::Instance {
             style::beginDisabled();
         }
 
-        // Ideal width for testing: 750pixels
-
         ImGui::FillWidth();
         _this->img.draw();
 
-        ImGui::LeftLabel("Sync");
-        ImGui::FillWidth();
-        ImGui::SliderFloat("##syncLvl", &_this->sync_level, -2, 2);
-
-        ImGui::LeftLabel("Min");
-        ImGui::FillWidth();
-        ImGui::SliderFloat("##minLvl", &_this->minLvl, -1.0, 1.0);
-
-        ImGui::LeftLabel("Span");
-        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::TextUnformatted("Horizontal Sync:");
+        ImGui::SameLine();
+        if (_this->sync.locked > 750) {
             ImGui::TextColored(ImVec4(0, 1, 0, 1), "Locked");
         }
         else {
             ImGui::TextUnformatted("Not locked");
         }
 
-        ImGui::Checkbox("Force Lock", &_this->sync.forceLock);
+        ImGui::TextUnformatted("Vertical Sync:");
+        ImGui::SameLine();
+        if (_this->vlock > 15) {
+            ImGui::TextColored(ImVec4(0, 1, 0, 1), "Locked");
+        }
+        else {
+            ImGui::TextUnformatted("Not locked");
+        }
+
+        ImGui::Checkbox("Fast Lock", &_this->sync.fastLock);
+        ImGui::Checkbox("Color Mode", &_this->colorMode);
 
         if (!_this->enabled) {
             style::endDisabled();
         }
+
+        ImGui::Text("Gain: %f", _this->gain);
+        ImGui::Text("Offset: %f", _this->offset);
     }
 
     static void handler(float *data, int count, void *ctx) {
         ATVDecoderModule *_this = (ATVDecoderModule *)ctx;
 
-        // Convert line to complex
-        _this->r2c.process(720, data, _this->r2c.out.writeBuf);
+        // Correct the offset
+        volk_32f_s32f_add_32f(data, data, _this->offset, count);
 
-        // Isolate the chroma subcarrier
-        _this->fir.process(720, _this->r2c.out.writeBuf, _this->fir.out.writeBuf);
+        // Correct the gain
+        volk_32f_s32f_multiply_32f(data, data, _this->gain, count);
 
-        // Run chroma carrier through the PLL
-        _this->pll.process(720, _this->fir.out.writeBuf, _this->pll.out.writeBuf, ((_this->ypos%2)==1) ^ _this->evenFrame);
+        // Compute the sync levels
+        float syncLLevel = 0.0f;
+        float syncRLevel = 0.0f;
+        volk_32f_accumulator_s32f(&syncLLevel, data, EQUAL_LEN);
+        volk_32f_accumulator_s32f(&syncRLevel, &data[EQUAL_LEN], SYNC_LEN - EQUAL_LEN);
+        syncLLevel *= 1.0f / EQUAL_LEN;
+        syncRLevel *= 1.0f / (SYNC_LEN - EQUAL_LEN);
+        float syncLevel = (syncLLevel + syncRLevel) * 0.5f; // TODO: It's technically correct but if the sizes were different it wouldn't be
 
-        // 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) ];
+        // Compute the blanking level
+        float blankLevel = 0.0f;
+        volk_32f_accumulator_s32f(&blankLevel, &data[HBLANK_START], HBLANK_LEN);
+        blankLevel /= (float)HBLANK_LEN;
 
-        //uint32_t* currentLine = &((uint32_t *)_this->img.buffer)[_this->ypos*720];
+        // Run the offset control loop
+        _this->offset -= (blankLevel / _this->gain)*0.001;
+        _this->offset = std::clamp<float>(_this->offset, -1.0f, 1.0f);
+        _this->gain -= (blankLevel - syncLevel + SYNC_LEVEL)*0.01f;
+        _this->gain = std::clamp<float>(_this->gain, 0.1f, 10.0f);
 
-        for (int i = 0; i < count; i++) {
-            int imval = std::clamp<float>((data[i] - _this->minLvl) * 255.0 / _this->spanLvl, 0, 255);
-            // uint32_t re = std::clamp<float>((_this->pll.out.writeBuf[i].re - _this->minLvl) * 255.0 / _this->spanLvl, 0, 255);
-            // uint32_t im = std::clamp<float>((_this->pll.out.writeBuf[i].im - _this->minLvl) * 255.0 / _this->spanLvl, 0, 255);
-            // currentLine[i] = 0xFF000000 | (im << 8) | re;
-            currentLine[i] = 0xFF000000 | (imval << 16) | (imval << 8) | imval;
-        }
-    
-        // Vertical scan logic
-        _this->ypos++;
-        bool rollover = _this->ypos >= 625;
-        if (rollover) {
-            {
-                std::lock_guard<std::mutex> lck(_this->evenFrameMtx);
-                _this->evenFrame = !_this->evenFrame;
+        // Detect the sync type
+        uint16_t shortSync = (syncLLevel < 0.5f*SYNC_LEVEL) && (syncRLevel > 0.5f*SYNC_LEVEL) && (blankLevel > 0.5f*SYNC_LEVEL);
+        uint16_t longSync = (syncLLevel < 0.5f*SYNC_LEVEL) && (syncRLevel < 0.5f*SYNC_LEVEL) && (blankLevel < 0.5f*SYNC_LEVEL);
+
+        // Save sync type to history
+        _this->syncHistory = (_this->syncHistory << 2) | (longSync << 1) | shortSync;
+
+        // Render the line if it's visible
+        if (_this->ypos >= 34 && _this->ypos <= 34+576-1) {
+            uint32_t* currentLine = &((uint32_t *)_this->img.buffer)[(_this->ypos - 34)*768];
+            for (int i = 155; i < (155+768); i++) {
+                int imval = std::clamp<float>(data[i] * 255.0f, 0, 255);
+                currentLine[i-155] = 0xFF000000 | (imval << 16) | (imval << 8) | imval;
             }
+        }
+
+        // Compute whether to rollover
+        bool rollToOdd = (_this->ypos == 624);
+        bool rollToEven = (_this->ypos == 623);
+
+        // Compute the field sync
+        bool syncToOdd = (_this->syncHistory == 0b0101011010010101);
+        bool syncToEven = (_this->syncHistory == 0b0001011010100101);
+
+        // Process the sync (NOTE: should start with 0b01, but for some reason I don't see a sync?)
+        if (rollToOdd || syncToOdd) {
+            // Update the vertical lock state
+            bool disagree = (rollToOdd ^ syncToOdd);
+            if (disagree && _this->vlock > 0) {
+                _this->vlock--;
+            }
+            else if (!disagree && _this->vlock < 20) {
+                _this->vlock++;
+            }
+
+            // Start the odd field
+            _this->ypos = 1;
+        }
+        else if (rollToEven || syncToEven) {
+            // Update the vertical lock state
+            bool disagree = (rollToEven ^ syncToEven);
+            if (disagree && _this->vlock > 0) {
+                _this->vlock--;
+            }
+            else if (!disagree && _this->vlock < 20) {
+                _this->vlock++;
+            }
+
+            // Start the even field
             _this->ypos = 0;
+
+            // Swap the video buffer
             _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);
-
-        // Save sync detection to history
-        _this->syncHistory >>= 2;
-        _this->syncHistory |= (((uint16_t)(sync1 < _this->sync_level)) << 9) | (((uint16_t)(sync0 < _this->sync_level)) << 8);
-
-        // Trigger vsync in case one is detected
-        // TODO: Also sync with odd field
-        if (!rollover && _this->syncHistory == 0b0000011111) {
-            {
-                std::lock_guard<std::mutex> lck(_this->evenFrameMtx);
-                _this->evenFrame = !_this->evenFrame;
-            }
-            _this->ypos = 0;
-            _this->img.swap();
+        else {
+            _this->ypos += 2;
         }
     }
 
+    // NEW SYNC:
+    float offset = 0.0f;
+    float gain = 1.0f;
+    uint16_t syncHistory = 0;
+    int ypos = 0;
+    int vlock = 0;
+
     std::string name;
     bool enabled = true;
 
     VFOManager::VFO *vfo = NULL;
-    dsp::demod::Quadrature demod;
+    //dsp::demod::Quadrature demod;
+    dsp::loop::FastAGC<dsp::complex_t> agc;
+    dsp::demod::Amplitude demod;
+    //dsp::demod::AM<float> demod;
     LineSync sync;
     dsp::sink::Handler<float> sink;
     dsp::convert::RealToComplex r2c;
-    dsp::tap<dsp::complex_t> chromaTaps;
-    dsp::filter::FIR<dsp::complex_t, dsp::complex_t> fir;
-    dsp::loop::ChromaPLL pll;
-    int ypos = 0;
 
-    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;
+    bool colorMode = false;
 
     ImGui::ImageDisplay img;
 };