Initial working RTK base setup

This commit is contained in:
2026-04-30 09:47:09 +02:00
commit f7460acbed
9 changed files with 2219 additions and 0 deletions
+603
View File
@@ -0,0 +1,603 @@
#!/usr/bin/env python3
from __future__ import annotations
import os
import time
import json
import socket
import serial
import argparse
import signal
import struct
from pathlib import Path
from typing import Optional, Dict, Tuple, List
# ============================================================
# Defaults
# ============================================================
ZED_PORT_DEFAULT = "/dev/serial/by-id/usb-u-blox_AG_-_www.u-blox.com_u-blox_GNSS_receiver-if00"
ZED_BAUD_DEFAULT = 115200
LORA_PORT_DEFAULT = "/dev/serial/by-id/usb-1a86_USB_Serial-if00-port0"
LORA_BAUD_DEFAULT = 9600 # <-- wichtig: LoRa UART ist bei dir 9600
STATE_PATH_DEFAULT = "/run/rtk/state.json"
SVIN_TARGET_DUR_DEFAULT = 120
SVIN_TARGET_ACC_DEFAULT = 11.0
# Read/Loop tuning
READ_IDLE_SLEEP_S = 0.002
STATE_WRITE_INTERVAL_S = 1.0
HOSTNAME = socket.gethostname()
# ============================================================
# State / Files
# ============================================================
def ensure_dir(path: str):
Path(path).mkdir(parents=True, exist_ok=True)
def ensure_parent_dir(file_path: str):
parent = os.path.dirname(file_path) or "."
ensure_dir(parent)
def write_state(path: str, data: dict):
ensure_parent_dir(path)
tmp = path + ".tmp"
with open(tmp, "w", encoding="utf-8") as f:
json.dump(data, f, ensure_ascii=False)
os.replace(tmp, path)
def now_ts() -> str:
return time.strftime("%Y-%m-%d %H:%M:%S")
# ============================================================
# Serial
# ============================================================
def open_serial(port: str, baud: int) -> serial.Serial:
print(f"🔌 Öffne {port} @ {baud} …")
s = serial.Serial(port, baud, timeout=0) # non-blocking
try:
s.dtr = True
s.rts = False
except Exception:
pass
try:
s.reset_input_buffer()
except Exception:
pass
print("✅ Port offen.")
return s
# ============================================================
# UBX helpers
# ============================================================
def ubx_checksum(payload: bytes) -> bytes:
ck_a = 0
ck_b = 0
for b in payload:
ck_a = (ck_a + b) & 0xFF
ck_b = (ck_b + ck_a) & 0xFF
return bytes([ck_a, ck_b])
def ubx_build(msg_class: int, msg_id: int, payload: bytes) -> bytes:
length = len(payload)
hdr = bytes([0xB5, 0x62, msg_class & 0xFF, msg_id & 0xFF, length & 0xFF, (length >> 8) & 0xFF])
body = bytes([msg_class & 0xFF, msg_id & 0xFF, length & 0xFF, (length >> 8) & 0xFF]) + payload
cks = ubx_checksum(body)
return hdr + payload + cks
def ubx_send(ser: serial.Serial, msg_class: int, msg_id: int, payload: bytes = b""):
ser.write(ubx_build(msg_class, msg_id, payload))
def ubx_cfg_msg(ser: serial.Serial, msg_class: int, msg_id: int, rate_usb: int):
"""
UBX-CFG-MSG (0x06 0x01) legacy: payload = class, id, rateI2C, rateUART1, rateUART2, rateUSB, rateSPI
"""
payload = bytes([msg_class & 0xFF, msg_id & 0xFF, 0x00, 0x00, 0x00, rate_usb & 0xFF, 0x00])
ubx_send(ser, 0x06, 0x01, payload)
# ============================================================
# RTCM Tap (binary file + rotation)
# ============================================================
class RtcmTap:
def __init__(self, path: Optional[str], rotate_mb: int = 0, max_files: int = 5):
self.path = path
self.rotate_bytes = int(rotate_mb) * 1024 * 1024 if rotate_mb and rotate_mb > 0 else 0
self.max_files = max(1, int(max_files))
self._fh = None
self.ok = False
self.bytes_written = 0
if self.path:
self._open_append()
def _open_append(self):
try:
ensure_parent_dir(self.path) # type: ignore[arg-type]
self._fh = open(self.path, "ab", buffering=0)
self.ok = True
except Exception as e:
print(f"⚠️ RTCM tap open failed ({self.path}): {e}")
self._fh = None
self.ok = False
def _rotate_if_needed(self):
if not self.path or not self.rotate_bytes or not self.ok:
return
try:
size = os.path.getsize(self.path)
except Exception:
return
if size < self.rotate_bytes:
return
try:
if self._fh:
self._fh.close()
except Exception:
pass
self._fh = None
for i in range(self.max_files - 1, 0, -1):
src = f"{self.path}.{i}"
dst = f"{self.path}.{i+1}"
if os.path.exists(src):
try:
if os.path.exists(dst):
os.remove(dst)
except Exception:
pass
try:
os.rename(src, dst)
except Exception:
pass
try:
dst1 = f"{self.path}.1"
if os.path.exists(dst1):
os.remove(dst1)
os.rename(self.path, dst1)
print(f"🌀 RTCM tap rotated: {self.path} -> {dst1}")
except Exception as e:
print(f"⚠️ RTCM tap rotate failed: {e}")
self._open_append()
def write(self, frame: bytes):
if not self.path or not self.ok or self._fh is None:
return
try:
self._fh.write(frame)
self.bytes_written += len(frame)
except Exception as e:
print(f"⚠️ RTCM tap write failed: {e}")
self.ok = False
try:
self._fh.close()
except Exception:
pass
self._fh = None
return
self._rotate_if_needed()
def close(self):
try:
if self._fh:
self._fh.close()
except Exception:
pass
self._fh = None
# ============================================================
# RTCM3 CRC24Q
# ============================================================
_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 rtcm_msg_type(payload: bytes) -> Optional[int]:
if len(payload) < 2:
return None
return (payload[0] << 4) | (payload[1] >> 4)
# ============================================================
# Stream Demux: RTCM + UBX from the same serial stream
# ============================================================
class StreamDemux:
"""
Extract RTCM3 frames (0xD3...) AND UBX frames (0xB5 0x62...) from a mixed stream.
Important: we must NOT discard UBX while searching for RTCM.
"""
def __init__(self):
self.buf = bytearray()
self.rtcm_invalid_crc = 0
self.ubx_invalid_crc = 0
def feed(self, data: bytes):
if data:
self.buf.extend(data)
@staticmethod
def _try_parse_rtcm(buf: bytearray, i: int) -> Optional[Tuple[int, bytes]]:
if len(buf) - i < 6:
return None
if buf[i] != 0xD3:
return None
length = ((buf[i+1] & 0x03) << 8) | buf[i+2]
if length > 1023:
return None
need = 3 + length + 3
if len(buf) - i < need:
return None
frame = bytes(buf[i:i+need])
body = frame[:3+length]
crc_bytes = frame[3+length:3+length+3]
want = (crc_bytes[0] << 16) | (crc_bytes[1] << 8) | crc_bytes[2]
got = crc24q(body)
if got != want:
return (-need, b"") # signal: parsed length, but bad crc
return (need, frame)
@staticmethod
def _try_parse_ubx(buf: bytearray, i: int) -> Optional[Tuple[int, int, int, bytes]]:
if len(buf) - i < 8:
return None
if buf[i] != 0xB5 or buf[i+1] != 0x62:
return None
msg_class = buf[i+2]
msg_id = buf[i+3]
ln = buf[i+4] | (buf[i+5] << 8)
need = 6 + ln + 2
if len(buf) - i < need:
return None
payload = bytes(buf[i+6:i+6+ln])
ck_rx = bytes(buf[i+6+ln:i+6+ln+2])
ck_calc = ubx_checksum(bytes([msg_class, msg_id, ln & 0xFF, (ln >> 8) & 0xFF]) + payload)
if ck_calc != ck_rx:
return (-need, msg_class, msg_id, b"") # bad crc
return (need, msg_class, msg_id, payload)
def pop_frames(self) -> Tuple[List[bytes], List[Tuple[int, int, bytes]]]:
"""
Returns (rtcm_frames_valid, ubx_frames_valid)
ubx_frames_valid items are (cls, id, payload)
"""
rtcm_out: List[bytes] = []
ubx_out: List[Tuple[int, int, bytes]] = []
i = 0
while i < len(self.buf):
b = self.buf[i]
if b == 0xD3:
r = self._try_parse_rtcm(self.buf, i)
if r is None:
break
n, frame = r
if n < 0:
self.rtcm_invalid_crc += 1
i += (-n)
else:
rtcm_out.append(frame)
i += n
continue
if b == 0xB5:
r2 = self._try_parse_ubx(self.buf, i)
if r2 is None:
break
n, cls, mid, payload = r2
if n < 0:
self.ubx_invalid_crc += 1
i += (-n)
else:
ubx_out.append((cls, mid, payload))
i += n
continue
i += 1
if i > 0:
del self.buf[:i]
if len(self.buf) > 1024 * 1024:
self.buf = self.buf[-1024*1024:]
return rtcm_out, ubx_out
# ============================================================
# NAV-SVIN parsing (ZED-F9P)
# ============================================================
import struct
def parse_nav_svin(payload: bytes):
# NAV-SVIN (0x01 0x3B) has a 4-byte header:
# uint8 version
# uint8[3] reserved0
# then the actual fields start at offset 4. [oai_citation:1‡docs.ros.org](https://docs.ros.org/en/kinetic/api/ublox_msgs/html/msg/NavSVIN.html)
if len(payload) < 40:
return None
version = payload[0]
# payload[1:4] reserved0
off = 4
iTOW, dur, meanX, meanY, meanZ = struct.unpack_from("<IIiii", payload, off)
off += 20
meanXHP, meanYHP, meanZHP, _reserved1 = struct.unpack_from("<bbbB", payload, off)
off += 4
meanAcc, obs = struct.unpack_from("<II", payload, off)
off += 8
valid, active = struct.unpack_from("<BB", payload, off)
# payload[off+2:off+4] reserved3
# meanX/Y/Z in cm, HP in 0.1 mm -> meters
meanX_m = (meanX * 0.01) + (meanXHP * 0.0001)
meanY_m = (meanY * 0.01) + (meanYHP * 0.0001)
meanZ_m = (meanZ * 0.01) + (meanZHP * 0.0001)
meanAcc_m = meanAcc * 0.0001 # 0.1 mm -> m [oai_citation:2‡docs.ros.org](https://docs.ros.org/en/kinetic/api/ublox_msgs/html/msg/NavSVIN.html)
return {
"version": int(version),
"iTOW_ms": int(iTOW),
"dur_s": int(dur),
"meanX_m": float(meanX_m),
"meanY_m": float(meanY_m),
"meanZ_m": float(meanZ_m),
"meanAcc_m": float(meanAcc_m),
"obs": int(obs),
"valid": bool(valid),
"active": bool(active),
}
# ============================================================
# Args / signals
# ============================================================
def parse_args():
ap = argparse.ArgumentParser(
description="ZED-F9P RTK Base: RTCM forward (USB -> LoRa) + state.json + RTCM tap"
)
ap.add_argument("--zed-port", default=ZED_PORT_DEFAULT)
ap.add_argument("--zed-baud", type=int, default=ZED_BAUD_DEFAULT)
ap.add_argument("--lora-port", default=LORA_PORT_DEFAULT)
ap.add_argument("--lora-baud", type=int, default=LORA_BAUD_DEFAULT)
ap.add_argument("--state-path", default=STATE_PATH_DEFAULT)
ap.add_argument("--svin-target-dur", type=int, default=SVIN_TARGET_DUR_DEFAULT)
ap.add_argument("--svin-target-acc", type=float, default=SVIN_TARGET_ACC_DEFAULT)
# RTCM tap
ap.add_argument(
"--rtcm-tap-file",
default="",
help="Write CRC-valid RTCM frames to this binary file (append). Example: /run/rtk/rtcm.tap",
)
ap.add_argument("--rtcm-tap-rotate-mb", type=int, default=0)
ap.add_argument("--rtcm-tap-max-files", type=int, default=5)
# SVIN handling
ap.add_argument(
"--enable-svin-status",
action="store_true",
help="Enable NAV-SVIN output on USB and parse it into state.json.",
)
return ap.parse_args()
_STOP = False
def _handle_stop(signum, frame):
global _STOP
_STOP = True
# ============================================================
# Main
# ============================================================
def main():
global _STOP
args = parse_args()
signal.signal(signal.SIGINT, _handle_stop)
signal.signal(signal.SIGTERM, _handle_stop)
start_time = time.time()
zed = open_serial(args.zed_port, args.zed_baud)
lora: Optional[serial.Serial] = None
lora_ok = False
try:
lora = open_serial(args.lora_port, args.lora_baud)
lora_ok = True
except Exception as e:
print(f"⚠️ LoRa nicht verfügbar ({e}). Forwarding deaktiviert.")
# Enable NAV-SVIN output on USB (best-effort)
if args.enable_svin_status:
try:
print("🛰️ Aktiviere UBX-NAV-SVIN auf USB (CFG-MSG)…")
ubx_cfg_msg(zed, 0x01, 0x3B, 1) # NAV-SVIN on USB
except Exception as e:
print(f"⚠️ NAV-SVIN enable failed: {e}")
tap_path = args.rtcm_tap_file.strip() or None
tap = RtcmTap(tap_path, rotate_mb=args.rtcm_tap_rotate_mb, max_files=args.rtcm_tap_max_files)
if tap_path:
print(f"🧷 RTCM tap aktiv: {tap_path} (rotate={args.rtcm_tap_rotate_mb}MB, keep={args.rtcm_tap_max_files})")
# Runtime stats (VALID RTCM only)
rtcm_frames = 0
rtcm_bytes = 0
lora_tx_bytes = 0
last_rtcm_ts: Optional[float] = None
last_state_write = 0.0
# Diagnostics
msg_counts: Dict[str, int] = {}
# SVIN state
svin_state = "UNKNOWN"
svin_active = False
svin_valid = False
svin_dur_s = 0
svin_mean_acc_m = 0.0
demux = StreamDemux()
print("📡 Starte RTCM Forwarding (ZED -> LoRa) …")
print("ℹ️ Hinweis: Es werden NUR CRC-validierte RTCM3 Frames gezählt/weitergeleitet (und ggf. getappt).")
while not _STOP:
n = zed.in_waiting
if n:
chunk = zed.read(n)
demux.feed(chunk)
rtcm_list, ubx_list = demux.pop_frames()
# Handle UBX frames (NAV-SVIN)
if args.enable_svin_status and ubx_list:
for cls, mid, payload in ubx_list:
if (cls, mid) == (0x01, 0x3B): # NAV-SVIN
s = parse_nav_svin(payload)
if s:
svin_active = bool(int(s["active"]) != 0)
svin_valid = bool(int(s["valid"]) != 0)
svin_dur_s = int(s["dur_s"])
svin_mean_acc_m = float(s["meanAcc_m"])
if svin_active and not svin_valid:
svin_state = "SURVEY_IN"
elif svin_valid:
svin_state = "VALID"
else:
svin_state = "IDLE"
# Handle RTCM frames
for frame in rtcm_list:
rtcm_frames += 1
rtcm_bytes += len(frame)
last_rtcm_ts = time.time()
if tap_path and tap.ok:
tap.write(frame)
length = ((frame[1] & 0x03) << 8) | frame[2]
payload = frame[3: 3 + length]
mt = rtcm_msg_type(payload)
if mt is not None:
k = str(mt)
msg_counts[k] = msg_counts.get(k, 0) + 1
if lora_ok and lora is not None:
try:
lora.write(frame)
lora_tx_bytes += len(frame)
except Exception as e:
print(f"⚠️ LoRa write failed -> deaktiviert ({e})")
lora_ok = False
else:
time.sleep(READ_IDLE_SLEEP_S)
now = time.time()
if now - last_state_write >= STATE_WRITE_INTERVAL_S:
last_state_write = now
rtcm_last_s = 9999.0 if last_rtcm_ts is None else round(now - last_rtcm_ts, 2)
state = {
"role": "base",
"host": HOSTNAME,
"zed_port": args.zed_port,
"lora_port": args.lora_port,
"uptime_s": int(now - start_time),
"zed_ok": True,
"lora_ok": bool(lora_ok),
"svin_state": svin_state,
"svin_active": bool(svin_active),
"svin_valid": bool(svin_valid),
"svin_dur_s": int(svin_dur_s),
"svin_meanAcc_m": float(svin_mean_acc_m),
"svin_target_dur_s": int(args.svin_target_dur),
"svin_target_acc_m": float(args.svin_target_acc),
"rtcm_out_frames": int(rtcm_frames),
"rtcm_out_bytes": int(rtcm_bytes),
"rtcm_last_s": float(rtcm_last_s),
"lora_tx_bytes": int(lora_tx_bytes),
"rtcm_invalid_crc": int(demux.rtcm_invalid_crc),
"ubx_invalid_crc": int(demux.ubx_invalid_crc),
"rtcm_msg_counts": msg_counts,
"rtcm_tap_file": tap_path or "",
"rtcm_tap_ok": bool(tap.ok) if tap_path else False,
"rtcm_tap_bytes": int(tap.bytes_written) if tap_path else 0,
"ts": now_ts(),
}
try:
write_state(args.state_path, state)
except Exception as e:
print(f"⚠️ write_state failed: {e}")
print("🛑 Stop signal received, exiting…")
try:
tap.close()
except Exception:
pass
try:
zed.close()
except Exception:
pass
try:
if lora is not None:
lora.close()
except Exception:
pass
if __name__ == "__main__":
main()