martin/rtk-rover
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add RTK rover database logging and site logger

Browse Source
This commit is contained in:
admin
2026-04-30 13:37:44 +02:00
commit a96f354f58
4 changed files with 1018 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+18
View File
@@ -0,0 +1,18 @@
cat > .gitignore <<'EOF'
# runtime / logs / databases
*.db
*.db-*
*.sqlite
logs/
*.log
*.tap
# Python
__pycache__/
*.pyc
.venv/
venv/
# backups from patching
*.bak_*
EOF
rtk_rover_recorder.py
Executable
+788
View File
@@ -0,0 +1,788 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys, time, struct, argparse, threading, os, sqlite3, uuid, select, termios, tty
from typing import Optional, Tuple, Dict
import serial
from serial.serialutil import SerialException
from site_logger import SiteLogger
# -------------------- Defaults --------------------
DEF_ZED_PORT = "/dev/serial/by-id/usb-u-blox_AG_-_www.u-blox.com_u-blox_GNSS_receiver-if00"
DEF_ZED_BAUD = 115200
DEF_LORA_PORT = "/dev/serial/by-id/usb-1a86_USB_Serial-if00-port0"
DEF_LORA_BAUD = 9600
PRINT_EVERY_S = 1.0
RTCM_GAP_WARN = 3.0
READ_TIMEOUT_S = 0.1
REOPEN_WAIT_S = 1.0
DEF_DB_PATH = os.path.expanduser("~/rtk/rtk_rover.db")
DEF_LOG_DIR = os.path.expanduser("~/rtk/logs")
# empirischer Offset: hMSL_raw + offset = hMSL_corr
DEFAULT_MSL_OFFSET = -31.5
# -------------------- UBX helpers --------------------
def ubx_checksum(payload: bytes) -> bytes:
a = b = 0
for x in payload:
a = (a + x) & 0xFF
b = (b + a) & 0xFF
return bytes([a, b])
def send_ubx(ser: serial.Serial, cls: int, mid: int, payload: bytes = b"") -> None:
head = b"\xB5\x62" + bytes([cls, mid]) + struct.pack("<H", len(payload))
ser.write(head + payload + ubx_checksum(bytes([cls, mid]) + struct.pack("<H", len(payload)) + payload))
def read_ubx_frame(ser: serial.Serial, timeout: float = 0.5):
end = time.time() + timeout
while time.time() < end:
b = ser.read(1)
if not b:
continue
if b == b"\xB5" and ser.read(1) == b"\x62":
break
else:
return None
hdr = ser.read(4)
if len(hdr) < 4:
return None
cls, mid, length = hdr[0], hdr[1], struct.unpack("<H", hdr[2:4])[0]
payload = ser.read(length)
cks = ser.read(2)
if len(payload) != length or len(cks) != 2:
return None
if ubx_checksum(bytes([cls, mid]) + struct.pack("<H", length) + payload) != cks:
return None
return (cls, mid, payload)
def wait_ack(ser: serial.Serial, exp_cls: int, exp_id: int, timeout: float = 1.0) -> bool:
end = time.time() + timeout
while time.time() < end:
f = read_ubx_frame(ser, end - time.time())
if not f:
continue
c, m, p = f
if c == 0x05 and m in (0x00, 0x01) and len(p) == 2 and (p[0], p[1]) == (exp_cls, exp_id):
return m == 0x01
return False
# -------------------- CFG / MON --------------------
def mon_ver(ser) -> Tuple[str, str]:
send_ubx(ser, 0x0A, 0x04)
v1 = v2 = "?"
end = time.time() + 0.6
while time.time() < end:
f = read_ubx_frame(ser, 0.1)
if not f or f[0] != 0x0A or f[1] != 0x04:
continue
p = f[2]
for i in range(0, len(p), 30):
s = p[i:i+30].split(b"\x00", 1)[0].decode("ascii", errors="ignore")
if s.startswith(("ROM", "EXT", "PROT")): v1 = s
if s.startswith("FWVER"): v2 = s
return v1, v2
def get_cfg_tmode3(ser):
send_ubx(ser, 0x06, 0x71)
f = read_ubx_frame(ser, 0.8)
if not f or f[0] != 0x06 or f[1] != 0x71 or len(f[2]) < 40:
return None
p = f[2]
mode = p[2]
return (mode, p)
def set_tmode3_disabled(ser) -> bool:
payload = bytearray(40)
payload[0] = 0
payload[2] = 0
send_ubx(ser, 0x06, 0x71, payload)
return wait_ack(ser, 0x06, 0x71, 1.0)
def set_nav_rate_1hz(ser):
payload = struct.pack("<HHH", 1000, 1, 0)
send_ubx(ser, 0x06, 0x08, payload)
wait_ack(ser, 0x06, 0x08, 0.5)
def quiet_nmea_usb(ser, usb_rate=0):
for mid in [0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09]:
payload = bytes([0xF0, mid, 0,0,0, usb_rate, 0])
send_ubx(ser, 0x06, 0x01, payload)
wait_ack(ser, 0x06, 0x01, 0.3)
def enable_nav_msgs_on_usb(ser):
for (cls, mid) in [(0x01,0x07), (0x01,0x14)]: # NAV-PVT, HPPOSLLH
payload = bytes([cls, mid, 0,0,0, 1, 0]) # USB index=3 -> rate=1
send_ubx(ser, 0x06, 0x01, payload)
wait_ack(ser, 0x06, 0x01, 0.5)
# -------------------- NAV Parser --------------------
def parse_nav_pvt(p):
if len(p) < 92: return None
iTOW = struct.unpack_from("<I", p, 0)[0]
year, month, day = struct.unpack_from("<HBB", p, 4)
hour, minute, second = struct.unpack_from("<BBB", p, 6)
fixType = p[20]
flags = p[21]
numSV = p[23]
lon = struct.unpack_from("<i", p, 24)[0] * 1e-7
lat = struct.unpack_from("<i", p, 28)[0] * 1e-7
height = struct.unpack_from("<i", p, 32)[0] / 1000.0
hMSL = struct.unpack_from("<i", p, 36)[0] / 1000.0
hAcc = struct.unpack_from("<I", p, 40)[0] / 1000.0
vAcc = struct.unpack_from("<I", p, 44)[0] / 1000.0
carrSoln = (p[21] >> 6) & 0x03 # 0 none,1 float,2 fix
return dict(iTOW=iTOW, time=(year,month,day,hour,minute,second),
fixType=fixType, gnssFixOK=(flags&1)!=0, diffSoln=(flags&2)!=0,
carrSoln=carrSoln, numSV=numSV, lat=lat, lon=lon,
height=height, hMSL=hMSL, hAcc=hAcc, vAcc=vAcc)
def parse_nav_hpposllh(p):
if len(p) < 36: return None
iTOW = struct.unpack_from("<I", p, 0)[0]
lon = struct.unpack_from("<i", p, 4)[0] * 1e-7
lat = struct.unpack_from("<i", p, 8)[0] * 1e-7
height = struct.unpack_from("<i", p, 12)[0] / 1000.0
hMSL = struct.unpack_from("<i", p, 16)[0] / 1000.0
lonHP = struct.unpack_from("<b", p, 20)[0] * 1e-9
latHP = struct.unpack_from("<b", p, 21)[0] * 1e-9
hHP = struct.unpack_from("<b", p, 22)[0] / 1000.0
hMSLHP = struct.unpack_from("<b", p, 23)[0] / 1000.0
hAcc = struct.unpack_from("<I", p, 24)[0] / 10000.0
vAcc = struct.unpack_from("<I", p, 28)[0] / 10000.0
return dict(iTOW=iTOW, lat=lat+latHP, lon=lon+lonHP,
height=height+hHP, hMSL=hMSL+hMSLHP, hAcc=hAcc, vAcc=vAcc)
# -------------------- RTCM Prüfer / Router --------------------
CRC24Q_POLY = 0x1864CFB
def crc24q(data: bytes) -> int:
crc = 0
for b in data:
crc ^= (b << 16)
for _ in range(8):
crc <<= 1
if crc & 0x1000000:
crc ^= CRC24Q_POLY
return crc & 0xFFFFFF
def parse_rtcm3_header_and_type(buf: bytes) -> Optional[Tuple[int,int]]:
if len(buf) < 6 or buf[0] != 0xD3:
return None
length = ((buf[1] & 0x03) << 8) | buf[2]
need = 3 + length + 3
if len(buf) < need:
return None
body = buf[:3+length]
crc_rx = (buf[3+length] << 16) | (buf[3+length+1] << 8) | buf[3+length+2]
if crc24q(body) != crc_rx:
return (-1, need)
msgnum = ((buf[3] & 0xFC) >> 2) << 8 | buf[4]
return (msgnum, need)
class RTCMRouter(threading.Thread):
def __init__(self, lora_port, lora_baud, zed_ser: serial.Serial, stats: Dict):
super().__init__(daemon=True)
self.lora_port = lora_port
self.lora_baud = lora_baud
self.zed_ser = zed_ser
self.stats = stats
self.stop_flag = threading.Event()
self.ser = None
self.buf = bytearray()
def open_lora(self):
if self.ser and self.ser.is_open: return
self.ser = serial.Serial(self.lora_port, self.lora_baud, timeout=READ_TIMEOUT_S)
def close_lora(self):
if self.ser:
try: self.ser.close()
except: pass
self.ser = None
def run(self):
while not self.stop_flag.is_set():
try:
if not self.ser or not self.ser.is_open:
self.open_lora()
data = self.ser.read(512)
if data:
self.stats["lora_bytes"] = self.stats.get("lora_bytes", 0) + len(data)
self.buf.extend(data)
i = 0
while i < len(self.buf):
if self.buf[i] != 0xD3:
i += 1
continue
parsed = parse_rtcm3_header_and_type(self.buf[i:])
if not parsed:
break
msgnum, framelen = parsed
frame = self.buf[i:i+framelen]
if msgnum == -1:
self.stats["rtcm_crc_err"] = self.stats.get("rtcm_crc_err", 0) + 1
i += framelen
continue
self.stats["rtcm_last_ok"] = time.time()
self.stats["rtcm_count"] = self.stats.get("rtcm_count", 0) + 1
self.stats["rtcm_preambles"] = self.stats.get("rtcm_preambles", 0) + 1
ty = self.stats.setdefault("rtcm_types", {})
ty[msgnum] = ty.get(msgnum, 0) + 1
try:
self.zed_ser.write(frame)
except SerialException:
pass
i += framelen
if i > 0:
del self.buf[:i]
else:
time.sleep(0.01)
except SerialException:
self.close_lora()
time.sleep(REOPEN_WAIT_S)
except Exception:
time.sleep(0.05)
def stop(self):
self.stop_flag.set()
self.close_lora()
# -------------------- Plausibility Filter --------------------
def is_plausible_fix(lat: Optional[float], lon: Optional[float], h_m: Optional[float],
hacc_m: Optional[float], vacc_m: Optional[float]) -> bool:
if lat is None or lon is None:
return False
if not (-90.0 <= lat <= 90.0):
return False
if not (-180.0 <= lon <= 180.0):
return False
if h_m is not None and not (-500.0 <= h_m <= 10000.0):
return False
if hacc_m is not None and hacc_m > 1000.0:
return False
if vacc_m is not None and vacc_m > 2000.0:
return False
return True
# -------------------- Recording / DB --------------------
SCHEMA_SQL = """
PRAGMA journal_mode=WAL;
PRAGMA synchronous=NORMAL;
CREATE TABLE IF NOT EXISTS sessions (
session_id TEXT PRIMARY KEY,
start_ts REAL NOT NULL,
end_ts REAL,
device_id TEXT,
note TEXT
);
CREATE TABLE IF NOT EXISTS gnss_pvt (
id INTEGER PRIMARY KEY AUTOINCREMENT,
session_id TEXT NOT NULL,
ts_utc TEXT,
unix_ts REAL NOT NULL,
iTOW_ms INTEGER,
lat REAL,
lon REAL,
height_m REAL,
hmsl_m REAL,
hmsl_corr_m REAL,
hacc_m REAL,
vacc_m REAL,
fix_type INTEGER,
carr_soln INTEGER,
diff_soln INTEGER,
gnss_ok INTEGER,
num_sats INTEGER,
source TEXT,
synced INTEGER DEFAULT 0,
FOREIGN KEY(session_id) REFERENCES sessions(session_id)
);
CREATE INDEX IF NOT EXISTS idx_gnss_session_time ON gnss_pvt(session_id, unix_ts);
CREATE INDEX IF NOT EXISTS idx_gnss_unsynced ON gnss_pvt(synced) WHERE synced=0;
-- Future processing / construction workflow tables
CREATE TABLE IF NOT EXISTS gnss_pvt_clean (
id INTEGER PRIMARY KEY AUTOINCREMENT,
raw_id INTEGER,
session_id TEXT NOT NULL,
ts_utc TEXT,
unix_ts REAL NOT NULL,
iTOW_ms INTEGER,
lat REAL,
lon REAL,
height_m REAL,
hmsl_m REAL,
hmsl_corr_m REAL,
hacc_m REAL,
vacc_m REAL,
fix_type INTEGER,
carr_soln INTEGER,
diff_soln INTEGER,
gnss_ok INTEGER,
num_sats INTEGER,
source TEXT,
quality_score REAL,
processing_run_id INTEGER,
FOREIGN KEY(session_id) REFERENCES sessions(session_id),
FOREIGN KEY(raw_id) REFERENCES gnss_pvt(id)
);
CREATE INDEX IF NOT EXISTS idx_clean_session_time ON gnss_pvt_clean(session_id, unix_ts);
CREATE TABLE IF NOT EXISTS processing_runs (
run_id INTEGER PRIMARY KEY AUTOINCREMENT,
session_id TEXT NOT NULL,
started_ts REAL NOT NULL,
finished_ts REAL,
algorithm TEXT,
parameters_json TEXT,
note TEXT,
FOREIGN KEY(session_id) REFERENCES sessions(session_id)
);
CREATE TABLE IF NOT EXISTS processing_flags (
id INTEGER PRIMARY KEY AUTOINCREMENT,
session_id TEXT NOT NULL,
raw_id INTEGER,
flag_type TEXT NOT NULL,
severity TEXT,
reason TEXT,
created_ts REAL NOT NULL,
processing_run_id INTEGER,
FOREIGN KEY(session_id) REFERENCES sessions(session_id),
FOREIGN KEY(raw_id) REFERENCES gnss_pvt(id)
);
CREATE INDEX IF NOT EXISTS idx_flags_session_raw ON processing_flags(session_id, raw_id);
CREATE TABLE IF NOT EXISTS roll_segments (
roll_id INTEGER PRIMARY KEY AUTOINCREMENT,
session_id TEXT NOT NULL,
start_ts REAL NOT NULL,
end_ts REAL,
start_raw_id INTEGER,
end_raw_id INTEGER,
source TEXT NOT NULL DEFAULT 'auto',
confidence REAL,
label TEXT,
note TEXT,
created_ts REAL NOT NULL,
FOREIGN KEY(session_id) REFERENCES sessions(session_id)
);
CREATE INDEX IF NOT EXISTS idx_roll_segments_session_time ON roll_segments(session_id, start_ts, end_ts);
CREATE TABLE IF NOT EXISTS manual_events (
event_id INTEGER PRIMARY KEY AUTOINCREMENT,
session_id TEXT,
unix_ts REAL NOT NULL,
event_type TEXT NOT NULL,
label TEXT,
note TEXT,
lat REAL,
lon REAL,
hmsl_corr_m REAL,
created_ts REAL NOT NULL,
FOREIGN KEY(session_id) REFERENCES sessions(session_id)
);
CREATE INDEX IF NOT EXISTS idx_manual_events_session_time ON manual_events(session_id, unix_ts);
"""
def iso_utc_from_pvt_time(tup) -> Optional[str]:
try:
y,mo,d,hh,mm,ss = tup
return f"{y:04d}-{mo:02d}-{d:02d}T{hh:02d}:{mm:02d}:{ss:02d}Z"
except Exception:
return None
class Recorder:
def __init__(self, db_path: str, device_id: str = None, log_dir: Optional[str] = None,
csv: bool = False, msl_offset: float = 0.0):
self.db_path = db_path
self.device_id = device_id
self.log_dir = log_dir
self.csv_enabled = csv
self.msl_offset = msl_offset
self.conn = sqlite3.connect(self.db_path, check_same_thread=False)
self.conn.execute("PRAGMA foreign_keys=ON;")
self.conn.executescript(SCHEMA_SQL)
self.conn.commit()
self.session_id: Optional[str] = None
self.csv_fh = None
if self.log_dir:
os.makedirs(self.log_dir, exist_ok=True)
self.lock = threading.Lock()
def is_recording(self) -> bool:
return self.session_id is not None
def start(self, note: str = None):
with self.lock:
if self.session_id:
return
sid = str(uuid.uuid4())
now = time.time()
self.conn.execute(
"INSERT INTO sessions(session_id,start_ts,device_id,note) VALUES(?,?,?,?)",
(sid, now, self.device_id, note)
)
self.conn.commit()
self.session_id = sid
if self.csv_enabled and self.log_dir:
fn = os.path.join(self.log_dir, f"session_{sid}.csv")
self.csv_fh = open(fn, "w", encoding="utf-8")
self.csv_fh.write("unix_ts,ts_utc,iTOW_ms,lat,lon,height_m,hmsl_m,hmsl_corr_m,hacc_m,vacc_m,fix_type,carr_soln,diff_soln,gnss_ok,num_sats,source\n")
self.csv_fh.flush()
def stop(self):
with self.lock:
if not self.session_id:
return
now = time.time()
self.conn.execute("UPDATE sessions SET end_ts=? WHERE session_id=?", (now, self.session_id))
self.conn.commit()
self.session_id = None
if self.csv_fh:
try: self.csv_fh.close()
except: pass
self.csv_fh = None
def add_fix(self, pvt: Optional[dict], hpp: Optional[dict]):
with self.lock:
if not self.session_id:
return
unix_ts = time.time()
ts_utc = iso_utc_from_pvt_time(pvt["time"]) if pvt else None
iTOW = int(pvt["iTOW"]) if pvt else (int(hpp["iTOW"]) if hpp else None)
use_hp = False
if hpp:
use_hp = is_plausible_fix(hpp.get("lat"), hpp.get("lon"), hpp.get("height"), hpp.get("hAcc"), hpp.get("vAcc"))
if use_hp:
lat, lon = hpp["lat"], hpp["lon"]
height, hmsl = hpp["height"], hpp["hMSL"]
hacc, vacc = hpp["hAcc"], hpp["vAcc"]
source = "HPPOSLLH"
elif pvt:
lat, lon = pvt["lat"], pvt["lon"]
height, hmsl = pvt["height"], pvt["hMSL"]
hacc, vacc = pvt["hAcc"], pvt["vAcc"]
source = "PVT"
else:
return
hmsl_corr = (hmsl + self.msl_offset) if (hmsl is not None) else None
fix_type = int(pvt["fixType"]) if pvt else None
carr_soln = int(pvt["carrSoln"]) if pvt else None
diff_soln = int(1 if pvt and pvt.get("diffSoln") else 0) if pvt else None
gnss_ok = int(1 if pvt and pvt.get("gnssFixOK") else 0) if pvt else None
num_sats = int(pvt["numSV"]) if pvt else None
self.conn.execute(
"""INSERT INTO gnss_pvt(session_id,ts_utc,unix_ts,iTOW_ms,lat,lon,height_m,hmsl_m,hmsl_corr_m,hacc_m,vacc_m,
fix_type,carr_soln,diff_soln,gnss_ok,num_sats,source)
VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?)""",
(self.session_id, ts_utc, unix_ts, iTOW, lat, lon, height, hmsl, hmsl_corr, hacc, vacc,
fix_type, carr_soln, diff_soln, gnss_ok, num_sats, source)
)
self.conn.commit()
if self.csv_fh:
self.csv_fh.write(f"{unix_ts},{ts_utc or ''},{iTOW or ''},{lat},{lon},{height},{hmsl},{hmsl_corr},{hacc},{vacc},{fix_type},{carr_soln},{diff_soln},{gnss_ok},{num_sats},{source}\n")
self.csv_fh.flush()
def close(self):
with self.lock:
try:
if self.session_id:
self.stop()
finally:
try: self.conn.close()
except: pass
# -------------------- UI helpers --------------------
def fix_type_str(pvt) -> str:
if not pvt: return "—"
base = {0:"NO FIX",1:"DEAD RECK",2:"2D",3:"3D",4:"GNSS+DR",5:"TIME"}.get(pvt["fixType"], "?")
if pvt["carrSoln"] == 2: return base + " / RTK FIX"
if pvt["carrSoln"] == 1: return base + " / RTK FLOAT"
if pvt["diffSoln"]: return base + " / DGNSS"
return base
class StdinKeyReader:
def __init__(self):
self.fd = sys.stdin.fileno()
self.old = termios.tcgetattr(self.fd)
tty.setcbreak(self.fd)
def read_key(self) -> Optional[str]:
r, _, _ = select.select([sys.stdin], [], [], 0)
if r:
return sys.stdin.read(1)
return None
def close(self):
termios.tcsetattr(self.fd, termios.TCSADRAIN, self.old)
# -------------------- Main --------------------
def main():
ap = argparse.ArgumentParser(description="RTK Rover Recorder (ZED-F9P + LoRa + SQLite)")
ap.add_argument("--zed", default=DEF_ZED_PORT)
ap.add_argument("--zed-baud", type=int, default=DEF_ZED_BAUD)
ap.add_argument("--lora", default=DEF_LORA_PORT)
ap.add_argument("--lora-baud", type=int, default=DEF_LORA_BAUD)
ap.add_argument("--print-every", type=float, default=PRINT_EVERY_S)
ap.add_argument("--db", default=DEF_DB_PATH)
ap.add_argument("--device-id", default=None)
ap.add_argument("--log-dir", default=DEF_LOG_DIR)
ap.add_argument("--csv", action="store_true")
ap.add_argument("--msl-offset", type=float, default=DEFAULT_MSL_OFFSET,
help="Apply correction: hMSL_corr = hMSL_raw + msl_offset")
ap.add_argument("--auto-record", action="store_true",
help="Start SQLite recording automatically on service start")
ap.add_argument("--record-note", default="auto",
help="Note stored in sessions table when --auto-record is used")
ap.add_argument("--site-log", action="store_true",
help="Enable construction-site FIX-only CSV/GeoJSON/status logging")
ap.add_argument("--site-log-dir", default="/var/log/rtk/projects",
help="Base directory for construction-site logs")
ap.add_argument("--site-project", default=None,
help="Project/session folder name; default timestamp or RTK_PROJECT env")
ap.add_argument("--site-min-fix-seconds", type=float, default=5.0,
help="Require continuous RTK FIX for N seconds before logging")
ap.add_argument("--site-max-hacc", type=float, default=0.05,
help="Only log fixes with hAcc <= this value in meters")
args = ap.parse_args()
print(f"🔌 Öffne ZED {args.zed} @ {args.zed_baud} …")
try:
zed = serial.Serial(args.zed, args.zed_baud, timeout=READ_TIMEOUT_S)
except Exception as e:
print(f"❌ Konnte ZED-Port nicht öffnen: {e}"); sys.exit(1)
print("✅ ZED-Port offen.")
print("🔎 Diagnose…")
v1, v2 = mon_ver(zed)
print(f" MON-VER: ('{v1}', '{v2}')")
st0 = get_cfg_tmode3(zed)
print(f" TMODE3 vorab: {st0}")
print("\n↩️ TMODE3=DISABLED (Rover)")
print(f" ACK: {'OK' if set_tmode3_disabled(zed) else 'kein ACK'}")
print("⚙️ NAV-Ausgaben (USB) setzen: NAV-PVT + HPPOSLLH (USB=1)")
enable_nav_msgs_on_usb(zed)
set_nav_rate_1hz(zed)
quiet_nmea_usb(zed, usb_rate=0)
stats = {"rtcm_last_ok": 0.0, "rtcm_count": 0, "rtcm_types": {},
"lora_bytes": 0, "rtcm_crc_err": 0, "rtcm_preambles": 0}
rtcm_router = RTCMRouter(args.lora, args.lora_baud, zed, stats)
try:
rtcm_router.start()
except Exception as e:
print(f"⚠️ Konnte LoRa-Thread nicht starten: {e}")
recorder = Recorder(args.db, device_id=args.device_id, log_dir=args.log_dir, csv=args.csv,
msl_offset=args.msl_offset)
if args.auto_record:
recorder.start(note=args.record_note)
print(f"⏺️ SQLite Auto-Recording START (session={recorder.session_id})")
site_logger = None
if args.site_log:
site_logger = SiteLogger(
base_dir=args.site_log_dir,
project_name=args.site_project,
min_fix_seconds=args.site_min_fix_seconds,
max_hacc_m=args.site_max_hacc,
msl_offset=args.msl_offset,
)
print(f"🏗️ Baustellen-Logging aktiv → {site_logger.project_dir}")
print(f"\n🧭 msl_offset={args.msl_offset:+.3f} m (hMSL_corr = hMSL_raw + offset)")
print("\n🎛️ Steuerung: 'r' = Recording Start/Stop | 'q' = Quit (sauber)")
print(f"🗃️ DB: {args.db}")
if args.csv:
print(f"🧾 CSV: aktiv → {args.log_dir}")
keyr = None
try:
keyr = StdinKeyReader()
except Exception:
print("⚠️ Kein TTY – Keyboard-Steuerung deaktiviert (normal im systemd Service).")
last_print = 0.0
last_nav_seen = 0.0
first_poll_deadline = time.time() + 3.0
last_pvt = None
last_hpp = None
last_recorded_itow = None
try:
while True:
if keyr:
k = keyr.read_key()
if k:
if k.lower() == "q":
break
if k.lower() == "r":
if recorder.is_recording():
recorder.stop()
print("⏹️ Recording STOP")
else:
recorder.start(note="manual")
print(f"⏺️ Recording START (session={recorder.session_id})")
f = read_ubx_frame(zed, timeout=0.05)
pvt = None
hpp = None
if f:
frames = [f]
t_end = time.time() + 0.04
while time.time() < t_end:
nf = read_ubx_frame(zed, timeout=0.01)
if not nf:
break
frames.append(nf)
for c, m, p in frames:
if (c, m) == (0x01, 0x07):
pvt = parse_nav_pvt(p); last_nav_seen = time.time()
elif (c, m) == (0x01, 0x14):
hpp = parse_nav_hpposllh(p); last_nav_seen = time.time()
if pvt: last_pvt = pvt
if hpp: last_hpp = hpp
if time.time() > first_poll_deadline and last_nav_seen == 0.0:
send_ubx(zed, 0x01, 0x07)
first_poll_deadline = float("inf")
if recorder.is_recording() and (last_pvt or last_hpp):
current_itow = None
if last_pvt:
current_itow = last_pvt.get("iTOW")
elif last_hpp:
current_itow = last_hpp.get("iTOW")
if current_itow is not None and current_itow != last_recorded_itow:
try:
recorder.add_fix(last_pvt, last_hpp)
last_recorded_itow = current_itow
except Exception as e:
print(f"DB add_fix ERROR: {e}", flush=True)
if site_logger is not None and pvt:
rtcm_age_s = None
if stats.get("rtcm_count", 0) > 0:
rtcm_age_s = time.time() - stats.get("rtcm_last_ok", 0.0)
site_logger.update(last_pvt, last_hpp, rtcm_age_s=rtcm_age_s)
now = time.time()
if now - last_print >= args.print_every:
last_print = now
if stats["rtcm_count"] == 0:
rtcm_info = f"RTCM: (noch nichts empfangen) | LoRa-Bytes={stats['lora_bytes']}"
else:
dt = now - stats["rtcm_last_ok"]
warn = " ⚠️" if dt > RTCM_GAP_WARN else ""
rtcm_info = f"RTCM: ok (letzte gültige vor {dt:.1f}s){warn} | Frames={stats['rtcm_count']} | CRC-Err={stats['rtcm_crc_err']} | LoRa-Bytes={stats['lora_bytes']}"
p = last_pvt
hp = last_hpp
fixs = fix_type_str(p) if p else "—"
use_hp = False
if hp:
use_hp = is_plausible_fix(hp.get("lat"), hp.get("lon"), hp.get("height"), hp.get("hAcc"), hp.get("vAcc"))
if use_hp:
lat, lon = hp["lat"], hp["lon"]
h_msl_raw = hp["hMSL"]
h_ell = hp["height"]
hacc, vacc = hp["hAcc"], hp["vAcc"]
src = "HPPOSLLH"
elif p:
lat, lon = p["lat"], p["lon"]
h_msl_raw = p["hMSL"]
h_ell = p["height"]
hacc, vacc = p["hAcc"], p["vAcc"]
src = "PVT"
else:
lat = lon = None
h_msl_raw = h_ell = None
hacc = vacc = None
src = "—"
h_msl_corr = (h_msl_raw + args.msl_offset) if (h_msl_raw is not None) else None
N = (h_ell - h_msl_raw) if (h_ell is not None and h_msl_raw is not None) else None
rec = "REC" if recorder.is_recording() else "idle"
print("—"*72)
print(rtcm_info)
if last_nav_seen == 0.0:
print("UBX: (noch keine NAV-Daten gesehen) – prüfe CFG-MSG (USB) / Kabel")
else:
ago = now - last_nav_seen
if ago > 3.0:
print(f"UBX: letzte NAV vor {ago:.1f}s ⚠️")
if p:
print(f"[{rec}] Fix: {fixs} | Sats: {p['numSV']} | hAcc: {hacc:.3f} m | vAcc: {vacc:.3f} m | src={src}")
else:
print(f"[{rec}] Fix: — | src={src}")
if lat is not None:
print(f"Pos: lat={lat:.7f}, lon={lon:.7f}, hMSL_raw={h_msl_raw:.3f} m | hMSL_corr={h_msl_corr:.3f} m | hEll={h_ell:.3f} m | N={N:.3f} m")
else:
print("Pos: —")
time.sleep(0.02)
except KeyboardInterrupt:
pass
finally:
print("\n⏹️ Stoppe …")
try:
if keyr: keyr.close()
except: pass
try:
recorder.close()
except: pass
try:
if site_logger is not None:
site_logger.close()
except: pass
try:
rtcm_router.stop()
except: pass
try:
zed.close()
except: pass
print("✓ Beendet.")
if __name__ == "__main__":
main()
site_logger.py
+176
View File
@@ -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
systemd/rtk-rover.service
+36
View File
@@ -0,0 +1,36 @@
[Unit]
Description=RTK Rover Recorder (ZED-F9P + LoRa + SQLite)
After=network-online.target
Wants=network-online.target
[Service]
Type=simple
User=naue
WorkingDirectory=/home/naue/rtk
EnvironmentFile=-/home/naue/rtk/rtk_rover.env
Environment=PYTHONUNBUFFERED=1
ExecStart=/usr/bin/python3 /home/naue/rtk/rtk_rover_recorder.py \
--zed /dev/serial/by-id/usb-u-blox_AG_-_www.u-blox.com_u-blox_GNSS_receiver-if00 \
--zed-baud 115200 \
--lora /dev/serial/by-id/usb-1a86_USB_Serial-if00-port0 \
--lora-baud 9600 --db /home/naue/rtk/rtk_rover.db \
--log-dir /home/naue/rtk/logs \
--device-id ${DEVICE_ID} --msl-offset 0 \
--site-log \
--site-project testlauf_001 \
--site-min-fix-seconds 5 \
--site-max-hacc 0.05 \
--auto-record \
--record-note "systemd auto recording"
Restart=always
RestartSec=2
TimeoutStopSec=10
StandardOutput=journal
StandardError=journal
NoNewPrivileges=true
PrivateTmp=true
[Install]
WantedBy=multi-user.target