rtk-rover/site_logger.py

#!/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