272 lines
10 KiB
Python
272 lines
10 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
Standalone NTRIP test script to observe RTCM stream behavior.
|
|
Connects to NTRIP caster and displays parsed RTCM message details.
|
|
"""
|
|
import base64
|
|
import socket
|
|
import time
|
|
from datetime import datetime, timezone
|
|
import math
|
|
|
|
from rtcm_parser import RTCMParser
|
|
|
|
|
|
# NTRIP configuration
|
|
CASTER_HOST = "truertk.pointonenav.com"
|
|
CASTER_PORT = 2101
|
|
MOUNTPOINT = "AUTO"
|
|
USERNAME = "9t7fwfbm57"
|
|
PASSWORD = "96m7bec9g8"
|
|
|
|
# Position for GGA (hardcoded)
|
|
LAT = 36.1140884
|
|
LON = -97.0880663
|
|
ALT = 390.0
|
|
|
|
# Debug options
|
|
DEBUG_HEX = False # Set to True to see hex dump of first 5 messages
|
|
DEBUG_1005_1006 = True # Show detailed debug for position messages
|
|
|
|
|
|
def build_gga(lat: float, lon: float, alt: float) -> bytes:
|
|
"""Build NMEA GGA sentence."""
|
|
now_utc = datetime.now(timezone.utc).strftime("%H%M%S")
|
|
|
|
# Convert to NMEA format
|
|
lat_hemi = "N" if lat >= 0 else "S"
|
|
lat_abs = abs(lat)
|
|
lat_deg = int(lat_abs)
|
|
lat_min = (lat_abs - lat_deg) * 60.0
|
|
lat_str = f"{lat_deg:02d}{lat_min:07.4f}"
|
|
|
|
lon_hemi = "E" if lon >= 0 else "W"
|
|
lon_abs = abs(lon)
|
|
lon_deg = int(lon_abs)
|
|
lon_min = (lon_abs - lon_deg) * 60.0
|
|
lon_str = f"{lon_deg:03d}{lon_min:07.4f}"
|
|
|
|
fields = [
|
|
"GPGGA",
|
|
now_utc,
|
|
lat_str, lat_hemi,
|
|
lon_str, lon_hemi,
|
|
"1", # GPS fix
|
|
"12", # Number of satellites
|
|
"1.0", # HDOP
|
|
f"{alt:.1f}", "M",
|
|
"", "M",
|
|
"", "",
|
|
]
|
|
core = ",".join(fields)
|
|
checksum = 0
|
|
for char in core:
|
|
checksum ^= ord(char)
|
|
sentence = f"${core}*{checksum:02X}\r\n"
|
|
return sentence.encode("ascii")
|
|
|
|
|
|
def make_ntrip_request(host: str, port: int, mount: str, user: str, password: str) -> bytes:
|
|
"""Create NTRIP v2 HTTP request."""
|
|
auth = base64.b64encode(f"{user}:{password}".encode("utf-8")).decode("ascii")
|
|
req = (
|
|
f"GET /{mount} HTTP/1.1\r\n"
|
|
f"Host: {host}:{port}\r\n"
|
|
f"Ntrip-Version: Ntrip/2.0\r\n"
|
|
f"User-Agent: NTRIP-Test/1.0\r\n"
|
|
f"Connection: close\r\n"
|
|
f"Authorization: Basic {auth}\r\n\r\n"
|
|
)
|
|
return req.encode("ascii")
|
|
|
|
|
|
def haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
|
|
"""Calculate distance between two points using Haversine formula."""
|
|
EARTH_RADIUS_M = 6371008.8
|
|
lat1_rad = math.radians(lat1)
|
|
lat2_rad = math.radians(lat2)
|
|
delta_lat = math.radians(lat2 - lat1)
|
|
delta_lon = math.radians(lon2 - lon1)
|
|
a = math.sin(delta_lat / 2) ** 2 + math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(delta_lon / 2) ** 2
|
|
return 2 * EARTH_RADIUS_M * math.asin(min(1.0, math.sqrt(a)))
|
|
|
|
|
|
def main():
|
|
print(f"NTRIP Test Client (with HTTP Chunked Encoding Support)")
|
|
print(f"=" * 80)
|
|
print(f"Caster: {CASTER_HOST}:{CASTER_PORT}/{MOUNTPOINT}")
|
|
print(f"Rover Position: {LAT:.7f}°, {LON:.7f}° @ {ALT:.1f}m")
|
|
print(f"=" * 80)
|
|
print()
|
|
|
|
parser = RTCMParser()
|
|
last_gga_time = 0
|
|
start_time = time.monotonic()
|
|
|
|
try:
|
|
# Connect to caster
|
|
print(f"[{time.strftime('%H:%M:%S')}] Connecting...")
|
|
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
sock.settimeout(30)
|
|
sock.connect((CASTER_HOST, CASTER_PORT))
|
|
|
|
# Send NTRIP request
|
|
sock.sendall(make_ntrip_request(CASTER_HOST, CASTER_PORT, MOUNTPOINT, USERNAME, PASSWORD))
|
|
|
|
# Read HTTP response headers
|
|
header = b""
|
|
while b"\r\n\r\n" not in header:
|
|
chunk = sock.recv(1)
|
|
if not chunk:
|
|
print("ERROR: Caster closed before headers received")
|
|
return
|
|
header += chunk
|
|
|
|
header_text = header.decode("iso-8859-1", errors="replace")
|
|
if "200 OK" not in header_text:
|
|
print(f"ERROR: NTRIP connection failed:")
|
|
print(header_text)
|
|
return
|
|
|
|
print(f"[{time.strftime('%H:%M:%S')}] Connected! Streaming RTCM data...\n")
|
|
|
|
# Main receive loop
|
|
while True:
|
|
now = time.monotonic()
|
|
|
|
# Send GGA every 10 seconds
|
|
if now - last_gga_time >= 10:
|
|
gga = build_gga(LAT, LON, ALT)
|
|
sock.sendall(gga)
|
|
print(f"[{time.strftime('%H:%M:%S')}] → Sent GGA to caster")
|
|
last_gga_time = now
|
|
|
|
# Receive data
|
|
data = sock.recv(4096)
|
|
if not data:
|
|
print(f"\n[{time.strftime('%H:%M:%S')}] Caster closed connection")
|
|
break
|
|
|
|
# Debug: show hex dump of raw data for first few messages
|
|
if DEBUG_HEX and parser.message_count < 5:
|
|
print(f"\n[DEBUG] Raw data ({len(data)} bytes):")
|
|
print(" ".join(f"{b:02X}" for b in data[:min(100, len(data))]))
|
|
if len(data) > 100:
|
|
print(f"... ({len(data) - 100} more bytes)")
|
|
print()
|
|
|
|
# Parse RTCM messages
|
|
messages = parser.parse_messages(data)
|
|
|
|
for msg in messages:
|
|
timestamp = time.strftime('%H:%M:%S')
|
|
msg_type = msg["type"]
|
|
length = msg["length"]
|
|
index = msg["index"]
|
|
|
|
# Build message info line
|
|
info = f"[{timestamp}] MSG #{index:4d} | RTCM {msg_type:4d} | {length:4d} bytes"
|
|
|
|
# Add description if available
|
|
if "description" in msg:
|
|
info += f" | {msg['description']}"
|
|
|
|
# Highlight position messages even if parsing failed
|
|
if msg_type in [1005, 1006]:
|
|
info += " ← BASE POSITION MESSAGE"
|
|
if "base_position" not in msg:
|
|
info += " (PARSING FAILED)"
|
|
if DEBUG_1005_1006 and "raw_hex" in msg:
|
|
info += f"\n Raw hex: {msg['raw_hex'][:80]}..."
|
|
elif DEBUG_1005_1006:
|
|
info += " (PARSED OK)"
|
|
|
|
# Check for base station position
|
|
if "base_position" in msg:
|
|
base = msg["base_position"]
|
|
|
|
# Check if this is first time seeing base station
|
|
is_first = parser.message_count == index and parser.base_station_position == base
|
|
|
|
if is_first:
|
|
info += f"\n ╔════════════════════════════════════════════════════════════════╗"
|
|
info += f"\n ║ 🎯 BASE STATION POSITION ACQUIRED ║"
|
|
info += f"\n ╚════════════════════════════════════════════════════════════════╝"
|
|
else:
|
|
info += f"\n └─ BASE STATION INFO"
|
|
|
|
info += f"\n Station ID: {base.get('station_id', 'N/A')}"
|
|
info += f"\n Position: {base['latitude']:.7f}°, {base['longitude']:.7f}°"
|
|
info += f"\n Altitude: {base['altitude_m']:.2f} m"
|
|
|
|
# ECEF coordinates
|
|
if 'ecef_x' in base:
|
|
info += f"\n ECEF: X={base['ecef_x']:.4f}, Y={base['ecef_y']:.4f}, Z={base['ecef_z']:.4f}"
|
|
|
|
# Calculate baseline distance
|
|
baseline = haversine_m(LAT, LON, base['latitude'], base['longitude'])
|
|
info += f"\n 📏 Baseline Distance: {baseline:.2f} m ({baseline/1000:.3f} km)"
|
|
|
|
if "antenna_height_m" in base:
|
|
info += f"\n 📡 Antenna Height: {base['antenna_height_m']:.4f} m"
|
|
|
|
if "itrf_year" in base and base["itrf_year"]:
|
|
info += f"\n ITRF Year: {base['itrf_year']}"
|
|
|
|
print(info)
|
|
|
|
# Print stats every 10 messages
|
|
if parser.message_count % 10 == 0 and parser.message_count > 0:
|
|
stats = parser.get_stats()
|
|
elapsed = time.monotonic() - start_time
|
|
if elapsed > 0:
|
|
bytes_per_hour = int(stats["total_bytes"] / elapsed * 3600)
|
|
print(f"\n{'─' * 80}")
|
|
print(f"STATS: {stats['message_count']} messages | {stats['total_bytes']:,} bytes | {bytes_per_hour:,} bytes/hour")
|
|
print(f"Message Types: {dict(list(stats['message_types'].items())[:10])}")
|
|
if stats["base_station"]:
|
|
base = stats["base_station"]
|
|
print(f"Base Station: {base['latitude']:.7f}°, {base['longitude']:.7f}° @ {base['altitude_m']:.2f}m")
|
|
baseline = haversine_m(LAT, LON, base['latitude'], base['longitude'])
|
|
print(f"Baseline: {baseline:.2f} m ({baseline/1000:.3f} km)")
|
|
print(f"{'─' * 80}\n")
|
|
|
|
except KeyboardInterrupt:
|
|
print(f"\n\n[{time.strftime('%H:%M:%S')}] Interrupted by user")
|
|
except Exception as e:
|
|
print(f"\nERROR: {e}")
|
|
finally:
|
|
if 'sock' in locals():
|
|
try:
|
|
sock.close()
|
|
except:
|
|
pass
|
|
|
|
# Print final stats
|
|
print(f"\n{'=' * 80}")
|
|
print("FINAL STATISTICS")
|
|
print(f"{'=' * 80}")
|
|
stats = parser.get_stats()
|
|
elapsed = time.monotonic() - start_time
|
|
print(f"Connected for: {int(elapsed)} seconds")
|
|
print(f"Total messages: {stats['message_count']}")
|
|
print(f"Total bytes: {stats['total_bytes']:,}")
|
|
if elapsed > 0:
|
|
print(f"Average rate: {int(stats['total_bytes'] / elapsed * 3600):,} bytes/hour")
|
|
print(f"\nMessage types received:")
|
|
for msg_type, count in sorted(stats['message_types'].items()):
|
|
print(f" RTCM {msg_type:4d}: {count:4d} messages")
|
|
if stats["base_station"]:
|
|
print(f"\nBase Station Information:")
|
|
base = stats["base_station"]
|
|
print(f" Station ID: {base.get('station_id', 'N/A')}")
|
|
print(f" Position: {base['latitude']:.7f}°, {base['longitude']:.7f}°")
|
|
print(f" Altitude: {base['altitude_m']:.2f} m")
|
|
baseline = haversine_m(LAT, LON, base['latitude'], base['longitude'])
|
|
print(f" Baseline from rover: {baseline:.2f} m ({baseline/1000:.3f} km)")
|
|
print(f"{'=' * 80}")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|