646 lines
23 KiB
Python
Executable File
646 lines
23 KiB
Python
Executable File
#!/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
|
||
|
||
# -------------------- 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;
|
||
"""
|
||
|
||
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")
|
||
|
||
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)
|
||
|
||
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
|
||
|
||
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 (pvt or hpp):
|
||
recorder.add_fix(pvt if pvt else last_pvt, hpp if hpp else last_hpp)
|
||
|
||
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:
|
||
rtcm_router.stop()
|
||
except: pass
|
||
try:
|
||
zed.close()
|
||
except: pass
|
||
print("✓ Beendet.")
|
||
|
||
if __name__ == "__main__":
|
||
main()
|