venus/axiom-nmea/debug/find_twd_hdg.py

#!/usr/bin/env python3
"""
Find TWD and Heading offsets using known values:
- TWD: 62-70°
- Heading: 29-35°
"""

import struct
from collections import defaultdict

# Known ranges
TWD_DEG_MIN, TWD_DEG_MAX = 60, 72  # Slightly wider
HDG_DEG_MIN, HDG_DEG_MAX = 27, 37  # Slightly wider

TWD_RAD_MIN = TWD_DEG_MIN * 0.0174533
TWD_RAD_MAX = TWD_DEG_MAX * 0.0174533
HDG_RAD_MIN = HDG_DEG_MIN * 0.0174533
HDG_RAD_MAX = HDG_DEG_MAX * 0.0174533

def decode_float(data, offset):
    if offset + 4 > len(data):
        return None
    try:
        val = struct.unpack('<f', data[offset:offset+4])[0]
        if val != val:
            return None
        return val
    except:
        return None

def read_pcap(filename):
    packets = []
    with open(filename, 'rb') as f:
        header = f.read(24)
        magic = struct.unpack('<I', header[0:4])[0]
        swapped = magic == 0xd4c3b2a1
        endian = '>' if swapped else '<'

        while True:
            pkt_header = f.read(16)
            if len(pkt_header) < 16:
                break
            ts_sec, ts_usec, incl_len, orig_len = struct.unpack(f'{endian}IIII', pkt_header)
            pkt_data = f.read(incl_len)
            if len(pkt_data) < incl_len:
                break

            if len(pkt_data) > 42 and pkt_data[12:14] == b'\x08\x00':
                ip_header_len = (pkt_data[14] & 0x0F) * 4
                payload_start = 14 + ip_header_len + 8
                if payload_start < len(pkt_data):
                    packets.append(pkt_data[payload_start:])
    return packets

print("Reading raymarine_sample_TWD_62-70_HDG_29-35.pcap...")
packets = read_pcap("raymarine_sample_TWD_62-70_HDG_29-35.pcap")
print(f"Loaded {len(packets)} packets\n")

print(f"Searching for:")
print(f"  TWD: {TWD_DEG_MIN}-{TWD_DEG_MAX}° ({TWD_RAD_MIN:.3f}-{TWD_RAD_MAX:.3f} rad)")
print(f"  HDG: {HDG_DEG_MIN}-{HDG_DEG_MAX}° ({HDG_RAD_MIN:.3f}-{HDG_RAD_MAX:.3f} rad)\n")

# Track candidates
twd_candidates = defaultdict(list)
hdg_candidates = defaultdict(list)

target_sizes = {344, 446, 788, 888, 931, 1031, 1472}

for pkt_idx, pkt in enumerate(packets):
    pkt_len = len(pkt)
    if pkt_len not in target_sizes:
        continue

    for offset in range(0x50, min(pkt_len - 4, 0x200)):
        val = decode_float(pkt, offset)
        if val is None:
            continue

        deg = val * 57.2958

        if TWD_RAD_MIN <= val <= TWD_RAD_MAX:
            twd_candidates[offset].append((pkt_len, deg))

        if HDG_RAD_MIN <= val <= HDG_RAD_MAX:
            hdg_candidates[offset].append((pkt_len, deg))

print("=" * 70)
print(f"TWD CANDIDATES ({TWD_DEG_MIN}-{TWD_DEG_MAX}°)")
print("=" * 70)

for offset in sorted(twd_candidates.keys(), key=lambda x: -len(twd_candidates[x]))[:15]:
    hits = twd_candidates[offset]
    degs = [d for _, d in hits]
    sizes = sorted(set(s for s, _ in hits))
    avg = sum(degs) / len(degs)
    print(f"  0x{offset:04x}: {len(hits):3d} hits, avg={avg:.1f}°, range={min(degs):.1f}-{max(degs):.1f}°, sizes={sizes}")

print("\n" + "=" * 70)
print(f"HEADING CANDIDATES ({HDG_DEG_MIN}-{HDG_DEG_MAX}°)")
print("=" * 70)

for offset in sorted(hdg_candidates.keys(), key=lambda x: -len(hdg_candidates[x]))[:15]:
    hits = hdg_candidates[offset]
    degs = [d for _, d in hits]
    sizes = sorted(set(s for s, _ in hits))
    avg = sum(degs) / len(degs)
    print(f"  0x{offset:04x}: {len(hits):3d} hits, avg={avg:.1f}°, range={min(degs):.1f}-{max(degs):.1f}°, sizes={sizes}")

# Cross-check: find offsets that appear in both (shouldn't happen if ranges don't overlap)
common = set(twd_candidates.keys()) & set(hdg_candidates.keys())
if common:
    print(f"\n⚠️  Offsets appearing in both ranges: {[hex(o) for o in common]}")