venus/axiom-nmea/debug/find_heading_vs_twd.py

#!/usr/bin/env python3
"""
Find heading vs TWD candidates.
At anchor pointing into wind: heading and TWD should be within ~50° of each other.
TWD expected: 69-73°, so heading could be ~20-120°
"""

import struct
from collections import defaultdict

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:  # NaN check
            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

# Search for angles in a wider range around expected TWD (69-73°)
# Heading at anchor into wind could be 20-120°
TARGET_DEG_MIN = 15
TARGET_DEG_MAX = 130
TARGET_RAD_MIN = TARGET_DEG_MIN * 0.0174533
TARGET_RAD_MAX = TARGET_DEG_MAX * 0.0174533

print("Reading raymarine_sample_twd_69-73.pcap...")
packets = read_pcap("raymarine_sample_twd_69-73.pcap")
print(f"Loaded {len(packets)} packets")
print(f"\nSearching for angles {TARGET_DEG_MIN}-{TARGET_DEG_MAX}° (could be heading or TWD)")
print("Expected: TWD ~69-73°, Heading within ±50° of that\n")

# Track candidates by offset
candidates = defaultdict(list)

for pkt_idx, pkt in enumerate(packets):
    pkt_len = len(pkt)
    if pkt_len < 100:
        continue

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

        # Check if in target radian range
        if TARGET_RAD_MIN <= val <= TARGET_RAD_MAX:
            deg = val * 57.2958
            candidates[offset].append((pkt_idx, pkt_len, val, deg))

print("=" * 70)
print("ANGLE CANDIDATES (heading or TWD)")
print("Filtering for offsets with >20 hits in target packet sizes")
print("=" * 70)

# Focus on packet sizes known to have wind data
target_sizes = {344, 446, 788, 888, 931, 1031, 1472}

for offset in sorted(candidates.keys()):
    hits = candidates[offset]
    # Filter to target packet sizes
    target_hits = [(i, s, v, d) for i, s, v, d in hits if s in target_sizes]

    if len(target_hits) < 10:
        continue

    degs = [d for _, _, _, d in target_hits]
    pkt_sizes = sorted(set(s for _, s, _, _ in target_hits))

    avg_deg = sum(degs) / len(degs)
    min_deg = min(degs)
    max_deg = max(degs)

    # Categorize based on value
    if 65 <= avg_deg <= 80:
        category = "*** LIKELY TWD ***"
    elif 20 <= avg_deg <= 50 or 90 <= avg_deg <= 120:
        category = "    (could be heading)"
    else:
        category = ""

    print(f"\n  0x{offset:04x}: {len(target_hits):3d} hits, avg={avg_deg:5.1f}°, range={min_deg:.1f}°-{max_deg:.1f}° {category}")
    print(f"    Sizes: {pkt_sizes}")

# Now compare offset 0x006b with nearby offsets
print("\n" + "=" * 70)
print("DETAILED COMPARISON: 0x006b vs nearby offsets")
print("=" * 70)

check_offsets = [0x0066, 0x006b, 0x0070, 0x0075, 0x007a]

for pkt_len in [344, 446, 788, 888]:
    matching = [p for p in packets if len(p) == pkt_len][:3]
    if not matching:
        continue

    print(f"\n  {pkt_len} byte packets (first 3):")
    for off in check_offsets:
        vals = [decode_float(p, off) for p in matching]
        degs = [f"{v * 57.2958:.1f}°" if v and 0 <= v <= 6.5 else "---" for v in vals]
        print(f"    0x{off:04x}: {degs}")