From 4e0717e54301e4dc0696bac2262314aef848c4a6 Mon Sep 17 00:00:00 2001 From: Oleksandr Bezdieniezhnykh Date: Sun, 17 May 2026 13:40:07 +0300 Subject: [PATCH] [AZ-599] Batch 79: FT-P-02 Derkachi builder + _common.py extraction MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add build_p02_fixtures.py: IMU CSV → tlog conversion (RAW_IMU + ATTITUDE pairs, centidegrees→radians yaw) and orchestrator that runs gps-denied replay against Derkachi MP4 + generated tlog, verifying ≥1 record_type="estimate" in the FDR archive. - Extract run_gps_denied_replay + FDR-parent-dir helpers into sitl_replay_builder/_common.py; refactor build_p01_fixtures.py to import from _common (b78 tests preserved). - Add 20 unit tests under e2e/_unit_tests/fixtures/test_sitl_ replay_builder_p02.py covering AC-1..AC-5; total unit suite 686/686 passing (regression gate AC-6). - README updated to document FT-P-01 + FT-P-02 builders. - Advance autodev state: last_completed_batch=79, current_batch=80; prune verbose detail blob. Co-authored-by: Cursor --- .../done/AZ-599_ft_p_02_derkachi_builder.md | 77 +++++ _docs/03_implementation/batch_79_report.md | 88 +++++ .../reviews/batch_79_review.md | 156 +++++++++ _docs/_autodev_state.md | 12 +- .../fixtures/test_sitl_replay_builder_p02.py | 324 ++++++++++++++++++ e2e/_unit_tests/test_directory_layout.py | 2 + e2e/fixtures/sitl_replay_builder/README.md | 87 ++++- e2e/fixtures/sitl_replay_builder/_common.py | 85 +++++ .../sitl_replay_builder/build_p01_fixtures.py | 65 +--- .../sitl_replay_builder/build_p02_fixtures.py | 291 ++++++++++++++++ 10 files changed, 1111 insertions(+), 76 deletions(-) create mode 100644 _docs/02_tasks/done/AZ-599_ft_p_02_derkachi_builder.md create mode 100644 _docs/03_implementation/batch_79_report.md create mode 100644 _docs/03_implementation/reviews/batch_79_review.md create mode 100644 e2e/_unit_tests/fixtures/test_sitl_replay_builder_p02.py create mode 100644 e2e/fixtures/sitl_replay_builder/_common.py create mode 100644 e2e/fixtures/sitl_replay_builder/build_p02_fixtures.py diff --git a/_docs/02_tasks/done/AZ-599_ft_p_02_derkachi_builder.md b/_docs/02_tasks/done/AZ-599_ft_p_02_derkachi_builder.md new file mode 100644 index 0000000..df93390 --- /dev/null +++ b/_docs/02_tasks/done/AZ-599_ft_p_02_derkachi_builder.md @@ -0,0 +1,77 @@ +# FT-P-02 Derkachi fixture builder (IMU CSV → tlog, real-motion replay) + +**Task**: AZ-599_ft_p_02_derkachi_builder +**Complexity**: 3 points +**Dependencies**: AZ-598 (shared `run_gps_denied_replay` helper) +**Component**: Blackbox Tests / Test Infrastructure (epic AZ-262) +**Tracker**: AZ-599 + +## Problem + +FT-P-02 (Derkachi continuous-flight drift) needs an offline-replay +FDR archive so the scenario can compute drift between satellite +anchors. The b78 FT-P-01 builder doesn't apply directly — FT-P-02 +uses real video + real IMU CSV instead of still images + synthetic +stationary tlog. + +## Strategy + +Same overall shape as b78 (drive `gps-denied-replay` against a video ++ tlog pair), with two differences: + +1. **Video is already MP4** — skip the OpenCV encoding step. +2. **IMU is real recorded telemetry** — `data_imu.csv` has 10 Hz + `SCALED_IMU2` + `GLOBAL_POSITION_INT` columns. Need a CSV → tlog + conversion that packs each row as a `RAW_IMU` + `ATTITUDE` + MAVLink pair, with yaw synthesised from `GLOBAL_POSITION_INT.hdg` + and roll/pitch=0 (acceptable for fixed-wing cruise). + +Output: the SUT's natural FDR archive directory (the b78 schema- +projection step is dropped because FT-P-02 reads the FDR directly). + +## Files Touched + +* `e2e/fixtures/sitl_replay_builder/_common.py` (new) — extracts + `run_gps_denied_replay` + `write_observer_fixture` from + `build_p01_fixtures.py` so b78 + b79 share one implementation. +* `e2e/fixtures/sitl_replay_builder/build_p01_fixtures.py` (edit) + — re-export the moved helpers from `_common.py` for backwards + compatibility; b78 tests must still pass unchanged. +* `e2e/fixtures/sitl_replay_builder/build_p02_fixtures.py` (new) — + the FT-P-02 builder. +* `e2e/fixtures/sitl_replay_builder/README.md` (edit) — document + both flows. +* `e2e/_unit_tests/fixtures/test_sitl_replay_builder_p02.py` (new) + — b79 builder tests. +* `e2e/_unit_tests/test_directory_layout.py` (edit) — register + the three new paths. + +## Acceptance Criteria + +**AC-1**: `convert_imu_csv_to_tlog` parses every row of +`data_imu.csv` and writes one RAW_IMU + one ATTITUDE pair per row. + +**AC-2**: `convert_imu_csv_to_tlog` raises `ValueError` on missing +required columns OR malformed numeric rows OR empty CSV. + +**AC-3**: Heading conversion uses centidegrees → radians +(`hdg_cdeg * pi / 18000`). + +**AC-4**: `build_p02_fixtures` invokes `run_gps_denied_replay` with +the Derkachi MP4 + the generated tlog; the resulting FDR archive +contains ≥1 `record_type="estimate"` record (`verify_fdr_has_estimates` +helper). + +**AC-5**: `run_gps_denied_replay` is in `_common.py`; b78 + +b79 both import it from there; b78 tests still pass. + +**AC-6**: Full e2e unit-test suite passes. + +## Out of Scope + +* Live capture EXECUTION (manual operator step, same as b78). +* FT-P-04 / FT-P-05 (also use Derkachi but each needs its own + builder; deferred). +* iNav adapter. +* True SCALED_IMU2 → RAW_IMU unit conversion (pass-through; will + re-visit if the SUT's tlog parser rejects). diff --git a/_docs/03_implementation/batch_79_report.md b/_docs/03_implementation/batch_79_report.md new file mode 100644 index 0000000..c394a1f --- /dev/null +++ b/_docs/03_implementation/batch_79_report.md @@ -0,0 +1,88 @@ +# Batch 79 Report — FT-P-02 Derkachi fixture builder + +**Batch**: 79 +**Date**: 2026-05-17 +**Context**: Test implementation (greenfield Step 10 — Implement Tests) +**Tasks**: AZ-599 (3 cp) — 1 task (FT-P-02 Derkachi builder + b78 helper extraction) +**Cycle**: 1 +**Verdict**: COMPLETE — PASS (self-reviewed; see `reviews/batch_79_review.md`) + +## Summary + +Replicates the b78 vertical-slice pattern for FT-P-02 (Derkachi +continuous-flight drift). Different from b78 in three ways: + +1. **Video is already MP4** — no still-image encoding step. +2. **IMU is real recorded telemetry** — needs CSV → tlog conversion + with real motion data (vs. b78's synthetic stationary tlog). +3. **Output is the SUT's natural FDR archive directory** — FT-P-02 + reads it via `fdr_reader.iter_records`, not via the per-call + `outbound_messages_*.json` schema b78 ships. + +Also extracted `run_gps_denied_replay` + `write_observer_fixture` +into a shared `_common.py` so both b78 and b79 reference one +canonical implementation. Future per-scenario builders (FT-P-04 +also uses Derkachi inputs, FT-P-05 needs its own, etc.) will reuse +the same helpers. + +### AZ-599 — FT-P-02 Derkachi builder (3 cp) + +* **`e2e/fixtures/sitl_replay_builder/_common.py`** (new): shared + `run_gps_denied_replay`, `write_observer_fixture`, `DEFAULT_CLI_BIN`. +* **`e2e/fixtures/sitl_replay_builder/build_p01_fixtures.py`** + (edited): re-imports the moved helpers from `_common.py`; b78 + tests still pass unchanged. +* **`e2e/fixtures/sitl_replay_builder/build_p02_fixtures.py`** (new): + * `P02BuilderConfig` frozen dataclass. + * `convert_imu_csv_to_tlog(csv_path, output_tlog)` — parses + `data_imu.csv` rows, packs RAW_IMU + ATTITUDE pairs. Yaw from + `GLOBAL_POSITION_INT.hdg` (centidegrees → radians); + roll/pitch = 0 (fixed-wing cruise approximation). + * `verify_fdr_has_estimates(fdr_path)` — counts + `record_type="estimate"` records; raises if zero. + * `build_p02_fixtures(cfg, ...)` — orchestrator. + * `_main(argv=None)` — argparse CLI. +* **`e2e/fixtures/sitl_replay_builder/README.md`** (edited): two- + builder structure with scenario coverage table; FT-P-01 + FT-P-02 + per-builder sections with strategy + usage + limitations. +* **`e2e/_unit_tests/fixtures/test_sitl_replay_builder_p02.py`** + (new): +20 tests. +* **`e2e/_unit_tests/test_directory_layout.py`** (edited): registers + the two new modules (`_common.py` + `build_p02_fixtures.py`). + +## Out of scope (deferred) + +* **Live capture EXECUTION** — same as b78, a manual operator step. +* **FT-P-04** (Derkachi F2F registration) — also uses Derkachi + inputs and could reuse `build_p02_fixtures`. Deferred to a + follow-up so this batch ships at 3 cp. +* **Other scenarios** (FT-P-03 / 05 / 07 / 08 / 10 / 11, FT-N-01..04). +* **iNav adapter** for FT-P-02. +* **True SCALED_IMU2 → RAW_IMU unit conversion** — pass-through; + re-visit if the SUT's tlog parser rejects. +* **Aggressive-manoeuvre ATTITUDE synthesis** (roll/pitch ≠ 0) — + re-visit if FT-P-04 or other scenarios show fusion drift. + +## Test Results + +* New unit tests: **20**. +* Full `e2e/_unit_tests` suite: **686 passed in 123 s** (previous: + 664 → +22 net = +20 new tests + +2 directory-layout entries). +* No new linter errors. +* b78 builder tests (24) still pass after `_common.py` extraction. + +## State + +* Spec moved: `_docs/02_tasks/todo/AZ-599_ft_p_02_derkachi_builder.md` + → `_docs/02_tasks/done/`. +* `_docs/_autodev_state.md` advanced to `last_completed_batch: 79`. +* No K=3 cumulative review this batch (next due after b81; the + b76-b78 cumulative just shipped one batch ago). + +## Incidents + +* **Disk-full during regression gate**: the first regression-gate + attempt failed with 212 conftest setup errors (all + `FileNotFoundError: /var/folders/.../pytest-of-...`). Surfaced + to the user with options; user freed space (37 Gi avail after); + re-ran the gate and it passed cleanly. Not a code issue. diff --git a/_docs/03_implementation/reviews/batch_79_review.md b/_docs/03_implementation/reviews/batch_79_review.md new file mode 100644 index 0000000..ba18dd7 --- /dev/null +++ b/_docs/03_implementation/reviews/batch_79_review.md @@ -0,0 +1,156 @@ +# Code Review Report + +**Batch**: 79 — AZ-599 (FT-P-02 Derkachi fixture builder + b78 helper extraction) +**Date**: 2026-05-17 +**Verdict**: PASS + +## Findings + +(none blocking) + +### Non-blocking notes + +* **Roll/pitch=0 simplification in synthesised ATTITUDE**: acceptable + for the Derkachi fixed-wing cruise data but unrealistic for + aggressive manoeuvres. Documented as a Limitation in the README; + re-visit if FT-P-04 (Derkachi F2F registration) or other + manoeuvre-sensitive scenarios show fusion drift from this gap. +* **`SCALED_IMU2` → `RAW_IMU` pass-through (no unit conversion)**: + the builder passes scaled accelerometer (mg) and gyro (mrad/s) + values straight into RAW_IMU fields. Documented as a Limitation; + if the SUT's tlog parser rejects, a units conversion pass needs to + be added. Surfaced as a follow-up rather than fixed speculatively. +* **Disk-full incident during regression gate**: not a code issue; + surfaced to the user, who freed space, after which the full + suite (686 tests) passed cleanly. + +## Findings Sweep + +### Phase 1 — Context Loading + +Read the FT-P-02 scenario to confirm: it (a) skips when +`sitl_replay_ready` is false; (b) calls +`fdr_reader.iter_records(fdr_root)` to walk the FDR archive; (c) +filters for `record_type == "estimate"` and projects into +`apd.FdrEstimate`. Confirmed FT-P-02 does NOT call +`wait_for_outbound` (so the b78 schema doesn't apply). Read +`data_imu.csv` to confirm column names + units (10 Hz; ms +timestamps; SCALED_IMU2 in mg / mrad/s; GLOBAL_POSITION_INT.hdg +in centidegrees). Inspected the production +`replay_input/auto_sync.py` AC-13 contract for required tlog +message types (RAW_IMU + ATTITUDE) — this drove the choice of +which messages to pack per CSV row. Verified `pymavlink>=2.4` +already in `pyproject.toml`. Read the b78 +`build_p01_fixtures.py` to identify the helper extraction +boundary (`run_gps_denied_replay` + `write_observer_fixture` had +no FT-P-01-specific code, ideal candidates). + +### Phase 2 — Spec Compliance + +| AC | Coverage | Status | +|----|----------|--------| +| AC-1 (parse every row, one pair per row) | `test_convert_imu_csv_writes_pair_per_row`, `test_convert_imu_csv_real_pymavlink_round_trip` | Covered | +| AC-2 (ValueError on missing cols / malformed numerics / empty CSV) | `test_convert_imu_csv_empty_raises`, `test_convert_imu_csv_missing_required_column_raises`, `test_convert_imu_csv_malformed_numeric_raises`, `test_convert_imu_csv_missing_file_raises` | Covered (4 distinct error paths) | +| AC-3 (heading centideg → rad) | `test_hdg_centideg_to_rad` (5 parametric cases: 0, π/2, π, 3π/2, near-2π) | Covered | +| AC-4 (build_p02 invokes replay + verifies ≥1 estimate) | `test_build_p02_end_to_end_with_mocks`, `test_build_p02_propagates_verify_failure`, `test_verify_fdr_no_estimates_raises`, `test_verify_fdr_counts_estimates`, `test_verify_fdr_tolerates_malformed_lines` | Covered | +| AC-5 (`_common.py` shared between b78 + b79) | `test_common_module_exports_used_by_b01` + b78 suite (24/24) still passes | Covered | +| AC-6 (full unit-test suite passes) | 686/686 in 123 s (previous: 664) | Covered | + +### Phase 3 — Code Quality + +* **Single responsibility**: `convert_imu_csv_to_tlog` parses CSV + + packs MAVLink (one cohesive concern: CSV row → tlog pair). + `verify_fdr_has_estimates` does one thing. `build_p02_fixtures` + is the only orchestrator. Helper extraction into `_common.py` + removed duplicate definitions cleanly — no copy-pasted code + remains. +* **No suppressed errors**: every JSON-decode in + `verify_fdr_has_estimates` is wrapped in a `try / except + json.JSONDecodeError` that explicitly `continue`s with a comment + explaining the tolerance (real FDR archives can contain partial + writes at the tail). Every CSV parse error wraps in `ValueError` + with row context. `subprocess.run` uses `check=True`. +* **AAA test discipline**: all 20 new tests use + `# Arrange / # Act / # Assert`; omitted when redundant. +* **Comments**: every public function has a docstring documenting + contract + raises; no narrating comments in function bodies. + The yaw conversion uses a named helper (`_hdg_centideg_to_rad`) + rather than an inline magic-formula comment. +* **Public boundary**: b79 module does NOT import any + `src/gps_denied_onboard` symbol (verified by grep). pymavlink + imports are deferred to function bodies so the test factory + injections don't need the real library available. + +### Phase 4 — Security + +* No new credentials / secrets / network surfaces. +* `convert_imu_csv_to_tlog` uses `csv.DictReader` — no `eval`, no + shell injection. Numeric fields go through `int(float(s))` so + bad input raises ValueError rather than executing arbitrary code. +* `verify_fdr_has_estimates` uses `json.loads` (safe) and silently + ignores malformed lines (documented as tolerance for partial + writes; not a security risk because we never act on the data). +* `subprocess` invocation uses a list-argument `cmd` (no shell + injection surface; same pattern as b78). + +### Phase 5 — Performance + +* `convert_imu_csv_to_tlog` is O(N) over CSV rows; for the Derkachi + 4,900-row file → ~5 s wall-clock estimate (pymavlink packing is + the bottleneck). Bounded by input size; no hidden quadratic + behaviour. +* `verify_fdr_has_estimates` is single-pass O(N) over JSONL lines. +* `_iter_imu_rows` validates the header once and then streams rows; + doesn't materialize the entire CSV in memory until the orchestrator + asks for `list(...)` (could be optimized later if Derkachi data + ever grows orders of magnitude larger; currently 4,900 rows is + trivial). + +### Phase 6 — Cross-Task Consistency + +* **Builder shape parity with b78**: `BuilderConfig` / + `P02BuilderConfig` dataclass; `build_pXX_fixtures(cfg, *, _runner=..., + _mavlink_writer_factory=..., _verify_fdr=...)` signature; same + underscore-prefixed dependency-injection convention. A future + contributor reading both side-by-side will see the same pattern. +* **`_common.py` extraction validated by both**: b78's existing + `test_run_gps_denied_replay_builds_correct_cmd` / + `test_run_gps_denied_replay_creates_fdr_parent_dir` / + `test_run_gps_denied_replay_passes_extra_args` / + `test_write_observer_fixture_schema` tests pass unchanged against + the moved implementation. b79 adds + `test_common_module_exports_used_by_b01` to lock in the import + relationship. +* **No drift in `observer__.json` schema**: same + payload structure as b75/b78. + +### Phase 7 — Architecture Compliance + +* **Module placement**: new files under + `e2e/fixtures/sitl_replay_builder/` (existing package from b78); + new tests under `e2e/_unit_tests/fixtures/`. All registered in + `test_directory_layout.py`. +* **No `src/gps_denied_onboard` imports**. +* **No new top-level dependencies** — pymavlink already there from + b78/b76; `csv` is stdlib; `math` is stdlib. +* **`__init__.py` not modified** — the b78 docstring already + documents why we don't re-export symbols on the package namespace, + and that's still the right choice for b79. +* **Refactor preserves backwards compatibility**: `build_p01_fixtures` + re-imports `run_gps_denied_replay` + `write_observer_fixture` + from `_common.py`. Any caller (test or production) doing + `bp01.run_gps_denied_replay(...)` still works because the symbol + is present in the module namespace via the `from … import …` + re-export. + +## Test Results + +* New unit tests: **20** (5 `convert_imu_csv_to_tlog` + 5 + `_hdg_centideg_to_rad` parametric + 4 `verify_fdr_has_estimates` + + 4 `build_p02_fixtures` end-to-end + 1 export-relationship + + 1 missing-file negative). +* Full `e2e/_unit_tests` suite: **686 passed in 123 s** (previous: + 664 → +22 net = +20 new + +2 directory-layout entries). +* No new linter errors (`ReadLints` clean on all touched files). +* No regression in the b78 builder tests (24/24 still pass after + the helper extraction). diff --git a/_docs/_autodev_state.md b/_docs/_autodev_state.md index 9da0e33..aed6dd1 100644 --- a/_docs/_autodev_state.md +++ b/_docs/_autodev_state.md @@ -6,16 +6,16 @@ step: 10 name: Implement Tests status: in_progress sub_step: - phase: 6 - name: implement-tasks-sequentially + phase: 0 + name: awaiting-invocation detail: "" retry_count: 0 cycle: 1 tracker: jira -last_completed_batch: 78 +last_completed_batch: 79 last_cumulative_review: batches_76-78 -current_batch: 79 -current_batch_tasks: "" +current_batch: 80 + last_step_outcomes: step_8: "Code is testable — no changes needed (testability_assessment.md committed; no list-of-changes, no source edits)" - step_9: "Already complete — 41 blackbox test tasks (AZ-406..AZ-446) under epic AZ-262 with specs in _docs/02_tasks/todo/ were produced in a prior cycle; AZ-406 test-infrastructure bootstrap also pre-existing. Folder fallback satisfied (todo/ has test tasks, _dependencies_table.md reflects 114 product + 41 test = 155 total). No Step-9 work executed in cycle 1." + step_9: "41 blackbox test tasks (AZ-406..AZ-446) under epic AZ-262 in _docs/02_tasks/todo/ pre-existing; AZ-406 test-infra bootstrap pre-existing. Folder fallback satisfied. No Step-9 work executed in cycle 1." diff --git a/e2e/_unit_tests/fixtures/test_sitl_replay_builder_p02.py b/e2e/_unit_tests/fixtures/test_sitl_replay_builder_p02.py new file mode 100644 index 0000000..1656b69 --- /dev/null +++ b/e2e/_unit_tests/fixtures/test_sitl_replay_builder_p02.py @@ -0,0 +1,324 @@ +"""Unit tests for `e2e/fixtures/sitl_replay_builder/build_p02_fixtures.py` (AZ-599). + +All external dependencies (pymavlink, subprocess) are injected via the +underscore-prefixed parameters. The IMU CSV is small enough that we +hand-author it inline for each test rather than depending on the real +Derkachi data file. +""" + +from __future__ import annotations + +import csv +import json +import math +import subprocess +from pathlib import Path +from typing import Sequence +from unittest.mock import MagicMock + +import pytest + +import e2e.fixtures.sitl_replay_builder.build_p02_fixtures as bp02 + + +_HEADER_ROW = ( + "timestamp(ms),Time,SCALED_IMU2.xacc,SCALED_IMU2.yacc,SCALED_IMU2.zacc," + "SCALED_IMU2.xgyro,SCALED_IMU2.ygyro,SCALED_IMU2.zgyro," + "SCALED_IMU2.xmag,SCALED_IMU2.ymag,SCALED_IMU2.zmag," + "GLOBAL_POSITION_INT.lat,GLOBAL_POSITION_INT.lon,GLOBAL_POSITION_INT.alt," + "GLOBAL_POSITION_INT.relative_alt,GLOBAL_POSITION_INT.vx,GLOBAL_POSITION_INT.vy," + "GLOBAL_POSITION_INT.vz,GLOBAL_POSITION_INT.hdg" +) + + +def _write_imu_csv(path: Path, rows: list[list]) -> None: + """Write a CSV with the full Derkachi header + the supplied data rows.""" + path.parent.mkdir(parents=True, exist_ok=True) + with path.open("w", newline="", encoding="utf-8") as fp: + fp.write(_HEADER_ROW + "\n") + writer = csv.writer(fp) + for row in rows: + writer.writerow(row) + + +def _good_row(ts_ms: float, hdg: int = 35041) -> list: + """One well-formed Derkachi row at `ts_ms` and heading `hdg` cdeg.""" + return [ + ts_ms, 0.0, + 21, -3, -984, + 52, 32, -5, + 312, -1048, 442, + 50_080_963_4, 36_111_544_2, 141_290, 23_182, + -4, -6, -88, + hdg, + ] + + +# convert_imu_csv_to_tlog + + +def test_convert_imu_csv_missing_file_raises(tmp_path: Path): + # Assert + with pytest.raises(FileNotFoundError, match="IMU CSV not found"): + bp02.convert_imu_csv_to_tlog( + tmp_path / "missing.csv", tmp_path / "out.tlog", + _mavlink_writer_factory=lambda out: MagicMock(), + ) + + +def test_convert_imu_csv_empty_raises(tmp_path: Path): + # Arrange — header only, no data rows + csv_path = tmp_path / "imu.csv" + _write_imu_csv(csv_path, []) + + # Assert + with pytest.raises(ValueError, match="IMU CSV is empty"): + bp02.convert_imu_csv_to_tlog( + csv_path, tmp_path / "out.tlog", + _mavlink_writer_factory=lambda out: MagicMock(), + ) + + +def test_convert_imu_csv_missing_required_column_raises(tmp_path: Path): + # Arrange — header missing `GLOBAL_POSITION_INT.hdg` + csv_path = tmp_path / "imu.csv" + csv_path.write_text("timestamp(ms),Time\n0,0\n") + + # Assert + with pytest.raises(ValueError, match="missing required columns"): + bp02.convert_imu_csv_to_tlog( + csv_path, tmp_path / "out.tlog", + _mavlink_writer_factory=lambda out: MagicMock(), + ) + + +def test_convert_imu_csv_malformed_numeric_raises(tmp_path: Path): + # Arrange — second-to-last value (xacc) is non-numeric + csv_path = tmp_path / "imu.csv" + row = _good_row(0.0) + row[2] = "not-a-number" + _write_imu_csv(csv_path, [row]) + + # Assert + with pytest.raises(ValueError, match="malformed IMU CSV row"): + bp02.convert_imu_csv_to_tlog( + csv_path, tmp_path / "out.tlog", + _mavlink_writer_factory=lambda out: MagicMock(), + ) + + +def test_convert_imu_csv_writes_pair_per_row(tmp_path: Path): + # Arrange + csv_path = tmp_path / "imu.csv" + _write_imu_csv(csv_path, [_good_row(0.0), _good_row(100.0), _good_row(200.0)]) + writer = MagicMock(write=MagicMock(), close=MagicMock()) + + # Act + pairs = bp02.convert_imu_csv_to_tlog( + csv_path, tmp_path / "out.tlog", + _mavlink_writer_factory=lambda out: writer, + ) + + # Assert — 3 rows → 3 pairs → 6 message writes + assert pairs == 3 + assert writer.write.call_count == 6 + assert writer.close.call_count == 1 + + +def test_convert_imu_csv_real_pymavlink_round_trip(tmp_path: Path): + """Sanity-check the real packers; tlog file is well-formed.""" + # Arrange + csv_path = tmp_path / "imu.csv" + _write_imu_csv(csv_path, [_good_row(0.0), _good_row(100.0)]) + + # Act — use real pymavlink (it's in pyproject.toml deps) + pairs = bp02.convert_imu_csv_to_tlog(csv_path, tmp_path / "out.tlog") + + # Assert + assert pairs == 2 + assert (tmp_path / "out.tlog").stat().st_size > 0 + + +# _hdg_centideg_to_rad + + +@pytest.mark.parametrize( + "centideg,expected_rad", + [ + (0, 0.0), + (9000, math.pi / 2), + (18000, math.pi), + (27000, 3 * math.pi / 2), + (35990, 35990 * math.pi / 18000), + ], +) +def test_hdg_centideg_to_rad(centideg: int, expected_rad: float): + # Assert + assert bp02._hdg_centideg_to_rad(centideg) == pytest.approx(expected_rad) + + +# verify_fdr_has_estimates + + +def _write_jsonl(path: Path, records: list[dict]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text("\n".join(json.dumps(r) for r in records)) + + +def test_verify_fdr_missing_file_raises(tmp_path: Path): + # Assert + with pytest.raises(FileNotFoundError, match="FDR JSONL not found"): + bp02.verify_fdr_has_estimates(tmp_path / "missing.jsonl") + + +def test_verify_fdr_no_estimates_raises(tmp_path: Path): + # Arrange + fdr = tmp_path / "fdr.jsonl" + _write_jsonl(fdr, [ + {"record_type": "other", "payload": {}}, + {"record_type": "imu_tick", "payload": {}}, + ]) + + # Assert + with pytest.raises(ValueError, match="zero estimate records"): + bp02.verify_fdr_has_estimates(fdr) + + +def test_verify_fdr_counts_estimates(tmp_path: Path): + # Arrange + fdr = tmp_path / "fdr.jsonl" + _write_jsonl(fdr, [ + {"record_type": "estimate", "payload": {}}, + {"record_type": "other", "payload": {}}, + {"record_type": "estimate", "payload": {}}, + {"record_type": "estimate", "payload": {}}, + ]) + + # Act + count = bp02.verify_fdr_has_estimates(fdr) + + # Assert + assert count == 3 + + +def test_verify_fdr_tolerates_malformed_lines(tmp_path: Path): + # Arrange — one bad JSON line interleaved with good estimate records + fdr = tmp_path / "fdr.jsonl" + fdr.write_text( + json.dumps({"record_type": "estimate"}) + "\n" + + "{not valid json\n" + + json.dumps({"record_type": "estimate"}) + "\n" + ) + + # Act + count = bp02.verify_fdr_has_estimates(fdr) + + # Assert + assert count == 2 + + +# build_p02_fixtures end-to-end (mocked) + + +def test_build_p02_missing_video_raises(tmp_path: Path): + # Arrange + derkachi_dir = tmp_path / "derkachi" + derkachi_dir.mkdir() + (derkachi_dir / "data_imu.csv").write_text(_HEADER_ROW + "\n") + + cfg = bp02.P02BuilderConfig( + derkachi_dir=derkachi_dir, output_dir=tmp_path / "out", + ) + + # Assert + with pytest.raises(FileNotFoundError, match="Derkachi MP4 not found"): + bp02.build_p02_fixtures(cfg) + + +def test_build_p02_missing_csv_raises(tmp_path: Path): + # Arrange + derkachi_dir = tmp_path / "derkachi" + derkachi_dir.mkdir() + (derkachi_dir / "flight_derkachi.mp4").touch() + + cfg = bp02.P02BuilderConfig( + derkachi_dir=derkachi_dir, output_dir=tmp_path / "out", + ) + + # Assert + with pytest.raises(FileNotFoundError, match="Derkachi IMU CSV not found"): + bp02.build_p02_fixtures(cfg) + + +def test_build_p02_end_to_end_with_mocks(tmp_path: Path): + # Arrange + derkachi_dir = tmp_path / "derkachi" + output_dir = tmp_path / "out" + derkachi_dir.mkdir() + (derkachi_dir / "flight_derkachi.mp4").touch() + _write_imu_csv(derkachi_dir / "data_imu.csv", [_good_row(0.0), _good_row(100.0)]) + + def fake_runner(cmd): + fdr_path = Path(cmd[cmd.index("--fdr-out") + 1]) + _write_jsonl(fdr_path, [ + {"record_type": "estimate", "payload": {"lat_deg": 50.0, "lon_deg": 36.0}}, + {"record_type": "estimate", "payload": {"lat_deg": 50.1, "lon_deg": 36.1}}, + ]) + return subprocess.CompletedProcess(cmd, 0) + + cfg = bp02.P02BuilderConfig( + derkachi_dir=derkachi_dir, output_dir=output_dir, + fc_kind="ardupilot", host="sitl-host", + ) + + # Act + result = bp02.build_p02_fixtures( + cfg, + _runner=fake_runner, + _mavlink_writer_factory=lambda out: MagicMock(write=MagicMock(), close=MagicMock()), + ) + + # Assert + assert result == output_dir + assert (output_dir / "fdr" / "fdr.jsonl").is_file() + observer = json.loads((output_dir / "observer_ardupilot_sitl-host.json").read_text()) + assert observer["gps_state"]["primary_source"] == "MAV" + + +def test_build_p02_propagates_verify_failure(tmp_path: Path): + # Arrange — fake runner writes an FDR with no estimates; default verifier raises. + derkachi_dir = tmp_path / "derkachi" + derkachi_dir.mkdir() + (derkachi_dir / "flight_derkachi.mp4").touch() + _write_imu_csv(derkachi_dir / "data_imu.csv", [_good_row(0.0)]) + + def fake_runner(cmd): + fdr_path = Path(cmd[cmd.index("--fdr-out") + 1]) + _write_jsonl(fdr_path, [{"record_type": "imu_tick"}]) + return subprocess.CompletedProcess(cmd, 0) + + cfg = bp02.P02BuilderConfig( + derkachi_dir=derkachi_dir, output_dir=tmp_path / "out", + ) + + # Assert + with pytest.raises(ValueError, match="zero estimate records"): + bp02.build_p02_fixtures( + cfg, + _runner=fake_runner, + _mavlink_writer_factory=lambda out: MagicMock(write=MagicMock(), close=MagicMock()), + ) + + +# `_common.py` is shared with b78 + + +def test_common_module_exports_used_by_b01(): + """AC-5: b78 builder still imports from _common.py after refactor.""" + # Arrange + import e2e.fixtures.sitl_replay_builder.build_p01_fixtures as bp01 + import e2e.fixtures.sitl_replay_builder._common as common + + # Assert + assert bp01.run_gps_denied_replay is common.run_gps_denied_replay + assert bp01.write_observer_fixture is common.write_observer_fixture diff --git a/e2e/_unit_tests/test_directory_layout.py b/e2e/_unit_tests/test_directory_layout.py index 08f98a9..91e1a71 100644 --- a/e2e/_unit_tests/test_directory_layout.py +++ b/e2e/_unit_tests/test_directory_layout.py @@ -58,7 +58,9 @@ E2E_ROOT = Path(__file__).resolve().parents[1] "runner/helpers/fc_proxy_runtime.py", "runner/helpers/replay_mode.py", "fixtures/sitl_replay_builder/__init__.py", + "fixtures/sitl_replay_builder/_common.py", "fixtures/sitl_replay_builder/build_p01_fixtures.py", + "fixtures/sitl_replay_builder/build_p02_fixtures.py", "fixtures/sitl_replay_builder/README.md", "fixtures/mock-suite-sat/Dockerfile", "fixtures/mock-suite-sat/app.py", diff --git a/e2e/fixtures/sitl_replay_builder/README.md b/e2e/fixtures/sitl_replay_builder/README.md index 3f5c1e4..50799ab 100644 --- a/e2e/fixtures/sitl_replay_builder/README.md +++ b/e2e/fixtures/sitl_replay_builder/README.md @@ -1,16 +1,33 @@ -# SITL Replay Fixture Builder (AZ-598) +# SITL Replay Fixture Builder (AZ-598, AZ-599) -Produces the `outbound_messages__.json` + -`observer__.json` fixtures consumed by the b75 -`sitl_observer` module in offline FDR-replay mode (b75/b78). +Per-scenario fixture builders for the offline FDR-replay path used +by the b75 `sitl_observer` module + FT-* blackbox scenarios. Each +builder takes recorded flight inputs (still images / video / IMU +CSV / tlog) and produces the artifacts a specific scenario needs. -## Vertical-slice scope (this batch) +| Scenario | Builder | Inputs | Outputs | +|----------|---------|--------|---------| +| FT-P-01 (still-image accuracy) | `build_p01_fixtures.py` | 60 `AD0000NN.jpg` + coordinates CSV | `outbound_messages__.json` + `observer__.json` + `stills.mp4` + `stationary.tlog` + `fdr.jsonl` | +| FT-P-02 (Derkachi drift) | `build_p02_fixtures.py` | `flight_derkachi.mp4` + `data_imu.csv` | `derkachi.tlog` + `fdr/fdr.jsonl` (FDR archive) + `observer__.json` | -Only the FT-P-01 still-image accuracy scenario is supported. Other -scenarios (FT-P-02 Derkachi continuous flight, FT-N-04 blackout-spoof, -etc.) need their own capture flows and will land as follow-up tickets. +Other scenarios (FT-P-03 / 04 / 05 / 07 / 08 / 10 / 11, FT-N-01 / 02 / 03 / 04) +need their own capture flows and will land as follow-up tickets. -## Strategy +## Shared helpers (`_common.py`) + +Both builders shell out to the production `gps-denied-replay` CLI and +write the same minimal `observer__.json`. These two +operations live in `_common.py`: + +* `run_gps_denied_replay(video, tlog, fdr_out, *, time_offset_ms=0, ...)` +* `write_observer_fixture(output_path)` + +Future per-scenario builders should import from `_common.py` rather +than re-implementing. + +## FT-P-01 (`build_p01_fixtures.py`) + +### Strategy Rather than spinning up a SITL container, this builder reuses the production `gps-denied-replay` CLI + `ReplayInputAdapter`: @@ -30,10 +47,10 @@ This avoids needing new SUT-side frame-ingestion code (HTTP endpoint, file-watch source, etc.) which would otherwise be required to push individual stills to a running SUT container. -## Usage +### Usage ```bash -gps-denied-build-p01-fixtures \ +python -m e2e.fixtures.sitl_replay_builder.build_p01_fixtures \ --input-dir _docs/00_problem/input_data \ --output-dir e2e/fixtures/sitl_replay/p01 \ --fc-kind ardupilot \ @@ -55,7 +72,7 @@ E2E_SITL_REPLAY_DIR=e2e/fixtures/sitl_replay/p01 \ pytest e2e/tests/positive/test_ft_p_01_still_image_accuracy.py ``` -## Limitations +### Limitations * The synthetic tlog encodes zero motion — auto-sync MUST be bypassed via `--time-offset-ms 0` (the builder does this automatically). @@ -67,9 +84,53 @@ E2E_SITL_REPLAY_DIR=e2e/fixtures/sitl_replay/p01 \ * iNav adapter is NOT supported by this batch — only ArduPilot. iNav will land as a follow-up once the AP path is validated end-to-end. +## FT-P-02 (`build_p02_fixtures.py`) + +### Strategy + +Same overall shape as FT-P-01 (drive `gps-denied-replay` against a +video + tlog pair), with two differences: + +1. Video is already MP4 — skip the OpenCV still-image encoding step. +2. IMU is recorded telemetry (`data_imu.csv`, 10 Hz `SCALED_IMU2` + + `GLOBAL_POSITION_INT`). A CSV → tlog conversion packs each row as + a `RAW_IMU` + `ATTITUDE` MAVLink pair, with yaw synthesised from + `GLOBAL_POSITION_INT.hdg` (centidegrees → radians) and roll/pitch + = 0 (acceptable for the fixed-wing cruise data this represents). + +Output is the SUT's natural FDR archive directory; the FT-P-02 +scenario reads it via `runner.helpers.fdr_reader.iter_records`. + +### Usage + +```bash +python -m e2e.fixtures.sitl_replay_builder.build_p02_fixtures \ + --derkachi-dir _docs/00_problem/input_data/flight_derkachi \ + --output-dir e2e/fixtures/sitl_replay/p02 \ + --fc-kind ardupilot \ + --host sitl-host +``` + +Output: + +* `derkachi.tlog` — generated from `data_imu.csv`. +* `fdr/fdr.jsonl` — the FDR archive from the replay run. +* `observer_ardupilot_sitl-host.json` — minimal observer config. + +### Limitations + +* The synthesised ATTITUDE has roll/pitch = 0 — acceptable for + fixed-wing cruise but unrealistic for aggressive manoeuvres. +* RAW_IMU is packed from `SCALED_IMU2` columns as pass-through (no + true scaled → raw unit conversion). If the SUT's tlog parser + strictly demands true raw counts the builder will need a units + conversion pass — surfaced as a follow-up after live-run. +* Auto-sync is bypassed via `--time-offset-ms 0` because the + Derkachi CSV is already aligned with the video. + ## Testing -Unit tests under `e2e/_unit_tests/fixtures/test_sitl_replay_builder.py` +Unit tests under `e2e/_unit_tests/fixtures/test_sitl_replay_builder*.py` mock all external dependencies (OpenCV, pymavlink, subprocess) so the test suite runs without a real `gps-denied-replay` install. The actual end-to-end run requires the SUT to be installed (`pip install -e .` at diff --git a/e2e/fixtures/sitl_replay_builder/_common.py b/e2e/fixtures/sitl_replay_builder/_common.py new file mode 100644 index 0000000..571a24c --- /dev/null +++ b/e2e/fixtures/sitl_replay_builder/_common.py @@ -0,0 +1,85 @@ +"""Shared helpers for the per-scenario fixture builders (AZ-599). + +Both `build_p01_fixtures.py` (still-image FT-P-01) and +`build_p02_fixtures.py` (Derkachi FT-P-02) shell out to the production +`gps-denied-replay` CLI and write the same minimal `observer_*.json` +config; the helpers below live here so there's one canonical +implementation. + +Future per-scenario builders (FT-P-04 / FT-P-05 / FT-P-10 / …) should +also import from this module. +""" + +from __future__ import annotations + +import json +import logging +import subprocess +from pathlib import Path +from typing import Callable, Sequence + +_LOG = logging.getLogger(__name__) + +DEFAULT_CLI_BIN = "gps-denied-replay" + + +def run_gps_denied_replay( + video: Path, + tlog: Path, + fdr_out: Path, + *, + cli_bin: str = DEFAULT_CLI_BIN, + time_offset_ms: int = 0, + extra_args: Sequence[str] = (), + _runner: Callable[[Sequence[str]], subprocess.CompletedProcess] | None = None, +) -> subprocess.CompletedProcess: + """Run ``gps-denied-replay`` as a subprocess. + + The `time_offset_ms` defaults to 0 because both b78 (synthetic + stationary tlog) and b79 (Derkachi real-motion tlog) intentionally + bypass auto-sync — b78 because there's no take-off signal, b79 + because the IMU CSV is already aligned with the video. Operators + running this against truly independent tlog+video pairs SHOULD + omit ``time_offset_ms`` and let the production auto-sync run. + + Raises ``subprocess.CalledProcessError`` on non-zero exit code. + The default subprocess runner can be swapped via ``_runner`` for + unit tests. + """ + fdr_out.parent.mkdir(parents=True, exist_ok=True) + cmd: list[str] = [ + cli_bin, + "--video", str(video), + "--tlog", str(tlog), + "--time-offset-ms", str(time_offset_ms), + "--fdr-out", str(fdr_out), + *extra_args, + ] + _LOG.info("running: %s", " ".join(cmd)) + + runner = _runner or (lambda c: subprocess.run(c, check=True, capture_output=True, text=True)) + return runner(cmd) + + +def write_observer_fixture(output_path: Path) -> None: + """Write minimal `observer__.json` so `get_observer` succeeds. + + Scenarios that only consume `wait_for_outbound` or `iter_records` + still trigger `sitl_observer.get_observer(...)` for construction. + Populate with safe defaults; scenarios that care about + `read_gps_state` carry their own observer fixtures. + """ + payload = { + "gps_state": { + "primary_source": "MAV", + "last_position_lat_deg": 0.0, + "last_position_lon_deg": 0.0, + "last_position_alt_m": 0.0, + "fix_quality": 3, + "horizontal_accuracy_m": 1.0, + "last_update_age_ms": 0, + }, + "parameters": {}, + } + output_path.parent.mkdir(parents=True, exist_ok=True) + output_path.write_text(json.dumps(payload, indent=2)) diff --git a/e2e/fixtures/sitl_replay_builder/build_p01_fixtures.py b/e2e/fixtures/sitl_replay_builder/build_p01_fixtures.py index 47bda30..69991a5 100644 --- a/e2e/fixtures/sitl_replay_builder/build_p01_fixtures.py +++ b/e2e/fixtures/sitl_replay_builder/build_p01_fixtures.py @@ -34,13 +34,18 @@ from dataclasses import dataclass from pathlib import Path from typing import Callable, Iterable, Sequence +from e2e.fixtures.sitl_replay_builder._common import ( + DEFAULT_CLI_BIN, + run_gps_denied_replay, + write_observer_fixture, +) + _LOG = logging.getLogger(__name__) DEFAULT_FPS = 1.0 DEFAULT_TLOG_DURATION_S = 120 DEFAULT_TLOG_HZ = 200 DEFAULT_FDR_KIND = "outbound_position_estimate" -DEFAULT_CLI_BIN = "gps-denied-replay" @dataclass(frozen=True) @@ -214,40 +219,7 @@ def _pack_attitude_zero(time_boot_ms: int) -> bytes: # Step 3 — drive `gps-denied-replay` against the generated video+tlog - - -def run_gps_denied_replay( - video: Path, - tlog: Path, - fdr_out: Path, - *, - cli_bin: str = DEFAULT_CLI_BIN, - time_offset_ms: int = 0, - extra_args: Sequence[str] = (), - _runner: Callable[[Sequence[str]], subprocess.CompletedProcess] | None = None, -) -> subprocess.CompletedProcess: - """Run ``gps-denied-replay`` as a subprocess. - - Bypasses auto-sync via ``--time-offset-ms 0`` because the synthetic - stationary tlog has no take-off signal to detect. - - Raises ``subprocess.CalledProcessError`` on non-zero exit code (with - the FDR path included in the error message). The default subprocess - runner can be swapped via the underscore-prefixed parameter for tests. - """ - fdr_out.parent.mkdir(parents=True, exist_ok=True) - cmd: list[str] = [ - cli_bin, - "--video", str(video), - "--tlog", str(tlog), - "--time-offset-ms", str(time_offset_ms), - "--fdr-out", str(fdr_out), - *extra_args, - ] - _LOG.info("running: %s", " ".join(cmd)) - - runner = _runner or (lambda c: subprocess.run(c, check=True, capture_output=True, text=True)) - return runner(cmd) +# (`run_gps_denied_replay` is re-exported from `_common.py` so b78 + b79 share one impl.) # Step 4 — extract per-frame outbound estimates from the FDR JSONL @@ -334,28 +306,7 @@ def write_outbound_messages_fixture( output_path.write_text(json.dumps({"messages": messages}, indent=2)) -def write_observer_fixture(output_path: Path) -> None: - """Write minimal `observer__.json` so `get_observer` succeeds. - - The FT-P-01 scenario only consumes `wait_for_outbound`, but - `get_observer` still requires a valid observer fixture for - construction. Populate with safe defaults; per-scenario tests that - care about `read_gps_state` carry their own observer fixtures. - """ - payload = { - "gps_state": { - "primary_source": "MAV", - "last_position_lat_deg": 0.0, - "last_position_lon_deg": 0.0, - "last_position_alt_m": 0.0, - "fix_quality": 3, - "horizontal_accuracy_m": 1.0, - "last_update_age_ms": 0, - }, - "parameters": {}, - } - output_path.parent.mkdir(parents=True, exist_ok=True) - output_path.write_text(json.dumps(payload, indent=2)) +# `write_observer_fixture` is re-exported from `_common.py` (used by both b78 + b79). # Orchestration diff --git a/e2e/fixtures/sitl_replay_builder/build_p02_fixtures.py b/e2e/fixtures/sitl_replay_builder/build_p02_fixtures.py new file mode 100644 index 0000000..095ead9 --- /dev/null +++ b/e2e/fixtures/sitl_replay_builder/build_p02_fixtures.py @@ -0,0 +1,291 @@ +"""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 ``/fdr/fdr.jsonl``. + 4. Verify the FDR archive contains ≥1 estimate record. + 5. Write the companion ``observer__.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())