From f7460acbed8f2f385da3e40a501a4b451f943d2c Mon Sep 17 00:00:00 2001 From: martin Date: Thu, 30 Apr 2026 09:47:09 +0200 Subject: [PATCH] Initial working RTK base setup --- scripts/rtkbase_lora.py | 603 +++++++++++++++++++++++++++++++++++++ scripts/status_mqtt.py | 262 ++++++++++++++++ systemd/rtk-base.service | 38 +++ systemd/rtk-status.service | 36 +++ tools/config.py | 38 +++ tools/rtk_base_mqtt.py | 338 +++++++++++++++++++++ tools/rtkbase_forwarder.py | 39 +++ tools/rtkbase_lora.py | 603 +++++++++++++++++++++++++++++++++++++ tools/status_mqtt.py | 262 ++++++++++++++++ 9 files changed, 2219 insertions(+) create mode 100755 scripts/rtkbase_lora.py create mode 100644 scripts/status_mqtt.py create mode 100644 systemd/rtk-base.service create mode 100644 systemd/rtk-status.service create mode 100755 tools/config.py create mode 100755 tools/rtk_base_mqtt.py create mode 100755 tools/rtkbase_forwarder.py create mode 100755 tools/rtkbase_lora.py create mode 100644 tools/status_mqtt.py diff --git a/scripts/rtkbase_lora.py b/scripts/rtkbase_lora.py new file mode 100755 index 0000000..1a482b6 --- /dev/null +++ b/scripts/rtkbase_lora.py @@ -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(" 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() diff --git a/scripts/status_mqtt.py b/scripts/status_mqtt.py new file mode 100644 index 0000000..3f567c2 --- /dev/null +++ b/scripts/status_mqtt.py @@ -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: /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() diff --git a/systemd/rtk-base.service b/systemd/rtk-base.service new file mode 100644 index 0000000..7691a7e --- /dev/null +++ b/systemd/rtk-base.service @@ -0,0 +1,38 @@ +[Unit] +Description=RTK Base (ZED-F9P + LoRa) +#After=network-online.target +#Wants=network-online.target + +# Optional, aber empfehlenswert: auf Serial-Devices warten (per systemd-escape ermitteln) +After=dev-serial-by\x2did-usb\x2du\x2dblox_AG_\x2d__www.u\x2dblox.com_u\x2dblox_GNSS_receiver\x2dif00.device +Wants=dev-serial-by\x2did-usb\x2du\x2dblox_AG_\x2d__www.u\x2dblox.com_u\x2dblox_GNSS_receiver\x2dif00.device +After=dev-serial-by\x2did-usb\x2d1a86_USB_Serial\x2dif00\x2dport0.device +Wants=dev-serial-by\x2did-usb\x2d1a86_USB_Serial\x2dif00\x2dport0.device + +StartLimitInterval=0 +StartLimitBurst=0 + +[Service] +Type=simple +User=naue +Group=naue +WorkingDirectory=/home/naue/rtk + +RuntimeDirectory=rtk +RuntimeDirectoryMode=0775 + +Environment=PYTHONUNBUFFERED=1 + +ExecStart=/usr/bin/python3 /home/naue/rtk/rtkbase_lora.py \ +# --enable-svin-status \ + --rtcm-tap-file /run/rtk/rtcm.tap \ + --rtcm-tap-rotate-mb 50 \ + --rtcm-tap-max-files 5 + +Restart=always +RestartSec=3 +TimeoutStopSec=10 +KillSignal=SIGTERM + +[Install] +WantedBy=multi-user.target diff --git a/systemd/rtk-status.service b/systemd/rtk-status.service new file mode 100644 index 0000000..cc6f4ae --- /dev/null +++ b/systemd/rtk-status.service @@ -0,0 +1,36 @@ +[Unit] +Description=RTK Status -> MQTT (reads /run/rtk/state.json) +After=network-online.target +Wants=network-online.target + +[Service] +User=naue +Group=naue +RuntimeDirectory=rtk +RuntimeDirectoryMode=0755 + +Type=simple +User=naue +WorkingDirectory=/home/naue/rtk + +# sorgt dafür, dass /run/rtk existiert (tmpfs, nach Boot leer) +RuntimeDirectory=rtk +RuntimeDirectoryMode=0755 + +ExecStart=/usr/bin/python3 /home/naue/rtk/status_mqtt.py \ + --state-file /run/rtk/state.json \ + --mqtt-host 192.168.2.36 \ + --mqtt-user wall-e \ + --mqtt-pass anTSoYhUEPRN5E \ + --topic-prefix rtk/base \ + --interval 5 \ + --retain \ + --ha-discovery \ + --device-name "RTK Base" \ + --device-id rtk_base + +Restart=always +RestartSec=2 + +[Install] +WantedBy=multi-user.target diff --git a/tools/config.py b/tools/config.py new file mode 100755 index 0000000..b51a2fd --- /dev/null +++ b/tools/config.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 +import serial, time + +PORT="/dev/serial/by-id/usb-1a86_USB_Serial-if00-port0" +BAUD=9600 + +def send(ser, cmd, wait=0.4): + ser.write((cmd+"\r\n").encode()) + ser.flush() + time.sleep(wait) + rx = ser.read(2000).decode(errors="ignore") + print(f"[{cmd}] {rx.strip()}") + return rx + +ser=serial.Serial(PORT, BAUD, timeout=0.5) +ser.reset_input_buffer() +ser.reset_output_buffer() + +# AT-Mode sauber betreten +time.sleep(1.5) +ser.write(b"+++\r\n"); ser.flush() +time.sleep(0.8) +print(ser.read(200).decode(errors="ignore").strip()) + +# Konfig setzen +send(ser, "AT+MODE0") +send(ser, "AT+SLEEP2") +send(ser, "AT+LEVEL7") # mehr Durchsatz-Reserve [oai_citation:4‡DX-LR03-433T30D Serial port application guide .pdf](sediment://file_000000005c3c7243bef470aff82f39f0) +send(ser, "AT+CR2") # 4/6 default [oai_citation:5‡DX-LR03-433T30D Serial port application guide .pdf](sediment://file_000000005c3c7243bef470aff82f39f0) +send(ser, "AT+CRC1") # LoRa CRC on [oai_citation:6‡DX-LR03-433T30D Serial port application guide .pdf](sediment://file_000000005c3c7243bef470aff82f39f0) +send(ser, "AT+POWE5") # max power [oai_citation:7‡DX-LR03-433T30D Serial port application guide .pdf](sediment://file_000000005c3c7243bef470aff82f39f0) + +# Anzeigen + Reset (damit es wirklich aktiv wird) +send(ser, "AT+HELP", wait=0.8) +send(ser, "AT+RESET", wait=0.6) + +ser.close() +print("done") diff --git a/tools/rtk_base_mqtt.py b/tools/rtk_base_mqtt.py new file mode 100755 index 0000000..cc19e3f --- /dev/null +++ b/tools/rtk_base_mqtt.py @@ -0,0 +1,338 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import argparse +import json +import os +import struct +import sys +import time +from dataclasses import dataclass, asdict +from typing import Optional, Tuple, List + +import serial + +# Optional: paho-mqtt (sudo apt install python3-paho-mqtt) +try: + import paho.mqtt.client as mqtt +except Exception: + mqtt = None + + +# ========================= +# 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 send_ubx(ser: serial.Serial, cls: int, mid: int, payload: bytes = b"") -> None: + head = b"\xB5\x62" + bytes([cls, mid]) + struct.pack(" Optional[Tuple[int, int, bytes]]: + end = time.time() + timeout + # sync + while time.time() < end: + b = ser.read(1) + if not b: + continue + if b == b"\xB5": + b2 = ser.read(1) + if b2 == 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(" bool: + end = time.time() + timeout + while time.time() < end: + f = read_ubx_frame(ser, timeout=end - time.time()) + if not f: + continue + c, m, p = f + # ACK-ACK=0x05 0x01, ACK-NAK=0x05 0x00; payload: cls,id + if c == 0x05 and m in (0x01, 0x00) and len(p) == 2 and (p[0], p[1]) == (exp_cls, exp_id): + return (m == 0x01) + return False + +def mon_ver(ser: serial.Serial) -> Tuple[str, str]: + send_ubx(ser, 0x0A, 0x04) # MON-VER + v1 = v2 = "?" + end = time.time() + 0.7 + while time.time() < end: + f = read_ubx_frame(ser, timeout=0.2) + if not f: + continue + c, m, p = f + if (c, m) != (0x0A, 0x04): + continue + 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 + + +# ========================= +# ZED-F9P config (minimal) +# ========================= + +def cfg_msg(ser: serial.Serial, msg_cls: int, msg_id: int, usb_rate: int) -> bool: + # CFG-MSG 0x06 0x01: class,id, rates[UART1,UART2,USB,SPIF0,I2C,UART3] + payload = bytes([msg_cls, msg_id, 0, 0, usb_rate, 0, 0]) + send_ubx(ser, 0x06, 0x01, payload) + return wait_ack(ser, 0x06, 0x01, 1.0) + +def quiet_nmea_on_usb(ser: serial.Serial) -> None: + # NMEA class 0xF0 ids + for mid in [0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09]: + cfg_msg(ser, 0xF0, mid, 0) + +def enable_nav_pvt_push_usb(ser: serial.Serial, rate: int = 1) -> bool: + return cfg_msg(ser, 0x01, 0x07, rate) # NAV-PVT + +def enable_nav_svin_push_usb(ser: serial.Serial, rate: int = 1) -> bool: + return cfg_msg(ser, 0x01, 0x3B, rate) # NAV-SVIN + +def poll_nav_svin(ser: serial.Serial, timeout: float = 1.0) -> Optional[Tuple[bool, bool, int, float]]: + # poll NAV-SVIN + send_ubx(ser, 0x01, 0x3B) + frm = read_ubx_frame(ser, timeout=timeout) + if not frm or (frm[0], frm[1]) != (0x01, 0x3B) or len(frm[2]) < 40: + return None + p = frm[2] + + # Layout-Variante A (bewährt bei dir): dur@8, meanAcc@28, valid@36, active@37 + dur = struct.unpack_from(" Optional[dict]: + # poll NAV-PVT + send_ubx(ser, 0x01, 0x07) + frm = read_ubx_frame(ser, timeout=timeout) + if not frm or (frm[0], frm[1]) != (0x01, 0x07) or len(frm[2]) < 92: + return None + p = frm[2] + fixType = p[20] + flags = p[21] + carrSoln = (p[21] >> 6) & 0x03 # 0 none, 1 float, 2 fix + numSV = p[23] + lon = struct.unpack_from(" str: + # fixType: 0 no, 1 dead reck, 2 2D, 3 3D, 4 GNSS+DR, 5 time only + base = {0:"NO FIX",1:"DR",2:"2D",3:"3D",4:"GNSS+DR",5:"TIME"}.get(fixType, f"FIX{fixType}") + if carrSoln == 1: + return f"{base} / RTK-FLOAT" + if carrSoln == 2: + return f"{base} / RTK-FIX" + return base + + +# ========================= +# RTCM sniff + (optional) LoRa TX +# ========================= + +def rtcm_try_parse_stream(data: bytes, max_frames: int = 50): + """ + Minimaler RTCM3 Frame Parser: + - Preamble 0xD3 + - 10-bit length in next 2 bytes + - frame = 3 header + length + 3 CRC + Liefert: frames(list[bytes]), rest(bytes), bad_count(int), types(list[int]) + """ + i = 0 + frames = [] + bad = 0 + types = [] + n = len(data) + + def get_len(b1, b2): + return ((b1 & 0x03) << 8) | b2 + + while i + 3 <= n and len(frames) < max_frames: + if data[i] != 0xD3: + i += 1 + continue + if i + 3 > n: + break + length = get_len(data[i+1], data[i+2]) + frame_len = 3 + length + 3 + if length > 2047 or frame_len < 6: + bad += 1 + i += 1 + continue + if i + frame_len > n: + break # rest kommt später + frame = data[i:i+frame_len] + # Type: first 12 bits after header => frame[3:5] + if length >= 2: + msg_type = ((frame[3] << 4) | (frame[4] >> 4)) & 0x0FFF + types.append(int(msg_type)) + frames.append(frame) + i += frame_len + + rest = data[i:] if i < n else b"" + return frames, rest, bad, types + +def lora_send_frames(lora: serial.Serial, frames: List[bytes], max_payload: int): + """ + Generisch: split frames in chunks <= max_payload and write raw. + DX-LR03: je nach Firmware sind "transparent mode" oder AT+SEND nötig. + -> Hier nur raw write. Falls du AT+SEND brauchst, sag Bescheid, dann baue ich das exakt für DX-LR03 ein. + """ + sent = 0 + for fr in frames: + off = 0 + while off < len(fr): + chunk = fr[off:off+max_payload] + lora.write(chunk) + sent += len(chunk) + off += len(chunk) + return sent + + +# ========================= +# MQTT +# ========================= + +class MqttPublisher: + def __init__(self, host: str, port: int, base_topic: str, client_id: str, + username: Optional[str], password: Optional[str], retain: bool = True): + if mqtt is None: + raise RuntimeError("paho-mqtt fehlt. Install: sudo apt install python3-paho-mqtt") + self.base_topic = base_topic.rstrip("/") + self.retain = retain + self.client = mqtt.Client(client_id=client_id, clean_session=True) + if username: + self.client.username_pw_set(username, password=password or None) + + # LWT: availability offline, wenn Prozess stirbt + self.client.will_set(f"{self.base_topic}/availability", "offline", qos=1, retain=True) + + self.client.connect(host, port, keepalive=30) + self.client.loop_start() + + # online markieren + self.publish("availability", "online", retain=True) + + def publish(self, sub: str, payload, retain: Optional[bool] = None, qos: int = 0): + if retain is None: + retain = self.retain + if isinstance(payload, (dict, list)): + payload = json.dumps(payload, ensure_ascii=False) + else: + payload = str(payload) + topic = f"{self.base_topic}/{sub.lstrip('/')}" + self.client.publish(topic, payload, qos=qos, retain=retain) + + def close(self): + try: + self.publish("availability", "offline", retain=True, qos=1) + except Exception: + pass + try: + self.client.loop_stop() + except Exception: + pass + try: + self.client.disconnect() + except Exception: + pass + + +# ========================= +# State +# ========================= + +@dataclass +class BaseState: + ts: float + uptime_s: float + gnss_connected: bool + lora_connected: bool + + # Survey-in + svin_active: Optional[bool] = None + svin_valid: Optional[bool] = None + svin_dur_s: Optional[int] = None + svin_mean_acc_m: Optional[float] = None + svin_target_s: Optional[int] = None + svin_target_acc_m: Optional[float] = None + + # Position + fix: Optional[str] = None + sats: Optional[int] = None + lat: Optional[float] = None + lon: Optional[float] = None + h_m: Optional[float] = None + hmsl_m: Optional[float] = None + hacc_m: Optional[float] = None + vacc_m: Optional[float] = None + + # RTCM + rtcm_ok: int = 0 + rtcm_bad: int = 0 + rtcm_bytes: int = 0 + rtcm_types: Optional[List[int]] = None + rtcm_last_ts: Optional[float] = None + + # LoRa + lora_tx_bytes: int = 0 + + +# ========================= +# Main loop +# ========================= + +def open_serial(port: str, baud: int, timeout: float = 0.1) -> serial.Serial: + return serial.Serial(port, baud, timeout=timeout) + +def main(): + ap = argparse.ArgumentParser(description="Robuste RTK Base: ZED-F9P + optional LoRa TX + MQTT Status") + ap.add_argument("--zed", "--port", dest="zed_port", default="/dev/ttyACM0") + ap.add_argument("--zed-baud", dest="zed_baud", type=int, default=115200) + + ap.add_argument("--lora", dest="lora_port", default="/dev/ttyUSB0") + ap.add_argument("--lora-baud", dest="lora_baud", type=int, default=57600) + ap.add_argument("--lora-max", dest="lora_max", type=int, default=50) diff --git a/tools/rtkbase_forwarder.py b/tools/rtkbase_forwarder.py new file mode 100755 index 0000000..41b8643 --- /dev/null +++ b/tools/rtkbase_forwarder.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +import time, serial + +ZED_PORT = "/dev/serial/by-id/usb-u-blox_AG_-_www.u-blox.com_u-blox_GNSS_receiver-if00" +ZED_BAUD = 115200 + +LORA_PORT = "/dev/serial/by-id/usb-1a86_USB_Serial-if00-port0" +LORA_BAUD = 9600 + +READ_CHUNK = 4096 + +def main(): + zed = serial.Serial(ZED_PORT, ZED_BAUD, timeout=0.2) + lora = serial.Serial(LORA_PORT, LORA_BAUD, timeout=0.2) + try: + last = time.time() + n_in = 0 + while True: + data = zed.read(READ_CHUNK) + if data: + lora.write(data) + n_in += len(data) + else: + time.sleep(0.005) + + # einfache Durchsatzanzeige alle 5s + now = time.time() + if now - last >= 5: + print(f"rtcm forwarded: {n_in/ (now-last):.0f} B/s") + last = now + n_in = 0 + finally: + try: zed.close() + except: pass + try: lora.close() + except: pass + +if __name__ == "__main__": + main() diff --git a/tools/rtkbase_lora.py b/tools/rtkbase_lora.py new file mode 100755 index 0000000..1a482b6 --- /dev/null +++ b/tools/rtkbase_lora.py @@ -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(" 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() diff --git a/tools/status_mqtt.py b/tools/status_mqtt.py new file mode 100644 index 0000000..3f567c2 --- /dev/null +++ b/tools/status_mqtt.py @@ -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: /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()