Add RFG UDP source to rph_radar

python_app/hardware_full/rph_radar/protocol.py

@@ -1,10 +1,15 @@
-"""Whole-sweep RPH1 packets from the collector's PTY (little endian).
+"""RPH sweep sources, both little endian.
 
-The 48-byte header carries payload size, source sweep/cycle IDs, monotonic capture
-window, input/output (65535 for untagged) and dirty flag. Payload points are
-8-byte ``marker, step, I, Q`` frames: A for main and A8 for reference.
-Only complete queued packets may be dropped; a packet already being written is
-always finished. Thus overflow cannot splice two physical sweeps together.
+E-502 collector PTY: whole-sweep RPH1 packets. The 48-byte header carries payload
+size, source sweep/cycle IDs, monotonic capture window, input/output (65535 for
+untagged) and dirty flag. Payload points are 8-byte ``marker, step, I, Q`` frames:
+A for main and A8 for reference. Only complete queued packets may be dropped; a
+packet already being written is always finished, so overflow cannot splice sweeps.
+
+RFG board UDP (``SWEEP_AVG=1``): a ``u16 values_per_point, u16 q_shift`` header and
+28-byte points ``u32 (sweep << 24) | step`` plus six int32 Q(q_shift) step averages
+``[a1c0, a1c1, a1c2, a2c0, a2c1, a2c2]``. Channel k is ``a1ck + j*a2ck``; the main
+signal is the synchronous detector ch0 - ch1 and the reference is ch2.
 """
 
 from dataclasses import dataclass
@@ -18,6 +23,8 @@ REFERENCE_MARKER = 0x00A8
 _HEADER = struct.Struct("<4sIQQQQHHI")
 _POINT = struct.Struct("<HHhh")
 _MAX_PAYLOAD_BYTES = 65534 * 2 * _POINT.size
+_RFG_HEADER = struct.Struct("<HH")
+_RFG_POINT = struct.Struct("<I6i")
 
 
 @dataclass(frozen=True, slots=True)
@@ -87,3 +94,68 @@ class RphRadarStreamParser:
                     None if inp == 65535 else (inp, out), bool(flags & 1),
                 ))
         return sweeps
+
+
+class RfgSweepAssembler:
+    """Collect RFG points into sweeps of exactly the laser task's ``steps``.
+
+    A sweep closes when the next sweep id appears. Ids advance by one per sweep, so an
+    id that is not ahead of the current one (mod 256) is a late point of an older
+    sweep and is ignored — unless 128 sweep periods passed, when any id is legitimate. It is kept only with steps ``0..steps-1`` in order: the
+    reference phase advances up to ~1.8 rad per step, so a lost, duplicate or
+    reordered point would break the unwrapped frequency axis. Arrival times are
+    kernel receive stamps (see the reader). Step k leaves the board
+    ``(k + 1) * step_ns`` after SWEEP_START and arrives no earlier, so the lower
+    envelope of ``arrival - (k + 1) * step_ns`` is the sweep start.
+    """
+
+    def __init__(self, steps: int, step_ns: int, period_ns: int) -> None:
+        self._steps, self._step_ns, self._period_ns = steps, step_ns, period_ns
+        self._sweep_id: int | None = None  # newest sweep; its points may still be arriving
+        self._last_ns = 0
+        self._points: list[tuple[int, complex, complex, int]] = []
+        self._count = 0
+
+    def reset(self) -> None:
+        """Drop the sweep in progress; its remaining points then form a dirty sweep."""
+        self._sweep_id, self._points = None, []
+
+    def feed(self, datagram: bytes, received_ns: int) -> list[RawSweep]:
+        if len(datagram) < _RFG_HEADER.size:
+            raise ValueError("RFG protocol violation: truncated datagram")
+        values, q_shift = _RFG_HEADER.unpack_from(datagram)
+        if values != 6 or q_shift > 30 or (len(datagram) - _RFG_HEADER.size) % _RFG_POINT.size:
+            raise ValueError("RFG protocol violation: unexpected datagram layout")
+        scale = 1.0 / (1 << q_shift)
+        sweeps = []
+        for word, a1c0, a1c1, a1c2, a2c0, a2c1, a2c2 in _RFG_POINT.iter_unpack(datagram[_RFG_HEADER.size:]):
+            if word >> 24 != self._sweep_id:
+                if (self._sweep_id is not None and not 0 < ((word >> 24) - self._sweep_id) % 256 < 128
+                        and received_ns - self._last_ns < 128 * self._period_ns):
+                    continue
+                sweeps.extend(self._close())
+                self._sweep_id = word >> 24
+            self._last_ns = received_ns
+            if len(self._points) <= self._steps:  # A stuck id cannot grow the buffer; the sweep is dirty.
+                self._points.append((word & 0xFFFFFF, complex(a1c0 - a1c1, a2c0 - a2c1) * scale,
+                                     complex(a1c2, a2c2) * scale, received_ns))
+        return sweeps
+
+    def _close(self) -> list[RawSweep]:
+        points, self._points = self._points, []
+        if not points:
+            return []
+        steps = np.asarray([point[0] for point in points], dtype=np.int32)
+        starts = [received - (step + 1) * self._step_ns for step, _, _, received in points]
+        start = min(starts)
+        # Every point of one sweep implies the same start within batching/latency jitter; an
+        # 8-bit id reused after an outage (256 sweeps, ~5 s) would splice two sweeps otherwise.
+        spliced = max(starts) - start > self._steps * self._step_ns
+        self._count += 1
+        return [RawSweep(
+            steps,
+            np.asarray([point[1] for point in points], dtype=np.complex64),
+            np.asarray([point[2] for point in points], dtype=np.complex64),
+            self._count, self._count, start, start + self._steps * self._step_ns,
+            dirty=spliced or not np.array_equal(steps, np.arange(self._steps)),
+        )]