209 lines
8.3 KiB
Python
209 lines
8.3 KiB
Python
#!/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<lat>[-+0-9.eE]+) "
|
|
r"lon=(?P<lon>[-+0-9.eE]+) height=(?P<height>[-+0-9.eE]+)"
|
|
)
|
|
RUNNING_RE = re.compile(r"navigation software running with (?P<sats>\d+) satellites")
|
|
NAVSOL_RE = re.compile(
|
|
r"NAVSOL valid=(?P<valid>[01]).*?fde=(?P<fde>[01]).*?"
|
|
r"sats=(?P<used>\d+)/(?P<available>\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())
|