Add RTK rover database logging and site logger
This commit is contained in:
+176
@@ -0,0 +1,176 @@
|
||||
#!/usr/bin/env python3
|
||||
import os, time, json
|
||||
from pathlib import Path
|
||||
|
||||
def ts():
|
||||
return time.strftime("%Y-%m-%d %H:%M:%S")
|
||||
|
||||
def iso():
|
||||
return time.strftime("%Y-%m-%dT%H:%M:%S")
|
||||
|
||||
def plausible(lat, lon, h, hacc, vacc):
|
||||
if lat is None or lon is None:
|
||||
return False
|
||||
if not (-90 <= lat <= 90 and -180 <= lon <= 180):
|
||||
return False
|
||||
if h is not None and not (-500 <= h <= 10000):
|
||||
return False
|
||||
if hacc is not None and hacc > 1000:
|
||||
return False
|
||||
if vacc is not None and vacc > 2000:
|
||||
return False
|
||||
return True
|
||||
|
||||
class SiteLogger:
|
||||
def __init__(self, base_dir, project_name=None, min_fix_seconds=5.0, max_hacc_m=0.05, msl_offset=0.0):
|
||||
self.base_dir = Path(base_dir)
|
||||
self.base_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
env_project = os.environ.get("RTK_PROJECT", "").strip()
|
||||
name = project_name or env_project or time.strftime("%Y-%m-%d_%H%M%S")
|
||||
self.project_dir = self.base_dir / name
|
||||
self.project_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
self.min_fix_seconds = float(min_fix_seconds)
|
||||
self.max_hacc_m = float(max_hacc_m)
|
||||
self.msl_offset = float(msl_offset)
|
||||
|
||||
self.csv_path = self.project_dir / "track_fix.csv"
|
||||
self.geojson_path = self.project_dir / "track_fix.geojson"
|
||||
self.status_path = self.project_dir / "status.log"
|
||||
|
||||
self.fix_since = None
|
||||
self.last_mode = None
|
||||
self.last_rtcm_ok = None
|
||||
self.csv_written = False
|
||||
self.geo_started = False
|
||||
self.geo_first = True
|
||||
|
||||
self.status(f"START project_dir={self.project_dir} min_fix={self.min_fix_seconds}s max_hacc={self.max_hacc_m}m")
|
||||
|
||||
def status(self, msg):
|
||||
with open(self.status_path, "a", encoding="utf-8") as f:
|
||||
f.write(f"{ts()} {msg}\n")
|
||||
|
||||
def mode_from_pvt(self, pvt):
|
||||
if not pvt:
|
||||
return "NO NAV"
|
||||
if pvt.get("fixType", 0) < 3:
|
||||
return "NOFIX/2D"
|
||||
carr = int(pvt.get("carrSoln", 0))
|
||||
if carr == 2:
|
||||
return "RTK FIX"
|
||||
if carr == 1:
|
||||
return "RTK FLOAT"
|
||||
if pvt.get("diffSoln"):
|
||||
return "DGNSS"
|
||||
return "3D"
|
||||
|
||||
def best_position(self, pvt, hpp):
|
||||
if hpp and plausible(hpp.get("lat"), hpp.get("lon"), hpp.get("height"), hpp.get("hAcc"), hpp.get("vAcc")):
|
||||
return {
|
||||
"src": "HPPOSLLH",
|
||||
"lat": hpp.get("lat"),
|
||||
"lon": hpp.get("lon"),
|
||||
"height": hpp.get("height"),
|
||||
"hMSL": hpp.get("hMSL"),
|
||||
"hAcc": hpp.get("hAcc"),
|
||||
"vAcc": hpp.get("vAcc"),
|
||||
}
|
||||
if pvt:
|
||||
return {
|
||||
"src": "PVT",
|
||||
"lat": pvt.get("lat"),
|
||||
"lon": pvt.get("lon"),
|
||||
"height": pvt.get("height"),
|
||||
"hMSL": pvt.get("hMSL"),
|
||||
"hAcc": pvt.get("hAcc"),
|
||||
"vAcc": pvt.get("vAcc"),
|
||||
}
|
||||
return None
|
||||
|
||||
def update(self, pvt, hpp=None, rtcm_age_s=None):
|
||||
if not pvt:
|
||||
return
|
||||
|
||||
mode = self.mode_from_pvt(pvt)
|
||||
if mode != self.last_mode:
|
||||
self.status(f"MODE {mode} sats={pvt.get('numSV')} hAcc={pvt.get('hAcc')} vAcc={pvt.get('vAcc')}")
|
||||
self.last_mode = mode
|
||||
|
||||
rtcm_ok = rtcm_age_s is not None and rtcm_age_s <= 3.0
|
||||
if rtcm_ok != self.last_rtcm_ok:
|
||||
self.status(f"RTCM {'OK' if rtcm_ok else 'LOST'} age={rtcm_age_s}")
|
||||
self.last_rtcm_ok = rtcm_ok
|
||||
|
||||
now = time.time()
|
||||
if int(pvt.get("carrSoln", 0)) == 2:
|
||||
if self.fix_since is None:
|
||||
self.fix_since = now
|
||||
if now - self.fix_since < self.min_fix_seconds:
|
||||
return
|
||||
else:
|
||||
self.fix_since = None
|
||||
return
|
||||
|
||||
pos = self.best_position(pvt, hpp)
|
||||
if not pos:
|
||||
return
|
||||
|
||||
hacc = pos.get("hAcc")
|
||||
if hacc is None or float(hacc) > self.max_hacc_m:
|
||||
return
|
||||
|
||||
lat, lon = pos["lat"], pos["lon"]
|
||||
hmsl_raw = pos["hMSL"]
|
||||
hmsl_corr = hmsl_raw + self.msl_offset if hmsl_raw is not None else None
|
||||
|
||||
if not self.csv_written:
|
||||
with open(self.csv_path, "w", encoding="utf-8") as f:
|
||||
f.write("unix_ts,iso,lat,lon,hMSL_raw_m,hMSL_corr_m,hEll_m,hAcc_m,vAcc_m,fixType,carrSoln,sats,source\n")
|
||||
self.csv_written = True
|
||||
|
||||
with open(self.csv_path, "a", encoding="utf-8") as f:
|
||||
f.write(
|
||||
f"{int(now)},{iso()},{lat:.8f},{lon:.8f},"
|
||||
f"{hmsl_raw:.3f},{hmsl_corr:.3f},{pos['height']:.3f},"
|
||||
f"{pos['hAcc']:.3f},{pos['vAcc']:.3f},"
|
||||
f"{pvt.get('fixType')},{pvt.get('carrSoln')},{pvt.get('numSV')},{pos['src']}\n"
|
||||
)
|
||||
|
||||
self.write_geojson(lat, lon, hmsl_corr, pos, pvt)
|
||||
|
||||
def write_geojson(self, lat, lon, hmsl_corr, pos, pvt):
|
||||
if not self.geo_started:
|
||||
with open(self.geojson_path, "w", encoding="utf-8") as f:
|
||||
f.write('{"type":"FeatureCollection","features":[\n')
|
||||
self.geo_started = True
|
||||
self.geo_first = True
|
||||
|
||||
feat = {
|
||||
"type": "Feature",
|
||||
"geometry": {"type": "Point", "coordinates": [lon, lat, hmsl_corr]},
|
||||
"properties": {
|
||||
"ts": ts(),
|
||||
"source": pos["src"],
|
||||
"hMSL_corr_m": hmsl_corr,
|
||||
"hAcc_m": pos["hAcc"],
|
||||
"vAcc_m": pos["vAcc"],
|
||||
"fixType": pvt.get("fixType"),
|
||||
"carrSoln": pvt.get("carrSoln"),
|
||||
"sats": pvt.get("numSV"),
|
||||
}
|
||||
}
|
||||
|
||||
with open(self.geojson_path, "a", encoding="utf-8") as f:
|
||||
if not self.geo_first:
|
||||
f.write(",\n")
|
||||
f.write(json.dumps(feat, ensure_ascii=False))
|
||||
self.geo_first = False
|
||||
|
||||
def close(self):
|
||||
self.status("STOP")
|
||||
if self.geo_started:
|
||||
with open(self.geojson_path, "a", encoding="utf-8") as f:
|
||||
f.write("\n]}\n")
|
||||
self.geo_started = False
|
||||
Reference in New Issue
Block a user