#!/usr/bin/env python3 """Cut G90 captures into per-window RTK CSV files. NMEA GGA/GNHPR UTC is the measurement time. Host receive UTC is retained only for diagnostics. The measurement UTC is mapped onto the IMU device clock with one robust affine clock model per window. """ from __future__ import annotations import argparse import csv import json import math import sys from datetime import datetime, timedelta, timezone from pathlib import Path import numpy as np ROOT = Path(__file__).resolve().parents[1] if str(ROOT) not in sys.path: sys.path.insert(0, str(ROOT)) from tools.h32_dlog.timeutil import utc_dotnet_ticks_to_unix_s from tools.rscap_v2.capture_format_v2 import file_summary, read_capture from tools.rscap_v2.g90_rtk import RtkSentence, iter_g90_sentences from tools.time_alignment import AffineClockModel, fit_affine_clock LOCAL_TZ = timezone(timedelta(hours=8)) HPR_MATCH_S = 0.08 CSV_FIELDS = [ "t", "t_measurement_utc_s", "t_host_utc_s", "receive_delay_s", "t_local", "receive_utc_ticks", "lat_deg", "lon_deg", "altitude_m", "fix_quality", "satellites", "hdop", "heading_deg", "pitch_deg", "roll_deg", "heading_quality", "heading_valid", "gga_utc", "hpr_utc", "hpr_measurement_utc_s", "checksum_valid", ] def _fmt(value) -> str: if value is None: return "" if isinstance(value, bool): return "1" if value else "0" if isinstance(value, float): if math.isnan(value): return "" return f"{value:.12g}" return str(value) def load_imu_clock_model( imu_csv: Path, ) -> tuple[float, float, float, float, AffineClockModel]: """Return device/host spans and a robust ``IMU device -> host UTC`` model.""" t_device: list[float] = [] t_host: list[float] = [] with imu_csv.open("r", encoding="utf-8", newline="") as handle: reader = csv.DictReader(handle) for row in reader: t_device.append(float(row["t"])) t_host.append(float(row["t_host_utc_s"])) if not t_host: raise ValueError(f"empty IMU csv: {imu_csv}") model = fit_affine_clock(np.asarray(t_device), np.asarray(t_host)) return min(t_device), max(t_device), min(t_host), max(t_host), model def sentence_host_s(row: RtkSentence) -> float: return utc_dotnet_ticks_to_unix_s(row.receive_utc_ticks) def nmea_utc_to_unix_s(value: str | None, receive_host_s: float) -> float: """Resolve NMEA ``hhmmss.s`` to the UTC day nearest host receive time.""" if value is None or not str(value).strip(): raise ValueError("missing NMEA UTC time") packed = float(value) hour = int(packed // 10000) minute = int((packed - hour * 10000) // 100) second = packed - hour * 10000 - minute * 100 if not (0 <= hour < 24 and 0 <= minute < 60 and 0.0 <= second < 60.0): raise ValueError(f"invalid NMEA UTC time: {value!r}") receive = datetime.fromtimestamp(receive_host_s, tz=timezone.utc) midnight = datetime( receive.year, receive.month, receive.day, tzinfo=timezone.utc, ).timestamp() same_day = midnight + hour * 3600 + minute * 60 + second return min( (same_day - 86400.0, same_day, same_day + 86400.0), key=lambda candidate: abs(candidate - receive_host_s), ) def sentence_measurement_utc_s(row: RtkSentence) -> float: return nmea_utc_to_unix_s( row.fields.get("position_time_utc"), sentence_host_s(row), ) def nearest_hpr(gga: RtkSentence, hpr_rows: list[RtkSentence]) -> RtkSentence | None: if not hpr_rows: return None lo, hi = 0, len(hpr_rows) - 1 target = sentence_measurement_utc_s(gga) while lo < hi: mid = (lo + hi) // 2 if sentence_measurement_utc_s(hpr_rows[mid]) < target: lo = mid + 1 else: hi = mid best = hpr_rows[lo] if lo > 0 and abs(sentence_measurement_utc_s(hpr_rows[lo - 1]) - target) < abs( sentence_measurement_utc_s(best) - target ): best = hpr_rows[lo - 1] if abs(sentence_measurement_utc_s(best) - target) > HPR_MATCH_S: return None return best def merged_row( gga: RtkSentence, hpr: RtkSentence | None, imu_to_host: AffineClockModel, ) -> dict: t_host = sentence_host_s(gga) t_measurement = sentence_measurement_utc_s(gga) t_hpr = None if hpr is None else sentence_measurement_utc_s(hpr) local = datetime.fromtimestamp(t_measurement, tz=timezone.utc).astimezone(LOCAL_TZ) fields = gga.fields hpr_fields = hpr.fields if hpr is not None else {} checksum = gga.checksum_valid and (hpr is None or hpr.checksum_valid) return { "t": imu_to_host.inverse(t_measurement), "t_measurement_utc_s": t_measurement, "t_host_utc_s": t_host, "receive_delay_s": t_host - t_measurement, "t_local": local.strftime("%Y-%m-%dT%H:%M:%S.%f")[:-3], "receive_utc_ticks": gga.receive_utc_ticks, "lat_deg": fields.get("lat_deg"), "lon_deg": fields.get("lon_deg"), "altitude_m": fields.get("altitude_m"), "fix_quality": fields.get("fix_quality"), "satellites": fields.get("satellites"), "hdop": fields.get("hdop"), "heading_deg": hpr_fields.get("heading_deg"), "pitch_deg": hpr_fields.get("pitch_deg"), "roll_deg": hpr_fields.get("roll_deg"), "heading_quality": hpr_fields.get("heading_quality"), "heading_valid": hpr_fields.get("heading_valid"), "gga_utc": fields.get("position_time_utc"), "hpr_utc": hpr_fields.get("position_time_utc"), "hpr_measurement_utc_s": t_hpr, "checksum_valid": checksum, } def write_rtk_csv(path: Path, rows: list[dict]) -> None: path.parent.mkdir(parents=True, exist_ok=True) with path.open("w", newline="", encoding="utf-8") as handle: writer = csv.DictWriter(handle, fieldnames=CSV_FIELDS) writer.writeheader() for row in rows: writer.writerow({key: _fmt(row[key]) for key in CSV_FIELDS}) def discover_windows(sessions_root: Path) -> list[Path]: windows = sorted( path for path in sessions_root.iterdir() if path.is_dir() and (path / "imu.csv").is_file() ) if not windows: raise SystemExit(f"no session dirs with imu.csv under {sessions_root}") return windows def find_default_rscap(sessions_root: Path) -> Path: parents = [sessions_root, sessions_root.parent] matches: list[Path] = [] for folder in parents: matches.extend(sorted(folder.glob("wheeltec-g90*.rscap"))) matches.extend(sorted(folder.glob("*g90*.rscap"))) if not matches: raise SystemExit(f"no G90 .rscap next to {sessions_root}") return matches[0] def _delay_summary(rows: list[dict]) -> dict[str, float] | None: if not rows: return None values = np.asarray([row["receive_delay_s"] for row in rows], dtype=np.float64) return { "median_s": float(np.median(values)), "p05_s": float(np.percentile(values, 5.0)), "p95_s": float(np.percentile(values, 95.0)), "std_s": float(np.std(values)), } def main(argv: list[str] | None = None) -> int: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--sessions-root", type=Path, required=True) parser.add_argument("--rtk-rscap", type=Path, action="append") parser.add_argument("--overwrite", action="store_true") args = parser.parse_args(argv) sessions_root = args.sessions_root.resolve() rscaps = [path.resolve() for path in (args.rtk_rscap or [find_default_rscap(sessions_root)])] for rscap in rscaps: if not rscap.is_file(): raise SystemExit(f"missing RTK capture: {rscap}") sentences: list[RtkSentence] = [] captures_meta = [] for rscap in rscaps: print(f"reading {rscap}", flush=True) capture = read_capture(rscap) print(f"chunks={len(capture.chunks)}", flush=True) sentences.extend(iter_g90_sentences(capture)) captures_meta.append(file_summary(capture)) sentences.sort(key=lambda row: row.receive_utc_ticks) gga_all = [row for row in sentences if row.sentence_type == "GGA"] hpr_all = [row for row in sentences if row.sentence_type == "GNHPR"] hpr_all.sort(key=sentence_measurement_utc_s) print( f"parsed sentences={len(sentences)} GGA={len(gga_all)} GNHPR={len(hpr_all)}", flush=True, ) summaries = { "rtk_rscap": [str(path) for path in rscaps], "captures": captures_meta, "parsed_sentences": len(sentences), "gga": len(gga_all), "gnhpr": len(hpr_all), "time_source": "NMEA measurement UTC mapped through IMU device->host affine clock", "windows": [], } for window in discover_windows(sessions_root): out_csv = window / "rtk.csv" if out_csv.exists() and not args.overwrite: raise SystemExit(f"{out_csv} exists; pass --overwrite") dev_min, dev_max, host_min, host_max, imu_to_host = load_imu_clock_model( window / "imu.csv" ) gga = [ row for row in gga_all if dev_min <= imu_to_host.inverse(sentence_measurement_utc_s(row)) <= dev_max ] hpr = [ row for row in hpr_all if dev_min - HPR_MATCH_S <= imu_to_host.inverse(sentence_measurement_utc_s(row)) <= dev_max + HPR_MATCH_S ] merged = [merged_row(row, nearest_hpr(row, hpr), imu_to_host) for row in gga] write_rtk_csv(out_csv, merged) brief = { "window": window.name, "imu_host_span_s": [host_min, host_max], "imu_device_span_s": [dev_min, dev_max], "imu_device_to_host_clock": imu_to_host.to_dict(), "rtk_receive_delay": _delay_summary(merged), "gga": len(gga), "gnhpr_in_window": len(hpr), "rows_written": len(merged), "heading_matched": sum(1 for row in merged if row["heading_deg"] is not None), "fix_quality_4_or_5": sum( 1 for row in merged if row["fix_quality"] in {4, 5} ), "out": str(out_csv), } summaries["windows"].append(brief) print(json.dumps(brief, ensure_ascii=False), flush=True) manifest = sessions_root / "rtk_export_summary.json" manifest.write_text(json.dumps(summaries, ensure_ascii=False, indent=2) + "\n", encoding="utf-8") print(f"summary: {manifest}") return 0 if __name__ == "__main__": raise SystemExit(main())