#!/usr/bin/env python3 """Compare stationary GPS firmware UART logs, grouped by satellite count.""" from __future__ import annotations import argparse import csv import math import re import statistics from pathlib import Path POSITION_RE = re.compile( r"position lat=(?P[-+0-9.eE]+) " r"lon=(?P[-+0-9.eE]+) height=(?P[-+0-9.eE]+)" ) RUNNING_RE = re.compile(r"navigation software running with (?P\d+) satellites") NAVSOL_RE = re.compile( r"NAVSOL valid=(?P[01]).*?fde=(?P[01]).*?" r"sats=(?P\d+)/(?P\d+)" ) def percentile(values: list[float], percentage: float) -> float: if not values: return math.nan ordered = sorted(values) position = (len(ordered) - 1) * percentage / 100.0 lower = math.floor(position) upper = math.ceil(position) if lower == upper: return ordered[lower] fraction = position - lower return ordered[lower] * (1.0 - fraction) + ordered[upper] * fraction def parse_log(path: Path) -> list[dict[str, float | int | None]]: samples: list[dict[str, float | int | None]] = [] current_satellites: int | None = None with path.open(newline="", encoding="utf-8", errors="replace") as stream: for row in csv.DictReader(stream): line = row.get("uart_line", "") running = RUNNING_RE.search(line) if running: current_satellites = int(running.group("sats")) position = POSITION_RE.search(line) if position: samples.append( { "time": float(row.get("host_monotonic_s", "nan")), "lat": float(position.group("lat")), "lon": float(position.group("lon")), "height": float(position.group("height")), "satellites": current_satellites, "used": None, "valid": None, "fde": None, } ) navsol = NAVSOL_RE.search(line) if navsol and samples: sample = samples[-1] sample["satellites"] = int(navsol.group("available")) sample["used"] = int(navsol.group("used")) sample["valid"] = int(navsol.group("valid")) sample["fde"] = int(navsol.group("fde")) current_satellites = int(navsol.group("available")) return [sample for sample in samples if sample["satellites"] is not None] def local_offsets( sample: dict[str, float | int | None], reference: tuple[float, float, float] ) -> tuple[float, float, float]: latitude, longitude, height = reference latitude_rad = math.radians(latitude) sin_latitude = math.sin(latitude_rad) eccentricity_squared = 6.69437999014e-3 semi_major_axis = 6378137.0 prime_vertical = semi_major_axis / math.sqrt( 1.0 - eccentricity_squared * sin_latitude * sin_latitude ) meridional = ( semi_major_axis * (1.0 - eccentricity_squared) / (1.0 - eccentricity_squared * sin_latitude * sin_latitude) ** 1.5 ) north = math.radians(float(sample["lat"]) - latitude) * (meridional + height) east = ( math.radians(float(sample["lon"]) - longitude) * (prime_vertical + height) * math.cos(latitude_rad) ) up = float(sample["height"]) - height return east, north, up def summarize_group( samples: list[dict[str, float | int | None]], reference: tuple[float, float, float] ) -> dict[str, float | int]: offsets = [local_offsets(sample, reference) for sample in samples] horizontal = [math.hypot(east, north) for east, north, _ in offsets] distance_3d = [math.sqrt(east * east + north * north + up * up) for east, north, up in offsets] vertical = [abs(up) for _, _, up in offsets] valid_values = [int(sample["valid"]) for sample in samples if sample["valid"] is not None] fde_values = [int(sample["fde"]) for sample in samples if sample["fde"] is not None] return { "samples": len(samples), "horizontal_mean_m": statistics.fmean(horizontal), "horizontal_p50_m": percentile(horizontal, 50.0), "horizontal_p95_m": percentile(horizontal, 95.0), "horizontal_max_m": max(horizontal), "vertical_abs_mean_m": statistics.fmean(vertical), "distance_3d_mean_m": statistics.fmean(distance_3d), "distance_3d_p95_m": percentile(distance_3d, 95.0), "distance_3d_max_m": max(distance_3d), "valid_rate_pct": 100.0 * statistics.fmean(valid_values) if valid_values else math.nan, "fde_rate_pct": 100.0 * statistics.fmean(fde_values) if fde_values else math.nan, } def main() -> int: parser = argparse.ArgumentParser() parser.add_argument("--old", type=Path, required=True) parser.add_argument("--new", type=Path, required=True) parser.add_argument("--output", type=Path, required=True) args = parser.parse_args() logs = {"old": parse_log(args.old), "new": parse_log(args.new)} if not logs["old"] or not logs["new"]: raise SystemExit("both logs must contain position samples with satellite counts") references = { name: ( statistics.median(float(sample["lat"]) for sample in samples), statistics.median(float(sample["lon"]) for sample in samples), statistics.median(float(sample["height"]) for sample in samples), ) for name, samples in logs.items() } rows: list[dict[str, str | int | float]] = [] for firmware, samples in logs.items(): groups: list[tuple[str, list[dict[str, float | int | None]]]] = [("all", samples)] groups.extend( (str(count), [sample for sample in samples if sample["satellites"] == count]) for count in sorted({int(sample["satellites"]) for sample in samples}) ) if any(sample["fde"] is not None for sample in samples): groups.extend( ( f"all_fde{fde_state}", [sample for sample in samples if sample["fde"] == fde_state], ) for fde_state in (0, 1) if any(sample["fde"] == fde_state for sample in samples) ) groups.extend( ( f"{count}_fde{fde_state}", [ sample for sample in samples if sample["satellites"] == count and sample["fde"] == fde_state ], ) for count in sorted({int(sample["satellites"]) for sample in samples}) for fde_state in (0, 1) if any( sample["satellites"] == count and sample["fde"] == fde_state for sample in samples ) ) for satellite_count, group in groups: rows.append( { "firmware": firmware, "satellites": satellite_count, **summarize_group(group, references[firmware]), } ) args.output.parent.mkdir(parents=True, exist_ok=True) with args.output.open("w", newline="", encoding="utf-8") as stream: writer = csv.DictWriter(stream, fieldnames=list(rows[0])) writer.writeheader() writer.writerows(rows) old_reference = {"lat": references["old"][0], "lon": references["old"][1], "height": references["old"][2]} new_offset = local_offsets(old_reference, references["new"]) median_separation = math.sqrt(sum(value * value for value in new_offset)) print(f"old_samples={len(logs['old'])} new_samples={len(logs['new'])}") print(f"old_reference={references['old']}") print(f"new_reference={references['new']}") print(f"median_position_separation_3d_m={median_separation:.3f}") for row in rows: print( f"{row['firmware']:>3} sats={row['satellites']:>3} n={row['samples']:>4} " f"hmean={row['horizontal_mean_m']:.2f} hp95={row['horizontal_p95_m']:.2f} " f"hmax={row['horizontal_max_m']:.2f} d3p95={row['distance_3d_p95_m']:.2f} " f"valid={row['valid_rate_pct']:.1f}% fde={row['fde_rate_pct']:.1f}%" ) print(f"wrote {args.output}") return 0 if __name__ == "__main__": raise SystemExit(main())