venus/axiom-nmea/debug/wind_finder.py

#!/usr/bin/env python3
"""
Diagnostic tool to find wind speed and direction values in Raymarine packets.
Searches for float values matching expected ranges.
"""

import struct
import sys
from collections import defaultdict

# Expected values
# Wind speed: 15-20 kts = 7.7-10.3 m/s
# Wind direction: 60-90 degrees = 1.05-1.57 radians

EXPECTED_SPEED_MS_MIN = 7.0
EXPECTED_SPEED_MS_MAX = 12.0
EXPECTED_DIR_RAD_MIN = 1.0
EXPECTED_DIR_RAD_MAX = 1.7

PCAP_MAGIC = 0xa1b2c3d4

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

def decode_double(data, offset):
    if offset + 8 > len(data):
        return None
    try:
        return struct.unpack('<d', data[offset:offset+8])[0]
    except:
        return None

def read_pcap(filename):
    """Read packets from pcap file."""
    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

            # Extract UDP payload
            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

def find_wind_candidates(packets):
    """Find all float values that could be wind speed or direction."""

    speed_candidates = defaultdict(list)  # offset -> list of values
    dir_candidates = defaultdict(list)

    for pkt_idx, data in enumerate(packets):
        if len(data) < 100:
            continue

        # Search for 32-bit floats
        for offset in range(0x30, min(len(data) - 4, 0x300)):
            val = decode_float(data, offset)
            if val is None or val != val:  # NaN check
                continue

            # Check for wind speed range (m/s)
            if EXPECTED_SPEED_MS_MIN <= val <= EXPECTED_SPEED_MS_MAX:
                speed_candidates[offset].append((pkt_idx, val, len(data)))

            # Check for direction range (radians)
            if EXPECTED_DIR_RAD_MIN <= val <= EXPECTED_DIR_RAD_MAX:
                dir_candidates[offset].append((pkt_idx, val, len(data)))

    return speed_candidates, dir_candidates

def main():
    filename = sys.argv[1] if len(sys.argv) > 1 else "raymarine_sample.pcap"

    print(f"Reading {filename}...")
    packets = read_pcap(filename)
    print(f"Loaded {len(packets)} packets\n")

    print(f"Searching for wind speed values ({EXPECTED_SPEED_MS_MIN}-{EXPECTED_SPEED_MS_MAX} m/s)")
    print(f"Searching for wind direction values ({EXPECTED_DIR_RAD_MIN}-{EXPECTED_DIR_RAD_MAX} rad)\n")

    speed_candidates, dir_candidates = find_wind_candidates(packets)

    print("=" * 70)
    print("WIND SPEED CANDIDATES (m/s)")
    print("=" * 70)

    # Sort by number of occurrences
    for offset in sorted(speed_candidates.keys(), key=lambda x: -len(speed_candidates[x]))[:15]:
        hits = speed_candidates[offset]
        values = [v for _, v, _ in hits]
        pkt_sizes = set(s for _, _, s in hits)
        avg_val = sum(values) / len(values)
        avg_kts = avg_val * 1.94384
        print(f"  Offset 0x{offset:04x}: {len(hits):4d} hits, avg {avg_val:.2f} m/s ({avg_kts:.1f} kts), sizes: {sorted(pkt_sizes)[:5]}")

    print("\n" + "=" * 70)
    print("WIND DIRECTION CANDIDATES (radians)")
    print("=" * 70)

    for offset in sorted(dir_candidates.keys(), key=lambda x: -len(dir_candidates[x]))[:15]:
        hits = dir_candidates[offset]
        values = [v for _, v, _ in hits]
        pkt_sizes = set(s for _, _, s in hits)
        avg_val = sum(values) / len(values)
        avg_deg = avg_val * 57.2958
        print(f"  Offset 0x{offset:04x}: {len(hits):4d} hits, avg {avg_val:.2f} rad ({avg_deg:.1f}°), sizes: {sorted(pkt_sizes)[:5]}")

    # Look for paired speed+direction at consecutive offsets
    print("\n" + "=" * 70)
    print("SPEED+DIRECTION PAIRS (4 bytes apart)")
    print("=" * 70)

    for speed_offset in speed_candidates:
        dir_offset = speed_offset + 4  # Next float
        if dir_offset in dir_candidates:
            speed_hits = len(speed_candidates[speed_offset])
            dir_hits = len(dir_candidates[dir_offset])
            if speed_hits > 5 and dir_hits > 5:
                speed_vals = [v for _, v, _ in speed_candidates[speed_offset]]
                dir_vals = [v for _, v, _ in dir_candidates[dir_offset]]
                avg_speed = sum(speed_vals) / len(speed_vals) * 1.94384
                avg_dir = sum(dir_vals) / len(dir_vals) * 57.2958
                print(f"  Speed @ 0x{speed_offset:04x} ({speed_hits} hits), Dir @ 0x{dir_offset:04x} ({dir_hits} hits)")
                print(f"    -> Avg: {avg_speed:.1f} kts @ {avg_dir:.1f}°")

if __name__ == "__main__":
    main()