Initial working RTK base setup
This commit is contained in:
Executable
+38
@@ -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")
|
||||
Executable
+338
@@ -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("<H", len(payload))
|
||||
msg = bytes([cls, mid]) + struct.pack("<H", len(payload)) + payload
|
||||
ser.write(head + payload + ubx_checksum(msg))
|
||||
|
||||
def read_ubx_frame(ser: serial.Serial, timeout: float = 1.0) -> 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("<H", hdr[2:4])[0]
|
||||
payload = ser.read(length)
|
||||
cks = ser.read(2)
|
||||
if len(payload) != length or len(cks) != 2:
|
||||
return None
|
||||
if ubx_checksum(bytes([cls, mid]) + struct.pack("<H", length) + payload) != cks:
|
||||
return None
|
||||
return (cls, mid, payload)
|
||||
|
||||
def wait_ack(ser: serial.Serial, exp_cls: int, exp_id: int, timeout: float = 1.0) -> bool:
|
||||
end = time.time() + timeout
|
||||
while time.time() < end:
|
||||
f = read_ubx_frame(ser, 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("<I", p, 8)[0]
|
||||
meanAcc_01mm = struct.unpack_from("<I", p, 28)[0]
|
||||
valid = p[36] != 0
|
||||
active = p[37] != 0
|
||||
return active, valid, dur, meanAcc_01mm / 10000.0
|
||||
|
||||
def poll_nav_pvt(ser: serial.Serial, timeout: float = 1.0) -> 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("<i", p, 24)[0] / 1e7
|
||||
lat = struct.unpack_from("<i", p, 28)[0] / 1e7
|
||||
height = struct.unpack_from("<i", p, 32)[0] / 1000.0
|
||||
hMSL = struct.unpack_from("<i", p, 36)[0] / 1000.0
|
||||
hAcc = struct.unpack_from("<I", p, 40)[0] / 1000.0
|
||||
vAcc = struct.unpack_from("<I", p, 44)[0] / 1000.0
|
||||
return {
|
||||
"fixType": int(fixType),
|
||||
"flags": int(flags),
|
||||
"carrSoln": int(carrSoln),
|
||||
"numSV": int(numSV),
|
||||
"lat": float(lat),
|
||||
"lon": float(lon),
|
||||
"height_m": float(height),
|
||||
"hMSL_m": float(hMSL),
|
||||
"hAcc_m": float(hAcc),
|
||||
"vAcc_m": float(vAcc),
|
||||
}
|
||||
|
||||
def fix_to_text(fixType: int, carrSoln: int) -> 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)
|
||||
Executable
+39
@@ -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()
|
||||
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