venus/axiom-nmea/debug/compare_heading_both_pcaps.py

#!/usr/bin/env python3
"""Compare heading candidates between both pcap files."""

import struct

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

# Heading candidate offsets by packet size (from TWD analysis)
HEADING_OFFSETS = {
    344: 0x0144,
    446: 0x0189,
    788: 0x0081,
    888: 0x0081,
    931: 0x0081,
    1031: 0x0081,
    1472: 0x0088,
}

# TWD offset (consistent across sizes)
TWD_OFFSET = 0x006b

print("=" * 70)
print("COMPARING TWD (0x006b) AND HEADING CANDIDATES ACROSS BOTH PCAPS")
print("=" * 70)

for pcap_file in ["raymarine_sample.pcap", "raymarine_sample_twd_69-73.pcap"]:
    print(f"\n{'='*70}")
    print(f"FILE: {pcap_file}")
    print("=" * 70)

    packets = read_pcap(pcap_file)

    # Group by packet size
    by_size = {}
    for pkt in packets:
        pkt_len = len(pkt)
        if pkt_len not in by_size:
            by_size[pkt_len] = []
        by_size[pkt_len].append(pkt)

    print(f"\n{'Pkt Size':<10} {'Count':<8} {'TWD (0x006b)':<18} {'Heading':<18} {'Diff':<10}")
    print("-" * 70)

    for pkt_len in sorted(HEADING_OFFSETS.keys()):
        if pkt_len not in by_size:
            continue

        pkts = by_size[pkt_len]
        heading_offset = HEADING_OFFSETS[pkt_len]

        # Get all values
        twd_vals = []
        hdg_vals = []

        for pkt in pkts:
            twd = decode_float(pkt, TWD_OFFSET)
            hdg = decode_float(pkt, heading_offset)

            if twd and 0 <= twd <= 6.5:
                twd_vals.append(twd * 57.2958)
            if hdg and 0 <= hdg <= 6.5:
                hdg_vals.append(hdg * 57.2958)

        if twd_vals and hdg_vals:
            twd_avg = sum(twd_vals) / len(twd_vals)
            hdg_avg = sum(hdg_vals) / len(hdg_vals)
            diff = abs(twd_avg - hdg_avg)

            twd_str = f"{twd_avg:.1f}° ({min(twd_vals):.0f}-{max(twd_vals):.0f})"
            hdg_str = f"{hdg_avg:.1f}° ({min(hdg_vals):.0f}-{max(hdg_vals):.0f})"

            print(f"{pkt_len:<10} {len(pkts):<8} {twd_str:<18} {hdg_str:<18} {diff:.1f}°")
        elif twd_vals:
            twd_avg = sum(twd_vals) / len(twd_vals)
            print(f"{pkt_len:<10} {len(pkts):<8} {twd_avg:.1f}°              ---")
        else:
            print(f"{pkt_len:<10} {len(pkts):<8} ---                ---")

print("\n" + "=" * 70)
print("INTERPRETATION")
print("=" * 70)
print("""
If heading and TWD are consistent across both captures:
- The offsets are correct for those data types
- Difference should be within ~50° (boat at anchor pointing into wind)

Expected relationships:
- TWD capture: TWD ~69-73°, so heading should be ~20-120°
- Original capture: Need to check what values make sense
""")