Initial commit

ntrip_test.py

@@ -0,0 +1,271 @@
+#!/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()