venus/axiom-nmea/debug/tank_finder.py

#!/usr/bin/env python3
"""
Tank Finder - Scan all multicast groups for values matching expected tank levels.
"""

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 values to find (tank levels)
TARGET_VALUES = [
    (66, 70),    # ~68% fuel tank
    (87, 91),    # ~89% fuel tank
    (0.66, 0.70),  # Decimal range
    (0.87, 0.91),  # Decimal 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_target_value(val: float) -> bool:
    """Check if value matches our target ranges."""
    for low, high in TARGET_VALUES:
        if low <= val <= high:
            return True
    return False


def scan_packet(data: bytes, group: str, port: int):
    """Scan a packet for target 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)) and is_target_value(value):
            matches.append((path, vtype, value))

    if matches:
        print(f"\n{'='*60}")
        print(f"MATCH on {group}:{port} (packet size: {len(data)})")
        print(f"{'='*60}")
        for path, vtype, value in matches:
            print(f"  Field {path} ({vtype}): {value}")

        # Show all Field 16 entries (tank data) for context
        print(f"\nAll Field 16 (Tank) entries:")
        tank_entries = {}
        for path, vtype, value in fields:
            if path.startswith("16."):
                parts = path.split(".")
                if len(parts) >= 2:
                    # Group by the implicit index (based on order seen)
                    entry_key = path  # We'll group differently
                    tank_entries[path] = (vtype, value)

        # Parse Field 16 entries properly - group consecutive 16.x fields
        current_tank = {}
        tank_list = []
        last_field = 0
        for path, vtype, value in fields:
            if path.startswith("16."):
                subfield = int(path.split(".")[1])
                # If we see a field number <= last, it's a new tank entry
                if subfield <= last_field and current_tank:
                    tank_list.append(current_tank)
                    current_tank = {}
                current_tank[subfield] = (vtype, value)
                last_field = subfield
        if current_tank:
            tank_list.append(current_tank)

        for i, tank in enumerate(tank_list):
            tank_id = tank.get(1, (None, "?"))[1]
            status = tank.get(2, (None, "?"))[1]
            level = tank.get(3, (None, "?"))[1]
            print(f"  Tank #{tank_id}: level={level}%, status={status}")


class MulticastScanner:
    def __init__(self, interface_ip: str):
        self.interface_ip = interface_ip
        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)
            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 tank level 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)')
    args = parser.parse_args()

    print(f"Scanning for values around 37-40 (or 0.37-0.40)...")
    print(f"Will scan for {args.time} seconds\n")

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

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


if __name__ == "__main__":
    main()