334 lines
10 KiB
Python
334 lines
10 KiB
Python
|
#!/usr/bin/env python3
|
||
|
|
import json
|
||
|
|
import math
|
||
|
|
import signal
|
||
|
|
import subprocess
|
||
|
|
import sys
|
||
|
|
import time
|
||
|
|
from typing import Any, Dict, List, Optional
|
||
|
|
|
||
|
|
|
||
|
|
# offset_ns is computed as:
|
||
|
|
|
||
|
|
# (clock_sec * 1e9 + clock_nsec) - (real_sec * 1e9 + real_nsec)
|
||
|
|
|
||
|
|
|
||
|
|
GPSPIPE_CMD = ["gpspipe", "-w", "--json"]
|
||
|
|
HOST_TAG = "ntprana"
|
||
|
|
|
||
|
|
|
||
|
|
running = True
|
||
|
|
|
||
|
|
|
||
|
|
def handle_signal(signum, frame):
|
||
|
|
global running
|
||
|
|
running = False
|
||
|
|
|
||
|
|
|
||
|
|
signal.signal(signal.SIGINT, handle_signal)
|
||
|
|
signal.signal(signal.SIGTERM, handle_signal)
|
||
|
|
|
||
|
|
|
||
|
|
def esc_tag(value: Any) -> str:
|
||
|
|
s = str(value)
|
||
|
|
return s.replace("\\", "\\\\").replace(" ", r"\ ").replace(",", r"\,").replace("=", r"\=")
|
||
|
|
|
||
|
|
|
||
|
|
def esc_field_str(value: Any) -> str:
|
||
|
|
s = str(value)
|
||
|
|
s = s.replace("\\", "\\\\").replace('"', r"\"")
|
||
|
|
return f'"{s}"'
|
||
|
|
|
||
|
|
|
||
|
|
def is_num(value: Any) -> bool:
|
||
|
|
return isinstance(value, (int, float)) and not isinstance(value, bool) and math.isfinite(value)
|
||
|
|
|
||
|
|
|
||
|
|
def bool_to_field(value: bool) -> str:
|
||
|
|
return "true" if value else "false"
|
||
|
|
|
||
|
|
|
||
|
|
def build_line(measurement: str, tags: Dict[str, Any], fields: Dict[str, Any], ts_ns: Optional[int] = None) -> Optional[str]:
|
||
|
|
clean_fields = []
|
||
|
|
for key, value in fields.items():
|
||
|
|
if value is None:
|
||
|
|
continue
|
||
|
|
if isinstance(value, bool):
|
||
|
|
clean_fields.append(f"{key}={bool_to_field(value)}")
|
||
|
|
elif isinstance(value, int) and not isinstance(value, bool):
|
||
|
|
clean_fields.append(f"{key}={value}i")
|
||
|
|
elif is_num(value):
|
||
|
|
clean_fields.append(f"{key}={value}")
|
||
|
|
else:
|
||
|
|
clean_fields.append(f"{key}={esc_field_str(value)}")
|
||
|
|
|
||
|
|
if not clean_fields:
|
||
|
|
return None
|
||
|
|
|
||
|
|
tag_str = ",".join(f"{esc_tag(k)}={esc_tag(v)}" for k, v in tags.items() if v is not None)
|
||
|
|
line = measurement
|
||
|
|
if tag_str:
|
||
|
|
line += "," + tag_str
|
||
|
|
line += " " + ",".join(clean_fields)
|
||
|
|
if ts_ns is not None:
|
||
|
|
line += f" {ts_ns}"
|
||
|
|
return line
|
||
|
|
|
||
|
|
|
||
|
|
def gps_constellation_name(gnssid: Optional[int]) -> str:
|
||
|
|
mapping = {
|
||
|
|
0: "gps",
|
||
|
|
1: "sbas",
|
||
|
|
2: "galileo",
|
||
|
|
3: "beidou",
|
||
|
|
4: "imes",
|
||
|
|
5: "qzss",
|
||
|
|
6: "glonass",
|
||
|
|
}
|
||
|
|
return mapping.get(gnssid, "unknown")
|
||
|
|
|
||
|
|
|
||
|
|
def summarize_satellites(satellites: List[Dict[str, Any]]) -> Dict[str, Any]:
|
||
|
|
out: Dict[str, Any] = {
|
||
|
|
"gps_visible": 0,
|
||
|
|
"gps_used": 0,
|
||
|
|
"sbas_visible": 0,
|
||
|
|
"sbas_used": 0,
|
||
|
|
"galileo_visible": 0,
|
||
|
|
"galileo_used": 0,
|
||
|
|
"beidou_visible": 0,
|
||
|
|
"beidou_used": 0,
|
||
|
|
"qzss_visible": 0,
|
||
|
|
"qzss_used": 0,
|
||
|
|
"glonass_visible": 0,
|
||
|
|
"glonass_used": 0,
|
||
|
|
"unknown_visible": 0,
|
||
|
|
"unknown_used": 0,
|
||
|
|
"used_signal_sum": 0.0,
|
||
|
|
"used_signal_count": 0,
|
||
|
|
"visible_signal_sum": 0.0,
|
||
|
|
"visible_signal_count": 0,
|
||
|
|
"max_signal": 0.0,
|
||
|
|
}
|
||
|
|
|
||
|
|
for sat in satellites:
|
||
|
|
constellation = gps_constellation_name(sat.get("gnssid"))
|
||
|
|
ss = sat.get("ss", 0.0)
|
||
|
|
used = bool(sat.get("used", False))
|
||
|
|
|
||
|
|
vis_key = f"{constellation}_visible"
|
||
|
|
used_key = f"{constellation}_used"
|
||
|
|
|
||
|
|
if vis_key not in out:
|
||
|
|
vis_key = "unknown_visible"
|
||
|
|
used_key = "unknown_used"
|
||
|
|
|
||
|
|
out[vis_key] += 1
|
||
|
|
if used:
|
||
|
|
out[used_key] += 1
|
||
|
|
|
||
|
|
if is_num(ss):
|
||
|
|
out["visible_signal_sum"] += float(ss)
|
||
|
|
out["visible_signal_count"] += 1
|
||
|
|
out["max_signal"] = max(out["max_signal"], float(ss))
|
||
|
|
if used:
|
||
|
|
out["used_signal_sum"] += float(ss)
|
||
|
|
out["used_signal_count"] += 1
|
||
|
|
|
||
|
|
out["avg_signal_visible"] = (
|
||
|
|
out["visible_signal_sum"] / out["visible_signal_count"] if out["visible_signal_count"] else None
|
||
|
|
)
|
||
|
|
out["avg_signal_used"] = (
|
||
|
|
out["used_signal_sum"] / out["used_signal_count"] if out["used_signal_count"] else None
|
||
|
|
)
|
||
|
|
|
||
|
|
# cleanup helper sums/counters from final output
|
||
|
|
del out["visible_signal_sum"]
|
||
|
|
del out["visible_signal_count"]
|
||
|
|
del out["used_signal_sum"]
|
||
|
|
del out["used_signal_count"]
|
||
|
|
|
||
|
|
return out
|
||
|
|
|
||
|
|
|
||
|
|
def pps_offset_ns(pps: Dict[str, Any]) -> Optional[int]:
|
||
|
|
real_sec = pps.get("real_sec")
|
||
|
|
real_nsec = pps.get("real_nsec")
|
||
|
|
clock_sec = pps.get("clock_sec")
|
||
|
|
clock_nsec = pps.get("clock_nsec")
|
||
|
|
|
||
|
|
if not all(isinstance(v, int) for v in [real_sec, real_nsec, clock_sec, clock_nsec]):
|
||
|
|
return None
|
||
|
|
|
||
|
|
real_total = real_sec * 1_000_000_000 + real_nsec
|
||
|
|
clock_total = clock_sec * 1_000_000_000 + clock_nsec
|
||
|
|
return clock_total - real_total
|
||
|
|
|
||
|
|
|
||
|
|
def main():
|
||
|
|
global running
|
||
|
|
|
||
|
|
proc = subprocess.Popen(
|
||
|
|
GPSPIPE_CMD,
|
||
|
|
stdout=subprocess.PIPE,
|
||
|
|
stderr=subprocess.PIPE,
|
||
|
|
text=True,
|
||
|
|
bufsize=1,
|
||
|
|
)
|
||
|
|
|
||
|
|
latest_tpv: Optional[Dict[str, Any]] = None
|
||
|
|
latest_sky: Optional[Dict[str, Any]] = None
|
||
|
|
latest_pps: Optional[Dict[str, Any]] = None
|
||
|
|
last_emit_sec: Optional[int] = None
|
||
|
|
# NGL at some point I just put cloudee to do it bc I've wrote simmilar code before many times
|
||
|
|
try:
|
||
|
|
assert proc.stdout is not None
|
||
|
|
while running:
|
||
|
|
line = proc.stdout.readline()
|
||
|
|
if not line:
|
||
|
|
if proc.poll() is not None:
|
||
|
|
break
|
||
|
|
time.sleep(0.05)
|
||
|
|
continue
|
||
|
|
|
||
|
|
line = line.strip()
|
||
|
|
if not line or not line.startswith("{"):
|
||
|
|
continue
|
||
|
|
|
||
|
|
try:
|
||
|
|
msg = json.loads(line)
|
||
|
|
except json.JSONDecodeError:
|
||
|
|
continue
|
||
|
|
|
||
|
|
cls = msg.get("class")
|
||
|
|
|
||
|
|
if cls == "TPV":
|
||
|
|
latest_tpv = msg
|
||
|
|
elif cls == "SKY":
|
||
|
|
if latest_sky is None:
|
||
|
|
latest_sky = msg
|
||
|
|
else:
|
||
|
|
merged = latest_sky.copy()
|
||
|
|
merged.update(msg)
|
||
|
|
|
||
|
|
# Keep the richer satellites list if the new SKY message doesn't include it
|
||
|
|
if "satellites" not in msg and "satellites" in latest_sky:
|
||
|
|
merged["satellites"] = latest_sky["satellites"]
|
||
|
|
|
||
|
|
# Keep nSat/uSat if the new SKY message doesn't include them
|
||
|
|
if "nSat" not in msg and "nSat" in latest_sky:
|
||
|
|
merged["nSat"] = latest_sky["nSat"]
|
||
|
|
|
||
|
|
if "uSat" not in msg and "uSat" in latest_sky:
|
||
|
|
merged["uSat"] = latest_sky["uSat"]
|
||
|
|
|
||
|
|
latest_sky = merged
|
||
|
|
elif cls == "PPS":
|
||
|
|
latest_pps = msg
|
||
|
|
else:
|
||
|
|
continue
|
||
|
|
|
||
|
|
now_sec = int(time.time())
|
||
|
|
if last_emit_sec == now_sec:
|
||
|
|
continue
|
||
|
|
|
||
|
|
lines_out = []
|
||
|
|
ts_ns = time.time_ns()
|
||
|
|
|
||
|
|
common_tags = {
|
||
|
|
"host": HOST_TAG,
|
||
|
|
"device": (latest_tpv or latest_sky or latest_pps or {}).get("device", "unknown"),
|
||
|
|
}
|
||
|
|
|
||
|
|
if latest_tpv:
|
||
|
|
fix_fields = {
|
||
|
|
"mode": latest_tpv.get("mode"),
|
||
|
|
"lat": latest_tpv.get("lat"),
|
||
|
|
"lon": latest_tpv.get("lon"),
|
||
|
|
"alt": latest_tpv.get("alt"),
|
||
|
|
"alt_msl": latest_tpv.get("altMSL"),
|
||
|
|
"alt_hae": latest_tpv.get("altHAE"),
|
||
|
|
"speed": latest_tpv.get("speed"),
|
||
|
|
"climb": latest_tpv.get("climb"),
|
||
|
|
"track": latest_tpv.get("track"),
|
||
|
|
"magtrack": latest_tpv.get("magtrack"),
|
||
|
|
"magvar": latest_tpv.get("magvar"),
|
||
|
|
"ept": latest_tpv.get("ept"),
|
||
|
|
"epx": latest_tpv.get("epx"),
|
||
|
|
"epy": latest_tpv.get("epy"),
|
||
|
|
"epv": latest_tpv.get("epv"),
|
||
|
|
"eph": latest_tpv.get("eph"),
|
||
|
|
"sep": latest_tpv.get("sep"),
|
||
|
|
"ecefpAcc": latest_tpv.get("ecefpAcc"),
|
||
|
|
"ecefvAcc": latest_tpv.get("ecefvAcc"),
|
||
|
|
"leapseconds": latest_tpv.get("leapseconds"),
|
||
|
|
"fix_valid": latest_tpv.get("mode", 0) >= 2,
|
||
|
|
"fix_3d": latest_tpv.get("mode", 0) == 3,
|
||
|
|
}
|
||
|
|
line_fix = build_line("gps_fix", common_tags, fix_fields, ts_ns)
|
||
|
|
if line_fix:
|
||
|
|
lines_out.append(line_fix)
|
||
|
|
|
||
|
|
if latest_sky:
|
||
|
|
satellites = latest_sky.get("satellites", [])
|
||
|
|
sat_summary = summarize_satellites(satellites) if isinstance(satellites, list) else {}
|
||
|
|
|
||
|
|
n_sat = latest_sky.get("nSat")
|
||
|
|
u_sat = latest_sky.get("uSat")
|
||
|
|
sat_used_ratio = None
|
||
|
|
if isinstance(n_sat, int) and n_sat > 0 and isinstance(u_sat, int):
|
||
|
|
sat_used_ratio = u_sat / n_sat
|
||
|
|
|
||
|
|
sky_fields = {
|
||
|
|
"nSat": n_sat,
|
||
|
|
"uSat": u_sat,
|
||
|
|
"sat_used_ratio": sat_used_ratio,
|
||
|
|
"gdop": latest_sky.get("gdop"),
|
||
|
|
"hdop": latest_sky.get("hdop"),
|
||
|
|
"pdop": latest_sky.get("pdop"),
|
||
|
|
"tdop": latest_sky.get("tdop"),
|
||
|
|
"vdop": latest_sky.get("vdop"),
|
||
|
|
"xdop": latest_sky.get("xdop"),
|
||
|
|
"ydop": latest_sky.get("ydop"),
|
||
|
|
**sat_summary,
|
||
|
|
}
|
||
|
|
line_sky = build_line("gps_sky", common_tags, sky_fields, ts_ns)
|
||
|
|
if line_sky:
|
||
|
|
lines_out.append(line_sky)
|
||
|
|
|
||
|
|
if latest_pps:
|
||
|
|
offset_ns = pps_offset_ns(latest_pps)
|
||
|
|
pps_fields = {
|
||
|
|
"offset_ns": offset_ns,
|
||
|
|
"precision_exp": latest_pps.get("precision"),
|
||
|
|
"present": True,
|
||
|
|
}
|
||
|
|
line_pps = build_line(
|
||
|
|
"gps_pps",
|
||
|
|
{
|
||
|
|
"host": HOST_TAG,
|
||
|
|
"device": latest_pps.get("device", "/dev/pps0"),
|
||
|
|
},
|
||
|
|
pps_fields,
|
||
|
|
ts_ns,
|
||
|
|
)
|
||
|
|
if line_pps:
|
||
|
|
lines_out.append(line_pps)
|
||
|
|
|
||
|
|
if lines_out:
|
||
|
|
sys.stdout.write("\n".join(lines_out) + "\n")
|
||
|
|
sys.stdout.flush()
|
||
|
|
last_emit_sec = now_sec
|
||
|
|
|
||
|
|
finally:
|
||
|
|
if proc.poll() is None:
|
||
|
|
proc.terminate()
|
||
|
|
try:
|
||
|
|
proc.wait(timeout=2)
|
||
|
|
except subprocess.TimeoutExpired:
|
||
|
|
proc.kill()
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main()
|