venus/axiom-nmea/debug/battery_finder.py

#!/usr/bin/env python3
"""
Battery Finder - Scan all multicast groups for values matching expected battery voltages.

Searches for house battery voltages (26.3V and 27.2V) across all protobuf fields.
"""

import struct
import socket
import time
import threading
from typing import Dict, Any, Optional, List, Tuple

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

HEADER_SIZE = 20

MULTICAST_GROUPS = [
    ("226.192.206.98", 2561),
    ("226.192.206.99", 2562),
    ("226.192.206.100", 2563),
    ("226.192.206.101", 2564),
    ("226.192.206.102", 2565),
    ("226.192.219.0", 3221),
    ("239.2.1.1", 2154),
]

# Target voltage values to find (with tolerance)
# Aft House: 26.3V, Stern House: 27.2V
TARGET_VOLTAGES = [
    (26.0, 26.6),    # Aft house battery ~26.3V
    (26.9, 27.5),    # Stern house battery ~27.2V
    (12.0, 15.0),    # 12V battery range (in case they're 12V systems)
    (24.0, 29.0),    # General 24V battery range
]


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

    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(self, path: str = "") -> List[Tuple[str, str, Any]]:
        """Parse and return list of (path, type, value) for all fields."""
        results = []
        while self.pos < len(self.data):
            try:
                tag = self.read_varint()
                field_num = tag >> 3
                wire_type = tag & 0x07

                if field_num == 0 or field_num > 1000:
                    break

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

                if wire_type == WIRE_VARINT:
                    value = self.read_varint()
                    results.append((field_path, "varint", value))
                elif wire_type == WIRE_FIXED64:
                    raw = self.data[self.pos:self.pos + 8]
                    self.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 = self.read_varint()
                    raw = self.data[self.pos:self.pos + length]
                    self.pos += length
                    # Try to parse as nested
                    try:
                        nested = ProtobufParser(raw)
                        nested_results = nested.parse(field_path)
                        if nested_results:
                            results.extend(nested_results)
                    except:
                        pass
                elif wire_type == WIRE_FIXED32:
                    raw = self.data[self.pos:self.pos + 4]
                    self.pos += 4
                    try:
                        f = struct.unpack('<f', raw)[0]
                        if f == f:  # not NaN
                            results.append((field_path, "float", f))
                    except:
                        pass
                else:
                    break
            except:
                break
        return results


def is_voltage_value(val: float) -> Optional[str]:
    """Check if value matches expected battery voltage ranges."""
    if 26.0 <= val <= 26.6:
        return "Aft House (~26.3V)"
    if 26.9 <= val <= 27.5:
        return "Stern House (~27.2V)"
    if 12.0 <= val <= 15.0:
        return "12V battery range"
    if 24.0 <= val <= 29.0:
        return "24V battery range"
    return None


def scan_packet(data: bytes, group: str, port: int, verbose: bool = False):
    """Scan a packet for voltage values."""
    if len(data) < HEADER_SIZE + 5:
        return

    proto_data = data[HEADER_SIZE:]
    parser = ProtobufParser(proto_data)
    fields = parser.parse()

    matches = []
    for path, vtype, value in fields:
        if isinstance(value, (int, float)):
            match_desc = is_voltage_value(value)
            if match_desc:
                matches.append((path, vtype, value, match_desc))

    if matches:
        print(f"\n{'='*70}")
        print(f"VOLTAGE MATCH on {group}:{port} (packet size: {len(data)})")
        print(f"{'='*70}")
        for path, vtype, value, desc in matches:
            print(f"  Field {path} ({vtype}): {value:.2f}V  <- {desc}")

        # Show context: look at parent field structure
        if verbose:
            print(f"\nAll numeric values in packet:")
            for path, vtype, value in fields:
                if isinstance(value, (int, float)) and value != 0:
                    print(f"  {path}: {value} ({vtype})")


class MulticastScanner:
    def __init__(self, interface_ip: str, verbose: bool = False):
        self.interface_ip = interface_ip
        self.verbose = verbose
        self.running = False
        self.lock = threading.Lock()

    def _create_socket(self, group: str, port: int):
        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(('', port))
        mreq = struct.pack("4s4s", socket.inet_aton(group), socket.inet_aton(self.interface_ip))
        sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
        sock.settimeout(1.0)
        return sock

    def _listen(self, sock, group: str, port: int):
        seen_sizes = set()
        while self.running:
            try:
                data, _ = sock.recvfrom(65535)
                # Only process each unique packet size once per group
                size_key = len(data)
                if size_key not in seen_sizes:
                    seen_sizes.add(size_key)
                    with self.lock:
                        scan_packet(data, group, port, self.verbose)
            except socket.timeout:
                continue
            except:
                pass

    def start(self):
        self.running = True
        threads = []
        for group, port in MULTICAST_GROUPS:
            try:
                sock = self._create_socket(group, port)
                t = threading.Thread(target=self._listen, args=(sock, group, port), daemon=True)
                t.start()
                threads.append(t)
                print(f"Scanning {group}:{port}")
            except Exception as e:
                print(f"Error on {group}:{port}: {e}")
        return threads

    def stop(self):
        self.running = False


def main():
    import argparse
    parser = argparse.ArgumentParser(description="Find battery voltage values in multicast data")
    parser.add_argument('-i', '--interface', required=True, help='Interface IP')
    parser.add_argument('-t', '--time', type=int, default=10, help='Scan duration (seconds)')
    parser.add_argument('-v', '--verbose', action='store_true', help='Show all numeric values')
    args = parser.parse_args()

    print(f"Scanning for battery voltages:")
    print(f"  - Aft House: ~26.3V (range 26.0-26.6)")
    print(f"  - Stern House: ~27.2V (range 26.9-27.5)")
    print(f"  - Also checking 12V and 24V ranges")
    print(f"\nWill scan for {args.time} seconds\n")

    scanner = MulticastScanner(args.interface, args.verbose)
    scanner.start()

    try:
        time.sleep(args.time)
    except KeyboardInterrupt:
        pass
    finally:
        scanner.stop()
        print("\nDone scanning")


if __name__ == "__main__":
    main()