gps-denied-onboard/e2e/fixtures/sitl_replay_builder/build_p02_fixtures.py

"""FT-P-02 Derkachi fixture builder (AZ-599).

Drives the production ``gps-denied-replay`` CLI against the recorded
Derkachi MP4 + a tlog converted from ``data_imu.csv``, producing an
FDR archive consumable by the FT-P-02 scenario (it walks the FDR via
``fdr_reader.iter_records`` and computes drift between satellite
anchors).

Differences from the b78 FT-P-01 builder (`build_p01_fixtures.py`):

* Video is already MP4 — no encoding step.
* IMU is real recorded telemetry — needs CSV → tlog conversion with
  real motion data (vs. b78's synthetic stationary tlog).
* Output is the SUT's natural FDR archive directory — no per-call
  schema projection.

Shared helpers (`run_gps_denied_replay`, `write_observer_fixture`)
live in `_common.py`.
"""

from __future__ import annotations

import argparse
import csv
import json
import logging
import math
import subprocess
import sys
from dataclasses import dataclass
from pathlib import Path
from typing import Callable, Iterator, Sequence

from e2e.fixtures.sitl_replay_builder._common import (
    DEFAULT_CLI_BIN,
    run_gps_denied_replay,
    write_observer_fixture,
)

_LOG = logging.getLogger(__name__)

REQUIRED_IMU_COLUMNS = (
    "timestamp(ms)",
    "SCALED_IMU2.xacc",
    "SCALED_IMU2.yacc",
    "SCALED_IMU2.zacc",
    "SCALED_IMU2.xgyro",
    "SCALED_IMU2.ygyro",
    "SCALED_IMU2.zgyro",
    "GLOBAL_POSITION_INT.hdg",
)


@dataclass(frozen=True)
class P02BuilderConfig:
    """Per-invocation Derkachi builder configuration."""

    derkachi_dir: Path
    output_dir: Path
    fc_kind: str = "ardupilot"
    host: str = "sitl-host"
    cli_bin: str = DEFAULT_CLI_BIN


# Step 1 — convert IMU CSV to tlog


def convert_imu_csv_to_tlog(
    csv_path: Path,
    output_tlog: Path,
    *,
    _mavlink_writer_factory: Callable | None = None,
) -> int:
    """Read `csv_path`, write one RAW_IMU + one ATTITUDE pair per row.

    The Derkachi CSV ships at 10 Hz with ``SCALED_IMU2.*`` accelerometer
    + gyro fields and ``GLOBAL_POSITION_INT.hdg`` heading in
    centidegrees. We pack RAW_IMU from the IMU columns (pass-through;
    units may need conversion if the SUT's tlog parser rejects), and
    synthesise ATTITUDE with yaw = `hdg_cdeg * pi / 18000` and
    roll/pitch = 0 — acceptable for fixed-wing cruise.

    Returns the number of pairs written.

    Raises:
        FileNotFoundError: `csv_path` missing.
        ValueError: empty CSV, missing required column, OR malformed
            numeric row.
    """
    if not csv_path.is_file():
        raise FileNotFoundError(f"IMU CSV not found: {csv_path}")

    rows = list(_iter_imu_rows(csv_path))
    if not rows:
        raise ValueError(f"IMU CSV is empty: {csv_path}")

    if _mavlink_writer_factory is None:
        from pymavlink import mavutil

        def _mavlink_writer_factory(out: Path):
            return mavutil.mavlogfile(str(out), write=True)

    output_tlog.parent.mkdir(parents=True, exist_ok=True)
    pairs = 0
    writer = _mavlink_writer_factory(output_tlog)
    try:
        for row in rows:
            try:
                ts_ms = float(row["timestamp(ms)"])
                xacc = int(float(row["SCALED_IMU2.xacc"]))
                yacc = int(float(row["SCALED_IMU2.yacc"]))
                zacc = int(float(row["SCALED_IMU2.zacc"]))
                xgyro = int(float(row["SCALED_IMU2.xgyro"]))
                ygyro = int(float(row["SCALED_IMU2.ygyro"]))
                zgyro = int(float(row["SCALED_IMU2.zgyro"]))
                hdg_cdeg = float(row["GLOBAL_POSITION_INT.hdg"])
            except (ValueError, KeyError) as exc:
                raise ValueError(
                    f"malformed IMU CSV row at {csv_path} row#{pairs + 1}: {exc}"
                ) from exc
            yaw_rad = _hdg_centideg_to_rad(hdg_cdeg)
            writer.write(_pack_raw_imu(int(ts_ms * 1000), xacc, yacc, zacc, xgyro, ygyro, zgyro))
            writer.write(_pack_attitude(int(ts_ms), yaw_rad))
            pairs += 1
    finally:
        close = getattr(writer, "close", None)
        if callable(close):
            close()

    return pairs


def _iter_imu_rows(csv_path: Path) -> Iterator[dict[str, str]]:
    """Yield CSV rows; validates required columns are present in the header."""
    with csv_path.open("r", newline="", encoding="utf-8") as fp:
        reader = csv.DictReader(fp)
        if reader.fieldnames is None:
            raise ValueError(f"IMU CSV missing header: {csv_path}")
        missing = [col for col in REQUIRED_IMU_COLUMNS if col not in reader.fieldnames]
        if missing:
            raise ValueError(
                f"IMU CSV {csv_path} missing required columns: {missing}"
            )
        yield from reader


def _hdg_centideg_to_rad(hdg_cdeg: float) -> float:
    """Convert centidegrees [0, 36000) to radians [0, 2pi)."""
    return (hdg_cdeg * math.pi) / 18000.0


