Initial working RTK base setup
This commit is contained in:
Executable
+603
@@ -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()
|
||||
@@ -0,0 +1,262 @@
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import socket
|
||||
import time
|
||||
from typing import Any, Dict, Optional
|
||||
|
||||
import paho.mqtt.client as mqtt
|
||||
|
||||
|
||||
def parse_args():
|
||||
ap = argparse.ArgumentParser(description="RTK Status -> MQTT (reads /run/rtk/state.json)")
|
||||
ap.add_argument("--state-file", default="/run/rtk/state.json")
|
||||
ap.add_argument("--mqtt-host", required=True)
|
||||
ap.add_argument("--mqtt-port", type=int, default=1883)
|
||||
ap.add_argument("--mqtt-user", default="")
|
||||
ap.add_argument("--mqtt-pass", default="")
|
||||
ap.add_argument("--topic-prefix", default="rtk/base")
|
||||
ap.add_argument("--interval", type=float, default=1.0)
|
||||
ap.add_argument("--retain", action="store_true", help="Retain MQTT publishes (default: false)")
|
||||
ap.add_argument("--client-id", default="rtk-status-base")
|
||||
ap.add_argument("--availability-topic", default="", help="Override availability topic (default: <prefix>/availability)")
|
||||
ap.add_argument("--debug", action="store_true")
|
||||
|
||||
# Home Assistant MQTT Discovery
|
||||
ap.add_argument("--ha-discovery", action="store_true", help="Publish Home Assistant MQTT Discovery config")
|
||||
ap.add_argument("--ha-prefix", default="homeassistant", help="Discovery prefix (default: homeassistant)")
|
||||
ap.add_argument("--device-name", default="RTK Base", help="Device name shown in Home Assistant")
|
||||
ap.add_argument("--device-id", default="rtk_base", help="Stable device id for Home Assistant (no spaces)")
|
||||
return ap.parse_args()
|
||||
|
||||
|
||||
def now_iso() -> str:
|
||||
return time.strftime("%Y-%m-%d %H:%M:%S", time.localtime())
|
||||
|
||||
|
||||
def safe_read_json(path: str) -> Optional[Dict[str, Any]]:
|
||||
try:
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
return json.load(f)
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
def flatten(prefix: str, obj: Any, out: Dict[str, Any]):
|
||||
"""Flatten nested dicts into MQTT-friendly key paths."""
|
||||
if isinstance(obj, dict):
|
||||
for k, v in obj.items():
|
||||
key = f"{prefix}/{k}" if prefix else str(k)
|
||||
flatten(key, v, out)
|
||||
elif isinstance(obj, list):
|
||||
out[prefix] = json.dumps(obj, ensure_ascii=False)
|
||||
else:
|
||||
out[prefix] = obj
|
||||
|
||||
|
||||
def _mqtt_client(client_id: str) -> mqtt.Client:
|
||||
# Robust gegen paho 1.x / 2.x Unterschiede
|
||||
return mqtt.Client(client_id=client_id, protocol=mqtt.MQTTv311)
|
||||
|
||||
|
||||
def publish_discovery(client: mqtt.Client, args, availability_topic: str):
|
||||
"""
|
||||
Publish Home Assistant MQTT Discovery configs.
|
||||
"""
|
||||
|
||||
dev = {
|
||||
"identifiers": [args.device_id],
|
||||
"name": args.device_name,
|
||||
"manufacturer": "u-blox / custom",
|
||||
"model": "ZED-F9P RTK",
|
||||
"sw_version": "rtk-status_mqtt",
|
||||
}
|
||||
|
||||
base = args.topic_prefix.rstrip("/")
|
||||
ha = args.ha_prefix.rstrip("/")
|
||||
|
||||
def pub_config(component: str, object_id: str, payload: Dict[str, Any]):
|
||||
topic = f"{ha}/{component}/{args.device_id}/{object_id}/config"
|
||||
client.publish(topic, json.dumps(payload, ensure_ascii=False), qos=0, retain=True)
|
||||
|
||||
def common(name: str, state_topic: str):
|
||||
return {
|
||||
"name": name,
|
||||
"state_topic": state_topic,
|
||||
"availability_topic": availability_topic,
|
||||
"payload_available": "online",
|
||||
"payload_not_available": "offline",
|
||||
"device": dev,
|
||||
}
|
||||
|
||||
def sensor_cfg(
|
||||
object_id: str,
|
||||
name: str,
|
||||
*,
|
||||
unit: Optional[str] = None,
|
||||
icon: Optional[str] = None,
|
||||
device_class: Optional[str] = None,
|
||||
state_class: Optional[str] = None,
|
||||
entity_category: Optional[str] = None,
|
||||
):
|
||||
payload = {
|
||||
**common(name, f"{base}/{object_id}"),
|
||||
"unique_id": f"{args.device_id}_{object_id}",
|
||||
}
|
||||
if unit:
|
||||
payload["unit_of_measurement"] = unit
|
||||
if icon:
|
||||
payload["icon"] = icon
|
||||
if device_class:
|
||||
payload["device_class"] = device_class
|
||||
if state_class:
|
||||
payload["state_class"] = state_class
|
||||
if entity_category:
|
||||
payload["entity_category"] = entity_category
|
||||
pub_config("sensor", object_id, payload)
|
||||
|
||||
def binary_sensor_cfg(
|
||||
object_id: str,
|
||||
name: str,
|
||||
*,
|
||||
icon: Optional[str] = None,
|
||||
device_class: Optional[str] = None,
|
||||
entity_category: Optional[str] = None,
|
||||
):
|
||||
payload = {
|
||||
**common(name, f"{base}/{object_id}"),
|
||||
"unique_id": f"{args.device_id}_{object_id}",
|
||||
"payload_on": "true",
|
||||
"payload_off": "false",
|
||||
}
|
||||
if icon:
|
||||
payload["icon"] = icon
|
||||
if device_class:
|
||||
payload["device_class"] = device_class
|
||||
if entity_category:
|
||||
payload["entity_category"] = entity_category
|
||||
pub_config("binary_sensor", object_id, payload)
|
||||
|
||||
# Existing / general sensors
|
||||
sensor_cfg("uptime_s", "RTK Uptime", unit="s", device_class="duration", icon="mdi:timer-outline")
|
||||
sensor_cfg("svin_meanAcc_m", "SVIN Mean Accuracy", unit="m", icon="mdi:crosshairs-gps", state_class="measurement")
|
||||
sensor_cfg("svin_state", "SVIN State", icon="mdi:satellite-variant")
|
||||
sensor_cfg("rtcm_last_s", "RTCM Age", unit="s", icon="mdi:timer-sand", state_class="measurement")
|
||||
sensor_cfg("rtcm_out_frames", "RTCM Frames Out", icon="mdi:counter", state_class="measurement")
|
||||
sensor_cfg("rtcm_out_bytes", "RTCM Bytes Out", unit="B", icon="mdi:database", state_class="measurement")
|
||||
sensor_cfg("lora_tx_bytes", "LoRa TX Bytes", unit="B", icon="mdi:transmission-tower", state_class="measurement")
|
||||
|
||||
binary_sensor_cfg("zed_ok", "ZED OK", icon="mdi:satellite-uplink")
|
||||
binary_sensor_cfg("lora_ok", "LoRa OK", icon="mdi:radio-handheld")
|
||||
binary_sensor_cfg("state_ok", "State File OK", icon="mdi:file-check-outline")
|
||||
|
||||
# New base position / quality / TMODE / SVIN sensors
|
||||
sensor_cfg("base_lat", "Base Latitude", icon="mdi:latitude")
|
||||
sensor_cfg("base_lon", "Base Longitude", icon="mdi:longitude")
|
||||
sensor_cfg("base_h_msl_m", "Base Height MSL", unit="m", icon="mdi:image-filter-hdr", state_class="measurement")
|
||||
sensor_cfg("base_h_ell_m", "Base Height Ellipsoid", unit="m", icon="mdi:image-filter-center-focus", state_class="measurement")
|
||||
sensor_cfg("base_geoid_sep_m", "Base Geoid Separation", unit="m", icon="mdi:terrain", state_class="measurement")
|
||||
sensor_cfg("base_fix_type", "Base Fix Type Code", icon="mdi:satellite-variant", state_class="measurement")
|
||||
sensor_cfg("base_fix_name", "Base Fix Type", icon="mdi:satellite-variant")
|
||||
sensor_cfg("base_sats", "Base Satellites", icon="mdi:satellite-uplink", state_class="measurement")
|
||||
sensor_cfg("base_hacc_m", "Base Horizontal Accuracy", unit="m", icon="mdi:crosshairs-gps", state_class="measurement")
|
||||
sensor_cfg("base_vacc_m", "Base Vertical Accuracy", unit="m", icon="mdi:arrow-expand-vertical", state_class="measurement")
|
||||
|
||||
sensor_cfg("tmode3_mode", "Base TMODE3 Code", icon="mdi:cog-transfer", state_class="measurement")
|
||||
sensor_cfg("tmode3_name", "Base TMODE3", icon="mdi:cog-transfer")
|
||||
|
||||
binary_sensor_cfg("svin_active", "Survey-In Active", icon="mdi:timer-sand")
|
||||
binary_sensor_cfg("svin_valid", "Survey-In Valid", icon="mdi:check-decagram")
|
||||
sensor_cfg("svin_dur_s", "Survey-In Duration", unit="s", icon="mdi:timer-outline", state_class="measurement")
|
||||
sensor_cfg("svin_meanAcc_m", "Survey-In Mean Accuracy", unit="m", icon="mdi:ruler", state_class="measurement")
|
||||
|
||||
sensor_cfg("svin_target_dur_s", "Survey-In Target Duration", unit="s", icon="mdi:timer-cog-outline", state_class="measurement")
|
||||
sensor_cfg("svin_target_acc_m", "Survey-In Target Accuracy", unit="m", icon="mdi:target", state_class="measurement")
|
||||
|
||||
|
||||
def main():
|
||||
args = parse_args()
|
||||
base = args.topic_prefix.rstrip("/")
|
||||
availability_topic = args.availability_topic.strip() or f"{base}/availability"
|
||||
|
||||
host = socket.gethostname()
|
||||
|
||||
client = _mqtt_client(args.client_id)
|
||||
|
||||
connected = {"ok": False}
|
||||
|
||||
def on_connect(client, userdata, flags, rc, *props):
|
||||
connected["ok"] = (rc == 0)
|
||||
if args.debug:
|
||||
print(f"[{now_iso()}] MQTT on_connect rc={rc}")
|
||||
|
||||
client.publish(availability_topic, "online", retain=True)
|
||||
|
||||
if args.ha_discovery:
|
||||
publish_discovery(client, args, availability_topic)
|
||||
|
||||
def on_disconnect(client, userdata, rc, *props):
|
||||
connected["ok"] = False
|
||||
if args.debug:
|
||||
print(f"[{now_iso()}] MQTT on_disconnect rc={rc}")
|
||||
|
||||
client.on_connect = on_connect
|
||||
client.on_disconnect = on_disconnect
|
||||
|
||||
if args.mqtt_user:
|
||||
client.username_pw_set(args.mqtt_user, args.mqtt_pass)
|
||||
|
||||
client.will_set(availability_topic, "offline", retain=True)
|
||||
|
||||
client.connect(args.mqtt_host, args.mqtt_port, 30)
|
||||
client.loop_start()
|
||||
|
||||
last_payload_hash = None
|
||||
|
||||
while True:
|
||||
st = safe_read_json(args.state_file)
|
||||
|
||||
flat: Dict[str, Any] = {}
|
||||
if st is not None:
|
||||
flatten("", st, flat)
|
||||
flat["host"] = host
|
||||
flat["state_ok"] = True
|
||||
else:
|
||||
flat = {
|
||||
"host": host,
|
||||
"state_ok": False,
|
||||
"publish_ts": now_iso(),
|
||||
}
|
||||
|
||||
flat["publish_ts"] = now_iso()
|
||||
|
||||
json_payload = json.dumps(
|
||||
flat if st is None else {**st, "host": host, "state_ok": True, "publish_ts": flat["publish_ts"]},
|
||||
ensure_ascii=False
|
||||
)
|
||||
|
||||
h = hash(json_payload)
|
||||
if h != last_payload_hash:
|
||||
client.publish(f"{base}/json", json_payload, retain=args.retain)
|
||||
last_payload_hash = h
|
||||
|
||||
for k, v in flat.items():
|
||||
topic = f"{base}/{k}"
|
||||
if isinstance(v, bool):
|
||||
payload = "true" if v else "false"
|
||||
elif v is None:
|
||||
payload = ""
|
||||
else:
|
||||
payload = str(v)
|
||||
client.publish(topic, payload, retain=args.retain)
|
||||
|
||||
client.publish(availability_topic, "online", retain=True)
|
||||
|
||||
time.sleep(max(0.2, args.interval))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user