venus/axiom-nmea/debug/pressure_finder.py

#!/usr/bin/env python3
"""
Pressure Finder - Locate barometric pressure data in Raymarine protobuf stream.

Scans for float values that could be pressure in various units.
Uses known pressure value to correlate field locations.

Usage:
    python pressure_finder.py -i YOUR_INTERFACE_IP -p 1021  # Known pressure in mbar
"""

import struct
import socket
import time
import argparse
from typing import Dict, List, Any, Optional
from collections import defaultdict

WIRE_VARINT = 0
WIRE_FIXED64 = 1
WIRE_LENGTH = 2
WIRE_FIXED32 = 5

HEADER_SIZE = 20

MULTICAST_GROUPS = [
    ("226.192.206.102", 2565),  # Main sensor data
    ("239.2.1.1", 2154),        # May contain additional data
]


class ProtobufParser:
    """Parse protobuf without schema."""

    def __init__(self, data: bytes):
        self.data = data
        self.pos = 0

    def remaining(self):
        return len(self.data) - self.pos

    def read_varint(self) -> int:
        result = 0
        shift = 0
        while self.pos < len(self.data):
            byte = self.data[self.pos]
            self.pos += 1
            result |= (byte & 0x7F) << shift
            if not (byte & 0x80):
                break
            shift += 7
        return result

    def parse_nested_deep(self, data: bytes, path: str = "", depth: int = 0, max_depth: int = 5) -> List[tuple]:
        """Recursively parse nested protobuf and return list of (path, type, value) tuples."""
        results = []
        pos = 0

        if depth > max_depth:
            return results

        while pos < len(data):
            if pos >= len(data):
                break
            try:
                # Read tag
                tag_byte = data[pos]
                pos += 1

                # Handle multi-byte varints for tag
                tag = tag_byte & 0x7F
                shift = 7
                while tag_byte & 0x80 and pos < len(data):
                    tag_byte = data[pos]
                    pos += 1
                    tag |= (tag_byte & 0x7F) << shift
                    shift += 7

                field_num = tag >> 3
                wire_type = tag & 0x07

                if field_num == 0 or field_num > 100:
                    break

                field_path = f"{path}.{field_num}" if path else str(field_num)

                if wire_type == WIRE_VARINT:
                    val = 0
                    shift = 0
                    while pos < len(data):
                        byte = data[pos]
                        pos += 1
                        val |= (byte & 0x7F) << shift
                        if not (byte & 0x80):
                            break
                        shift += 7
                    results.append((field_path, 'varint', val))

                elif wire_type == WIRE_FIXED32:
                    raw = data[pos:pos + 4]
                    pos += 4
                    try:
                        f = struct.unpack('<f', raw)[0]
                        if f == f:  # not NaN
                            results.append((field_path, 'float', f))
                    except:
                        pass

                elif wire_type == WIRE_FIXED64:
                    raw = data[pos:pos + 8]
                    pos += 8
                    try:
                        d = struct.unpack('<d', raw)[0]
                        if d == d:  # not NaN
                            results.append((field_path, 'double', d))
                    except:
                        pass

                elif wire_type == WIRE_LENGTH:
                    length = 0
                    shift = 0
                    while pos < len(data):
                        byte = data[pos]
                        pos += 1
                        length |= (byte & 0x7F) << shift
                        if not (byte & 0x80):
                            break
                        shift += 7
                    raw = data[pos:pos + length]
                    pos += length

                    # Try to parse as nested message
                    if len(raw) >= 2:
                        nested_results = self.parse_nested_deep(raw, field_path, depth + 1, max_depth)
                        if nested_results:
                            results.extend(nested_results)
                else:
                    break

            except Exception:
                break

        return results


def is_pressure_like(val: float, target_mbar: float) -> Optional[str]:
    """Check if a float value could be barometric pressure.

    Checks multiple unit possibilities and returns match description.
    """
    tolerance = 0.02  # 2% tolerance

    # Convert target to various units
    target_pa = target_mbar * 100      # Pascals (1021 mbar = 102100 Pa)
    target_hpa = target_mbar           # hPa = mbar
    target_kpa = target_mbar / 10      # kPa (102.1)
    target_bar = target_mbar / 1000    # bar (1.021)
    target_inhg = target_mbar * 0.02953  # inHg (~30.15)
    target_psi = target_mbar * 0.0145  # PSI (~14.8)

    checks = [
        (target_mbar, "mbar (direct)"),
        (target_hpa, "hPa"),
        (target_pa, "Pascals"),
        (target_kpa, "kPa"),
        (target_bar, "bar"),
        (target_inhg, "inHg"),
        (target_psi, "PSI"),
    ]

    for target, unit in checks:
        if target > 0 and abs(val - target) / target < tolerance:
            return unit

    return None