def _pack_raw_imu(time_usec: int, xacc: int, yacc: int, zacc: int,
                  xgyro: int, ygyro: int, zgyro: int) -> bytes:
    """Pack a RAW_IMU MAVLink frame (msg id 27) with real motion data."""
    from pymavlink.dialects.v20 import ardupilotmega as mavlink

    packer = mavlink.MAVLink(file=None, srcSystem=1, srcComponent=1)
    msg = mavlink.MAVLink_raw_imu_message(
        time_usec=time_usec,
        xacc=xacc, yacc=yacc, zacc=zacc,
        xgyro=xgyro, ygyro=ygyro, zgyro=zgyro,
        xmag=0, ymag=0, zmag=0,
        id=0, temperature=0,
    )
    return msg.pack(packer)


def _pack_attitude(time_boot_ms: int, yaw_rad: float) -> bytes:
    """Pack an ATTITUDE MAVLink frame (msg id 30) with synthesised yaw."""
    from pymavlink.dialects.v20 import ardupilotmega as mavlink

    packer = mavlink.MAVLink(file=None, srcSystem=1, srcComponent=1)
    msg = mavlink.MAVLink_attitude_message(
        time_boot_ms=time_boot_ms,
        roll=0.0, pitch=0.0, yaw=float(yaw_rad),
        rollspeed=0.0, pitchspeed=0.0, yawspeed=0.0,
    )
    return msg.pack(packer)


# Step 2 — verify the FDR archive has at least one estimate record


def verify_fdr_has_estimates(fdr_path: Path) -> int:
    """Return the count of `record_type=="estimate"` records in `fdr_path`.

    Raises ``ValueError`` if the file has zero such records — that
    means the replay produced nothing useful for FT-P-02 to analyze.
    Tolerates missing fields per record (only `record_type` is required
    for filtering).
    """
    if not fdr_path.is_file():
        raise FileNotFoundError(f"FDR JSONL not found: {fdr_path}")

    count = 0
    with fdr_path.open("r", encoding="utf-8") as fp:
        for line in fp:
            line = line.strip()
            if not line:
                continue
            try:
                record = json.loads(line)
            except json.JSONDecodeError:
                continue
            if record.get("record_type") == "estimate":
                count += 1

    if count == 0:
        raise ValueError(
            f"FDR archive {fdr_path} contains zero estimate records; "
            f"the replay did not produce any outbound estimates for FT-P-02 to analyze"
        )
    return count


# Orchestration


def build_p02_fixtures(
    cfg: P02BuilderConfig,
    *,
    _runner: Callable[[Sequence[str]], subprocess.CompletedProcess] | None = None,
    _mavlink_writer_factory: Callable | None = None,
    _verify_fdr: Callable[[Path], int] | None = None,
) -> Path:
    """End-to-end FT-P-02 fixture build. Returns the output directory.

    Steps:

    1. Resolve the Derkachi MP4 + IMU CSV under ``cfg.derkachi_dir``.
    2. Convert IMU CSV to ``derkachi.tlog`` under ``cfg.output_dir``.
    3. Run ``gps-denied-replay`` against the MP4 + tlog; write FDR JSONL
       at ``<output_dir>/fdr/fdr.jsonl``.
    4. Verify the FDR archive contains ≥1 estimate record.
    5. Write the companion ``observer_<fc_kind>_<host>.json``.
    """
    mp4 = cfg.derkachi_dir / "flight_derkachi.mp4"
    csv_path = cfg.derkachi_dir / "data_imu.csv"
    if not mp4.is_file():
        raise FileNotFoundError(f"Derkachi MP4 not found: {mp4}")
    if not csv_path.is_file():
        raise FileNotFoundError(f"Derkachi IMU CSV not found: {csv_path}")

    cfg.output_dir.mkdir(parents=True, exist_ok=True)
    tlog = cfg.output_dir / "derkachi.tlog"
    fdr_dir = cfg.output_dir / "fdr"
    fdr_jsonl = fdr_dir / "fdr.jsonl"

    convert_imu_csv_to_tlog(
        csv_path, tlog, _mavlink_writer_factory=_mavlink_writer_factory,
    )
    run_gps_denied_replay(
        mp4, tlog, fdr_jsonl, cli_bin=cfg.cli_bin, _runner=_runner,
    )
    verifier = _verify_fdr or verify_fdr_has_estimates
    estimate_count = verifier(fdr_jsonl)
    _LOG.info("FT-P-02 FDR archive contains %d estimate records", estimate_count)

    observer_path = cfg.output_dir / f"observer_{cfg.fc_kind}_{cfg.host}.json"
    write_observer_fixture(observer_path)
    return cfg.output_dir


def _main(argv: Sequence[str] | None = None) -> int:
    parser = argparse.ArgumentParser(
        prog="build_p02_fixtures",
        description="Build FT-P-02 Derkachi replay fixtures via gps-denied-replay.",
    )
    parser.add_argument("--derkachi-dir", type=Path, required=True,
                        help="Directory containing flight_derkachi.mp4 + data_imu.csv")
    parser.add_argument("--output-dir", type=Path, required=True,
                        help="Output dir for derkachi.tlog + fdr/ archive + observer fixture")
    parser.add_argument("--fc-kind", choices=("ardupilot", "inav"), default="ardupilot")
    parser.add_argument("--host", default="sitl-host")
    parser.add_argument("--cli-bin", default=DEFAULT_CLI_BIN)
    args = parser.parse_args(argv)

    logging.basicConfig(level=logging.INFO)
    cfg = P02BuilderConfig(
        derkachi_dir=args.derkachi_dir,
        output_dir=args.output_dir,
        fc_kind=args.fc_kind,
        host=args.host,
        cli_bin=args.cli_bin,
    )
    build_p02_fixtures(cfg)
    return 0


if __name__ == "__main__":  # pragma: no cover
    sys.exit(_main())