From de7b4dac4af5ff592258f48eee72d191201826d7 Mon Sep 17 00:00:00 2001
From: Nao Pross <np@0hm.ch>
Date: Thu, 2 Dec 2021 21:24:33 +0100
Subject: Begin frequency correction (incomplete)

---
 tests/correlator/epy_block_0.py | 75 +++++++++++++++++++++++++++--------------
 1 file changed, 50 insertions(+), 25 deletions(-)

(limited to 'tests/correlator/epy_block_0.py')

diff --git a/tests/correlator/epy_block_0.py b/tests/correlator/epy_block_0.py
index 1fa4794..90ad75e 100644
--- a/tests/correlator/epy_block_0.py
+++ b/tests/correlator/epy_block_0.py
@@ -20,56 +20,81 @@ class blk(gr.sync_block):
         # because of block processing a tagged block could
         # be split in half so we need to keep track of the
         # "previous" values
-        self.last_phase = 0
+        self.last_tag = None
 
     def work(self, input_items, output_items):
-        # TODO: interpolate phase values for frequency correction
-
+        # nicer aliases
         out = output_items[0]
         inp = input_items[0]
 
-        # create a phase correction vector
-        phase = np.zeros(len(inp), dtype=np.float64)
+        def print_phase_correction(start, end, phase, freq):
+            print(f"Correction for block {start:3d} to {end:3d} is phase = {phase: 2.4f} rad, freq = {freq * 1e3: 2.4f} milli rad/samp")
 
-        # read tags
+        # read only phase tags
         tags = self.get_tags_in_window(0, 0, len(inp))
 
-        # get only phase tags
         is_phase = lambda tag: pmt.to_python(tag.key) == "phase_est"
         phase_tags = list(filter(is_phase, tags))
 
+        # FIXME: what if there are no tags? check that!
+
         print(f"Processing {len(inp)} samples, with {len(phase_tags)} tags")
 
-        # compute correction from previous block
-        first_tag = phase_tags[0]
-        first_idx = first_tag.offset - self.sampnr
-        phase[:first_idx] = self.last_phase
+        # create a phase correction vector
+        phase = np.zeros(len(inp), dtype=np.float64)
+
+        # compute correction from previous block (if present)
+        if self.last_tag:
+            # variables for first and last phase values
+            lval = pmt.to_python(self.last_tag.value)
+            fval = pmt.to_python(phase_tags[0].value)
+
+            # compute index for phase vector
+            fidx = phase_tags[0].offset - self.sampnr
+
+            # compute frequency offset
+            nsamples = phase_tags[0].offset - self.last_tag.offset
+            freq = (fval - lval) / nsamples
+
+            # total phase correction is: phase + freq * time
+            phase[:fidx] = lval * np.ones(fidx) + freq * np.arange(0, fidx)
+
+            # compute correction
+            print_phase_correction(0, fidx, lval, freq)
 
         # iterate phase tags "in the middle"
+        # FIXME: what if there are less than 2 tags?
+        #        the code below will probably crash
         for prev_tag, next_tag in zip(phase_tags, phase_tags[1:]):
             # unpack pmt values
             pval = pmt.to_python(prev_tag.value)
+            nval = pmt.to_python(next_tag.value)
 
             # compute indexes in phase vector
             pidx = prev_tag.offset - self.sampnr
-            idx = next_tag.offset - self.sampnr
+            nidx = next_tag.offset - self.sampnr
 
-            # compute phase correction for block
-            phase[pidx:idx] = pval
-            print(f"Correction for block {pidx} to {idx} is {pval}")
+            # compute frquency correction for block by linearly interpolating
+            # frame values
+            nsamples = nidx - pidx
+            freq = (nval - pval) / nsamples
 
-        # compute the remaining part of the block
-        last_tag = phase_tags[-1]
-        last_val = pmt.to_python(last_tag.value)
-        last_idx = last_tag.offset - self.sampnr
+            # total correction is: phase + freq * time
+            phase[pidx:nidx] = pval * np.ones(nsamples) + freq * np.arange(0, nsamples)
+            print_phase_correction(pidx, nidx, pval, freq)
 
-        phase[last_idx:] = last_val
+        # for the last block because the next tag is unknown (in the future) we
+        # cannot predict the frequency offset. Thus we save the last tag for
+        # the next call.
+        self.last_tag = phase_tags[-1]
+        val = pmt.to_python(self.last_tag.value)
+        idx = self.last_tag.offset - self.sampnr
 
-        # save values for next call
-        self.last_phase = last_val
+        phase[idx:] = val
 
-        # mark samples as processed and compute to output
-        self.sampnr += len(inp)
+        # compute correction
         out[:] = inp * np.exp(-1j * phase)
 
-        return len(out)
+        # increment processed samples counter
+        self.sampnr += len(inp)
+        return len(phase)
-- 
cgit v1.2.1