#!/usr/bin/env python3
import serial, socket, threading, time, math, sys, os
from datetime import datetime
# === Konfiguration ===
SERIAL_PORT = "/dev/ttyUSB0"
BAUDRATE = 115200
TCP_PORT = 2102 # Port, über den Rover sich verbinden
# Feste Position deiner Basisstation
BASE_LAT = 48.13743
BASE_LON = 11.57549
BASE_ALT = 64.12
# === Globale Variablen ===
clients = []
stop_flag = False
# === Hilfsfunktionen ===
def log(msg):
now = datetime.now().strftime("%H:%M:%S.%f")[:-3]
print(f"[{now}] {msg}")
def calc_x_y_z(lat, lon, alt):
"""WGS84 → ECEF (XYZ)"""
a = 6378137.0
e2 = 6.69437999014e-3
lat_r = math.radians(lat)
lon_r = math.radians(lon)
N = a / math.sqrt(1 - e2 * math.sin(lat_r)**2)
x = (N + alt) * math.cos(lat_r) * math.cos(lon_r)
y = (N + alt) * math.cos(lat_r) * math.sin(lon_r)
z = (N * (1 - e2) + alt) * math.sin(lat_r)
return x, y, z
def nmea_checksum(sentence):
"""Berechnet NMEA-Checksumme"""
csum = 0
for c in sentence:
csum ^= ord(c)
return f"*{csum:02X}"
def send_cmd(ser, cmd):
"""Sendet ASCII-Befehl mit automatischer Prüfsumme"""
if "*" not in cmd:
base = cmd.split("$")[1]
cmd = f"${base}{nmea_checksum(base)}"
ser.write((cmd + "\r\n").encode())
log(f"[TX] {cmd}")
time.sleep(0.2)
# === Initialisierung LC29H ===
def init_lc29_base(ser):
"""Setzt LC29H-DA in Basis-Mode"""
log("=== LC29HDA Basis Init Start ===")
# Berechne ECEF-Koordinaten
x, y, z = calc_x_y_z(BASE_LAT, BASE_LON, BASE_ALT)
log(f"[BASE POS] LAT={BASE_LAT:.7f}, LON={BASE_LON:.7f}, ALT={BASE_ALT:.2f}")
log(f"[BASE POS] X={x:.4f}, Y={y:.4f}, Z={z:.4f}")
# Standort (ECEF) schreiben
send_cmd(ser, f"$PQTMCFGSVIN,W,2,0,0.0,{x:.4f},{y:.4f},{z:.4f}")
# Antenne aktivieren, Ausgabe einschalten
send_cmd(ser, "$PQTMCFGRTCM,W,1,1,1,1,1,1,1,1,1,1") # alle RTCM aktivieren
#send_cmd(ser, "$PQTMCFGMSG,0,1,0,0,0,0,0") # nur RTCM-Ausgabe
send_cmd(ser, "$PQTMVERNO")
log("[DONE] LC29HDA ready and transmitting RTCM ✓")
# === RTCM Listener ===
def rx_thread(ser):
"""Liest RTCM von LC29H und verteilt sie an Clients"""
global stop_flag
buffer = b""
while not stop_flag:
try:
chunk = ser.read(1024)
if not chunk:
continue
buffer += chunk
# RTCM frames starten immer mit 0xD3
while len(buffer) > 2:
if buffer[0] != 0xD3:
buffer = buffer[1:]
continue
if len(buffer) < 3:
break
length = ((buffer[1] & 0x03) << 8) | buffer[2]
if len(buffer) < 3 + length + 3:
break
frame = buffer[:3 + length + 3]
buffer = buffer[3 + length + 3:]
if len(frame) < 6:
continue
msg_type = ((frame[3] & 0xFC) << 4) | (frame[4] >> 4)
log(f"[RTCM3] Type {msg_type:<4} | Size {len(frame)} bytes")
# An alle verbundenen Rover senden
for c in clients.copy():
try:
c.sendall(frame)
except Exception:
clients.remove(c)
except Exception as e:
log(f"[RX ERROR] {e}")
time.sleep(1)
# === TCP-Server für Rover ===
def tcp_server():
"""TCP Server, der RTCM an Rover streamt"""
global clients
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind(("0.0.0.0", TCP_PORT))
s.listen(1)
log(f"[SERVER] RTCM TCP Server läuft auf Port {TCP_PORT}")
while True:
conn, addr = s.accept()
log(f"[SERVER] Neuer Rover verbunden: {addr}")
clients.append(conn)
# === Main ===
def main():
ser = serial.Serial(SERIAL_PORT, BAUDRATE, timeout=1)
init_lc29_base(ser)
threading.Thread(target=tcp_server, daemon=True).start()
rx_thread(ser)
if __name__ == "__main__":
try:
main()
except KeyboardInterrupt:
log("Beendet durch Benutzer.")
stop_flag = True
sys.exit(0)