BMW_E8x_EPS/gateway/eps_control.py

"""Drive the EPS with a minimal, hand-controlled message set.

Where eps_bench.py replays whole captured sets to find out what's needed,
this assumes the answer is already known - CAS terminal status (0x130) to
bring the unit up, DSC road speed (0x1A0) to set how much assist it gives -
and gives you direct control of those two signals.

Encoding strategy, and why it differs per message:

  0x130 terminal: the capture contains real frames for each terminal state
    (off / R / 15 / running / cranking), so a state change just switches
    which captured sequence is cycling. Counters and checksums stay exactly
    as the car produced them - nothing to solve.

  0x1A0 road speed: the reference capture is stationary throughout, so
    there is no ground truth for a moving-speed frame. What we do know is
    that the best-fitting checksum over that data covers bytes 2..6 (87.5%
    of frames, tools/solve_checksum.py) - which excludes the speed field in
    bytes 0..1. So patching speed into a captured frame should leave its
    checksum valid. That is an inference, not a verified fact: watch the
    EPS response monitor when you move the slider, and treat a drop-out as
    the encoding being rejected.
"""
from __future__ import annotations

import csv
import threading
import time
from dataclasses import dataclass, field
from pathlib import Path
from typing import Optional

CAN_ID_TERMINAL = 0x130
CAN_ID_ROAD_SPEED = 0x1A0

TERMINAL_STATES = {
    "off": 0x00,
    "terminal R": 0x40,
    "ignition (KL15)": 0x41,
    "engine running": 0x45,
    "cranking": 0x55,
}

# Periods the car uses for these two messages (median from the captures).
PERIOD_TERMINAL = 0.1
PERIOD_ROAD_SPEED = 0.02

SPEED_SCALE = 0.1  # km/h per bit, per files/PTCAN_protocol.md
STANDSTILL_FLAG = 0x80  # b1 bit7, set while the car reports stationary


def load_state_sequences(path: Path, arbitration_id: int, state_byte: int = 0) -> dict[int, list[bytes]]:
    """Group a capture's frames for one ID by the value of one byte.

    Used to pull genuine per-terminal-state 0x130 frames out of a recording,
    so switching state means switching which real sequence we cycle rather
    than synthesising a payload whose checksum we can't verify.
    """
    groups: dict[int, list[bytes]] = {}
    with open(path, newline="") as f:
        for row in csv.DictReader(f):
            if int(row["arbitration_id"], 16) != arbitration_id:
                continue
            data = bytes.fromhex(row["data_hex"])
            if len(data) <= state_byte:
                continue
            groups.setdefault(data[state_byte], []).append(data)
    return groups


def load_frames(path: Path, arbitration_id: int, limit: int = 400) -> list[bytes]:
    out = []
    with open(path, newline="") as f:
        for row in csv.DictReader(f):
            if int(row["arbitration_id"], 16) == arbitration_id:
                out.append(bytes.fromhex(row["data_hex"]))
                if len(out) >= limit:
                    break
    return out


def encode_road_speed(template: bytes, kmh: float) -> bytes:
    """Patch a road-speed value into a captured 0x1A0 frame.

    Only bytes 0-1 are touched; see the module docstring for why that should
    leave the frame's checksum intact.
    """
    raw = max(0, min(0x0FFF, int(round(kmh / SPEED_SCALE))))
    out = bytearray(template)
    out[0] = raw & 0xFF
    flags = out[1] & 0xF0
    if kmh > 0:
        flags &= ~STANDSTILL_FLAG & 0xFF
    else:
        flags |= STANDSTILL_FLAG
    out[1] = flags | ((raw >> 8) & 0x0F)
    return bytes(out)


def decode_road_speed(data: bytes) -> float:
    return (data[0] | ((data[1] & 0x0F) << 8)) * SPEED_SCALE


def decode_eps_message(arbitration_id: int, data: bytes) -> dict:
    """Best-effort decode of the three messages the EPS transmits.

    Derived from byte-frequency analysis of bench captures (see
    eps-comms/findings.md). Fields marked "?" are inferred from structure
    rather than confirmed against a known-good reference, so treat them as
    leads rather than facts.
    """
    out: dict[str, str] = {}
    if not data:
        return out

    if arbitration_id == 0x1FB and len(data) >= 2:
        # b0 runs F0..FE: high nibble is a constant marker, low nibble the
        # 0..14 alive counter (skipping 15) used everywhere on this bus.
        out["alive counter"] = str(data[0] & 0x0F)
        out["marker"] = f"0x{data[0] >> 4:X}"
        if data[0] >> 4 != 0xF:
            out["note"] = "unexpected marker - normally 0xF"

    elif arbitration_id == 0x4B0 and len(data) >= 4:
        out["counter?"] = str(data[1] & 0x0F)
        flags = data[1] >> 4
        out["flags?"] = f"0x{flags:X}"
        out["checksum?"] = f"0x{data[0]:02X}"
        if data[3] != 0xFF:
            out["b3"] = f"0x{data[3]:02X} (normally FF)"

    elif arbitration_id == 0x5B0 and len(data) >= 4:
        # Two payloads seen: 01 03 80 FF... right at power-up, then
        # 40 81 01 15 FF... once it settles - looks like an init/ready pair.
        out["state?"] = "starting" if data[0] == 0x01 else "ready" if data[0] == 0x40 else f"0x{data[0]:02X}"
        out["raw"] = data[:4].hex(" ").upper()

    return out