def scan_packet(data: bytes, target_mbar: float, group: str, port: int,
                candidates: Dict[str, Dict]) -> None:
    """Scan packet for pressure-like values."""
    if len(data) < HEADER_SIZE + 5:
        return

    proto_data = data[HEADER_SIZE:]
    parser = ProtobufParser(proto_data)

    # Get all fields recursively
    all_fields = parser.parse_all_fields() if hasattr(parser, 'parse_all_fields') else {}

    # Deep parse all length-delimited fields
    parser = ProtobufParser(proto_data)
    parser.pos = 0
    all_results = parser.parse_nested_deep(proto_data, "")

    for path, vtype, value in all_results:
        if vtype in ('float', 'double'):
            unit = is_pressure_like(value, target_mbar)
            if unit:
                if path not in candidates:
                    candidates[path] = {
                        'values': [],
                        'unit': unit,
                        'type': vtype,
                        'count': 0
                    }
                candidates[path]['values'].append(value)
                candidates[path]['count'] += 1


def parse_all_fields(self) -> Dict[int, List[Any]]:
    """Parse and collect all fields."""
    fields = {}

    while self.pos < len(self.data):
        if self.remaining() < 1:
            break
        try:
            tag = self.read_varint()
            field_num = tag >> 3
            wire_type = tag & 0x07

            if field_num == 0 or field_num > 1000:
                break

            if wire_type == WIRE_VARINT:
                value = ('varint', self.read_varint())
            elif wire_type == WIRE_FIXED64:
                raw = self.data[self.pos:self.pos + 8]
                self.pos += 8
                try:
                    d = struct.unpack('<d', raw)[0]
                    value = ('double', d)
                except:
                    value = ('fixed64', raw.hex())
            elif wire_type == WIRE_LENGTH:
                length = self.read_varint()
                raw = self.data[self.pos:self.pos + length]
                self.pos += length
                value = ('length', raw)
            elif wire_type == WIRE_FIXED32:
                raw = self.data[self.pos:self.pos + 4]
                self.pos += 4
                try:
                    f = struct.unpack('<f', raw)[0]
                    value = ('float', f)
                except:
                    value = ('fixed32', raw.hex())
            else:
                break

            if field_num not in fields:
                fields[field_num] = []
            fields[field_num].append(value)

        except:
            break

    return fields


ProtobufParser.parse_all_fields = parse_all_fields


def main():
    parser = argparse.ArgumentParser(description="Find barometric pressure field in Raymarine data")
    parser.add_argument('-i', '--interface', required=True, help='Interface IP')
    parser.add_argument('-p', '--pressure', type=float, required=True,
                        help='Known current pressure in mbar (e.g., 1021)')
    parser.add_argument('-t', '--time', type=int, default=10, help='Capture time (seconds)')
    parser.add_argument('-g', '--group', default="226.192.206.102", help='Multicast group')
    parser.add_argument('--port', type=int, default=2565, help='UDP Port')
    args = parser.parse_args()

    print(f"Pressure Finder - Looking for {args.pressure} mbar")
    print("=" * 60)
    print(f"Target values to find:")
    print(f"  mbar/hPa: {args.pressure:.1f}")
    print(f"  Pascals:  {args.pressure * 100:.0f}")
    print(f"  kPa:      {args.pressure / 10:.2f}")
    print(f"  inHg:     {args.pressure * 0.02953:.2f}")
    print(f"  bar:      {args.pressure / 1000:.4f}")
    print("=" * 60)
    print(f"\nListening on {args.group}:{args.port} for {args.time} seconds...")

    # Create socket
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
    sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    if hasattr(socket, 'SO_REUSEPORT'):
        sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
    sock.bind(('', args.port))
    mreq = struct.pack("4s4s", socket.inet_aton(args.group), socket.inet_aton(args.interface))
    sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
    sock.settimeout(1.0)

    candidates = {}  # field_path -> info
    packet_count = 0
    end_time = time.time() + args.time

    try:
        while time.time() < end_time:
            try:
                data, _ = sock.recvfrom(65535)
                packet_count += 1
                scan_packet(data, args.pressure, args.group, args.port, candidates)
            except socket.timeout:
                continue
    except KeyboardInterrupt:
        pass
    finally:
        sock.close()

    print(f"\n\nResults after scanning {packet_count} packets:")
    print("=" * 60)

    if not candidates:
        print("No pressure-like values found!")
        print("\nSuggestions:")
        print("1. Verify the pressure sensor is connected and broadcasting")
        print("2. Try different multicast groups (239.2.1.1:2154)")
        print("3. Check if pressure is in a different packet size")
    else:
        print(f"\nFound {len(candidates)} candidate field(s):\n")

        # Sort by count (most frequent first)
        for path, info in sorted(candidates.items(), key=lambda x: -x[1]['count']):
            values = info['values']
            avg_val = sum(values) / len(values)
            min_val = min(values)
            max_val = max(values)

            print(f"Field {path}:")
            print(f"  Type: {info['type']}")
            print(f"  Matches: {info['count']} packets")
            print(f"  Unit likely: {info['unit']}")
            print(f"  Values: min={min_val:.2f}, max={max_val:.2f}, avg={avg_val:.2f}")
            if info['unit'] == 'Pascals':
                print(f"  As mbar: {avg_val/100:.1f} mbar")
            elif info['unit'] == 'kPa':
                print(f"  As mbar: {avg_val*10:.1f} mbar")
            print()

    print("\nDone.")


if __name__ == "__main__":
    main()