@dataclass
class EpsRx:
    count: int = 0
    last_seen: float = 0.0
    last_data: bytes = b""
    changed_mask: int = 0
    history: list = field(default_factory=list)


class EpsController:
    """Transmits just the terminal + road-speed messages, on their own thread."""

    def __init__(self, adapter, capture: Path, log_path: Optional[Path] = None):
        self.adapter = adapter
        self.terminal_sequences = load_state_sequences(capture, CAN_ID_TERMINAL)
        self.speed_templates = load_frames(capture, CAN_ID_ROAD_SPEED)
        self.terminal_state = 0x00
        self.speed_kmh = 0.0
        self.send_speed = True
        self.rx: dict[int, EpsRx] = {}
        self.tx_count = 0

        self._lock = threading.Lock()
        self._stop = threading.Event()
        self._thread: Optional[threading.Thread] = None
        self._t0 = 0.0
        self._term_i = 0
        self._speed_i = 0
        self._log_file = None
        self._log_writer = None
        if log_path is not None:
            self._log_file = open(log_path, "w", newline="")
            self._log_writer = csv.writer(self._log_file)
            self._log_writer.writerow(["t", "wall", "dir", "arbitration_id", "dlc", "data_hex", "note"])

    def available_states(self) -> dict[str, int]:
        """Terminal states we actually hold captured frames for."""
        return {name: v for name, v in TERMINAL_STATES.items() if v in self.terminal_sequences}

    def set_terminal(self, value: int) -> None:
        with self._lock:
            self.terminal_state = value
            self._term_i = 0
        self._log_note(f"terminal -> 0x{value:02X}")

    def set_speed(self, kmh: float) -> None:
        with self._lock:
            self.speed_kmh = kmh
        self._log_note(f"speed -> {kmh:.1f} km/h")

    def _log_note(self, note: str) -> None:
        if self._log_writer is not None:
            self._log_writer.writerow([round(time.monotonic() - self._t0, 4), round(time.time(), 4),
                                       "note", "", "", "", note])

    def is_running(self) -> bool:
        return bool(self._thread and self._thread.is_alive())

    def start(self) -> None:
        if self.is_running():
            return
        self._stop.clear()
        self._t0 = time.monotonic()
        self._thread = threading.Thread(target=self._run, daemon=True)
        self._thread.start()

    def stop(self) -> None:
        self._stop.set()
        if self._thread:
            self._thread.join(timeout=2)
        if self._log_file is not None:
            self._log_file.close()
            self._log_file = None
            self._log_writer = None

    def snapshot(self):
        with self._lock:
            return dict(self.rx), self.tx_count

    def _run(self) -> None:
        next_term = next_speed = time.monotonic()
        while not self._stop.is_set():
            now = time.monotonic()
            batch = []

            if now >= next_term:
                next_term = now + PERIOD_TERMINAL
                with self._lock:
                    seq = self.terminal_sequences.get(self.terminal_state)
                    if seq:
                        data = seq[self._term_i % len(seq)]
                        self._term_i += 1
                        batch.append((CAN_ID_TERMINAL, data, False))

            if now >= next_speed:
                next_speed = now + PERIOD_ROAD_SPEED
                with self._lock:
                    if self.send_speed and self.speed_templates:
                        tmpl = self.speed_templates[self._speed_i % len(self.speed_templates)]
                        self._speed_i += 1
                        batch.append((CAN_ID_ROAD_SPEED, encode_road_speed(tmpl, self.speed_kmh), False))

            if batch:
                self.adapter.send_frames(batch)
                with self._lock:
                    self.tx_count += len(batch)
                if self._log_writer is not None:
                    wall = time.time()
                    for aid, data, _ in batch:
                        self._log_writer.writerow([round(now - self._t0, 4), round(wall, 4), "tx",
                                                   f"{aid:X}", len(data), data.hex().upper(), ""])

            deadline = min(next_term, next_speed)
            while time.monotonic() < deadline:
                frame = self.adapter.read_frame(timeout=0.001)
                if frame is None:
                    break
                self._record_rx(frame)
            slack = deadline - time.monotonic()
            if slack > 0:
                time.sleep(min(slack, 0.005))

    def _record_rx(self, frame) -> None:
        t = time.monotonic() - self._t0
        with self._lock:
            info = self.rx.get(frame.arbitration_id)
            if info is None:
                info = EpsRx()
                self.rx[frame.arbitration_id] = info
            if info.last_data and len(info.last_data) == len(frame.data):
                for i, (a, b) in enumerate(zip(info.last_data, frame.data)):
                    if a != b:
                        info.changed_mask |= 1 << i
            info.count += 1
            info.last_seen = t
            info.last_data = frame.data
            info.history.append((t, frame.data))
            if len(info.history) > 200:
                del info.history[:100]
        if self._log_writer is not None:
            self._log_writer.writerow([round(t, 4), round(time.time(), 4), "rx",
                                       f"{frame.arbitration_id:X}", len(frame.data),
                                       frame.data.hex().upper(), ""])