318 lines
9.2 KiB
Python
318 lines
9.2 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
Debug RTCM 1005/1006 message parsing with detailed bit extraction.
|
|
"""
|
|
import base64
|
|
import socket
|
|
import time
|
|
from datetime import datetime
|
|
import math
|
|
|
|
|
|
# NTRIP configuration
|
|
CASTER_HOST = "truertk.pointonenav.com"
|
|
CASTER_PORT = 2101
|
|
MOUNTPOINT = "AUTO"
|
|
USERNAME = "9t7fwfbm57"
|
|
PASSWORD = "96m7bec9g8"
|
|
|
|
LAT = 36.1140884
|
|
LON = -97.0880663
|
|
ALT = 390.0
|
|
|
|
|
|
def build_gga(lat: float, lon: float, alt: float) -> bytes:
|
|
"""Build NMEA GGA sentence."""
|
|
now_utc = datetime.utcnow().strftime("%H%M%S")
|
|
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", "12", "1.0", f"{alt:.1f}", "M", "", "M", "", ""]
|
|
core = ",".join(fields)
|
|
checksum = 0
|
|
for char in core:
|
|
checksum ^= ord(char)
|
|
return f"${core}*{checksum:02X}\r\n".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: Debug-1005/1.0\r\n"
|
|
f"Connection: close\r\n"
|
|
f"Authorization: Basic {auth}\r\n\r\n"
|
|
)
|
|
return req.encode("ascii")
|
|
|
|
|
|
def parse_chunked_data(data: bytes) -> bytes:
|
|
"""Parse HTTP chunked encoding."""
|
|
clean_data = bytearray()
|
|
i = 0
|
|
|
|
while i < len(data):
|
|
line_end = data.find(b'\r\n', i)
|
|
if line_end == -1:
|
|
break
|
|
|
|
chunk_size_line = data[i:line_end]
|
|
try:
|
|
chunk_size_str = chunk_size_line.decode('ascii', errors='ignore').split(';')[0].strip()
|
|
chunk_size = int(chunk_size_str, 16)
|
|
|
|
if chunk_size == 0:
|
|
break
|
|
|
|
chunk_data_start = line_end + 2
|
|
chunk_data_end = chunk_data_start + chunk_size
|
|
|
|
if chunk_data_end + 2 > len(data):
|
|
break
|
|
|
|
chunk_data = data[chunk_data_start:chunk_data_end]
|
|
clean_data.extend(chunk_data)
|
|
i = chunk_data_end + 2
|
|
|
|
except (ValueError, UnicodeDecodeError):
|
|
i += 1
|
|
continue
|
|
|
|
return bytes(clean_data)
|
|
|
|
|
|
def debug_parse_1005(payload: bytes):
|
|
"""Debug parse RTCM 1005 with detailed output."""
|
|
print(f"\n{'=' * 80}")
|
|
print(f"RTCM 1005 MESSAGE DEBUG")
|
|
print(f"{'=' * 80}")
|
|
print(f"Payload length: {len(payload)} bytes")
|
|
print(f"Hex: {payload[:50].hex()}")
|
|
print()
|
|
|
|
# Show as bits
|
|
bits_str = ''.join(f'{b:08b}' for b in payload[:25])
|
|
print("First 200 bits:")
|
|
for i in range(0, min(200, len(bits_str)), 50):
|
|
print(f" {i:3d}: {bits_str[i:i+50]}")
|
|
print()
|
|
|
|
# Parse fields manually
|
|
bit_array = []
|
|
for byte in payload:
|
|
for i in range(7, -1, -1):
|
|
bit_array.append((byte >> i) & 1)
|
|
|
|
def get_bits(start, length):
|
|
"""Extract bits from array."""
|
|
value = 0
|
|
for i in range(length):
|
|
value = (value << 1) | bit_array[start + i]
|
|
return value
|
|
|
|
def get_signed_bits(start, length):
|
|
"""Extract signed value."""
|
|
value = get_bits(start, length)
|
|
if value & (1 << (length - 1)):
|
|
value -= (1 << length)
|
|
return value
|
|
|
|
pos = 0
|
|
|
|
# Message Number (12 bits)
|
|
msg_num = get_bits(pos, 12)
|
|
print(f"Bit {pos:3d}-{pos+11:3d} (12 bits): Message Number = {msg_num}")
|
|
pos += 12
|
|
|
|
# Station ID (12 bits)
|
|
station_id = get_bits(pos, 12)
|
|
print(f"Bit {pos:3d}-{pos+11:3d} (12 bits): Station ID = {station_id}")
|
|
pos += 12
|
|
|
|
# ITRF Year (6 bits)
|
|
itrf = get_bits(pos, 6)
|
|
print(f"Bit {pos:3d}-{pos+5:3d} (6 bits): ITRF Year = {itrf} ({1980 + itrf if itrf else 'N/A'})")
|
|
pos += 6
|
|
|
|
# GPS (1), GLONASS (1), Galileo (1), Ref Station (1)
|
|
gps = get_bits(pos, 1)
|
|
pos += 1
|
|
glonass = get_bits(pos, 1)
|
|
pos += 1
|
|
galileo = get_bits(pos, 1)
|
|
pos += 1
|
|
ref_station = get_bits(pos, 1)
|
|
pos += 1
|
|
print(f"Indicators: GPS={gps}, GLONASS={glonass}, Galileo={galileo}, RefStation={ref_station}")
|
|
|
|
# ECEF-X (38 bits, signed, 0.0001m resolution)
|
|
ecef_x_raw = get_signed_bits(pos, 38)
|
|
ecef_x = ecef_x_raw * 0.0001
|
|
print(f"Bit {pos:3d}-{pos+37:3d} (38 bits): ECEF-X = {ecef_x_raw} raw = {ecef_x:.4f} m")
|
|
pos += 38
|
|
|
|
# Single Receiver Oscillator (1 bit)
|
|
oscillator = get_bits(pos, 1)
|
|
pos += 1
|
|
|
|
# Reserved (1 bit)
|
|
pos += 1
|
|
|
|
# ECEF-Y (38 bits, signed)
|
|
ecef_y_raw = get_signed_bits(pos, 38)
|
|
ecef_y = ecef_y_raw * 0.0001
|
|
print(f"Bit {pos:3d}-{pos+37:3d} (38 bits): ECEF-Y = {ecef_y_raw} raw = {ecef_y:.4f} m")
|
|
pos += 38
|
|
|
|
# Quarter Cycle Indicator (2 bits)
|
|
quarter = get_bits(pos, 2)
|
|
pos += 2
|
|
|
|
# ECEF-Z (38 bits, signed)
|
|
ecef_z_raw = get_signed_bits(pos, 38)
|
|
ecef_z = ecef_z_raw * 0.0001
|
|
print(f"Bit {pos:3d}-{pos+37:3d} (38 bits): ECEF-Z = {ecef_z_raw} raw = {ecef_z:.4f} m")
|
|
pos += 38
|
|
|
|
print(f"\nTotal bits parsed: {pos}")
|
|
print()
|
|
|
|
# Convert ECEF to LLA
|
|
print(f"ECEF Coordinates:")
|
|
print(f" X: {ecef_x:14.4f} m")
|
|
print(f" Y: {ecef_y:14.4f} m")
|
|
print(f" Z: {ecef_z:14.4f} m")
|
|
print()
|
|
|
|
# WGS84 conversion
|
|
a = 6378137.0
|
|
e2 = 6.69437999014e-3
|
|
|
|
lon_rad = math.atan2(ecef_y, ecef_x)
|
|
p = math.sqrt(ecef_x * ecef_x + ecef_y * ecef_y)
|
|
lat_rad = math.atan2(ecef_z, p * (1 - e2))
|
|
|
|
for _ in range(10):
|
|
N = a / math.sqrt(1 - e2 * math.sin(lat_rad) ** 2)
|
|
alt = p / math.cos(lat_rad) - N
|
|
lat_new = math.atan2(ecef_z, p * (1 - e2 * N / (N + alt)))
|
|
if abs(lat_new - lat_rad) < 1e-12:
|
|
break
|
|
lat_rad = lat_new
|
|
|
|
N = a / math.sqrt(1 - e2 * math.sin(lat_rad) ** 2)
|
|
alt = p / math.cos(lat_rad) - N if abs(math.cos(lat_rad)) > 1e-10 else ecef_z / math.sin(lat_rad) - N * (1 - e2)
|
|
|
|
lat = math.degrees(lat_rad)
|
|
lon = math.degrees(lon_rad)
|
|
|
|
print(f"Geodetic Coordinates (WGS84):")
|
|
print(f" Latitude: {lat:12.7f}°")
|
|
print(f" Longitude: {lon:12.7f}°")
|
|
print(f" Altitude: {alt:12.2f} m")
|
|
print()
|
|
|
|
# Distance from rover
|
|
rover_lat = 36.1140884
|
|
rover_lon = -97.0880663
|
|
|
|
lat1_rad = math.radians(rover_lat)
|
|
lat2_rad = math.radians(lat)
|
|
delta_lat = math.radians(lat - rover_lat)
|
|
delta_lon = math.radians(lon - rover_lon)
|
|
a_hav = math.sin(delta_lat / 2) ** 2 + math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(delta_lon / 2) ** 2
|
|
distance = 2 * 6371008.8 * math.asin(min(1.0, math.sqrt(a_hav)))
|
|
|
|
print(f"Distance from rover ({rover_lat:.4f}°, {rover_lon:.4f}°):")
|
|
print(f" {distance:.2f} m ({distance/1000:.3f} km)")
|
|
|
|
|
|
def main():
|
|
print("RTCM 1005/1006 Debug Tool")
|
|
print("Connecting to NTRIP caster...")
|
|
|
|
try:
|
|
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
sock.settimeout(30)
|
|
sock.connect((CASTER_HOST, CASTER_PORT))
|
|
sock.sendall(make_ntrip_request(CASTER_HOST, CASTER_PORT, MOUNTPOINT, USERNAME, PASSWORD))
|
|
|
|
# Read headers
|
|
header = b""
|
|
while b"\r\n\r\n" not in header:
|
|
chunk = sock.recv(1)
|
|
if not chunk:
|
|
print("ERROR: Connection closed")
|
|
return
|
|
header += chunk
|
|
|
|
if "200 OK" not in header.decode("iso-8859-1", errors="replace"):
|
|
print("ERROR: Connection failed")
|
|
return
|
|
|
|
print("Connected! Searching for RTCM 1005 or 1006 message...\n")
|
|
|
|
last_gga = 0
|
|
found_count = 0
|
|
|
|
while found_count < 3:
|
|
now = time.monotonic()
|
|
if now - last_gga >= 10:
|
|
sock.sendall(build_gga(LAT, LON, ALT))
|
|
last_gga = now
|
|
|
|
raw_data = sock.recv(4096)
|
|
if not raw_data:
|
|
break
|
|
|
|
# Parse chunks
|
|
data = parse_chunked_data(raw_data)
|
|
|
|
# Find RTCM messages
|
|
i = 0
|
|
while i < len(data):
|
|
if data[i] == 0xD3 and i + 2 < len(data):
|
|
length = ((data[i+1] & 0x03) << 8) | data[i+2]
|
|
msg_total_len = 3 + length + 3
|
|
|
|
if i + msg_total_len <= len(data) and length >= 3:
|
|
payload = data[i+3:i+3+length]
|
|
msg_type = (payload[0] << 4) | (payload[1] >> 4)
|
|
|
|
if msg_type in [1005, 1006]:
|
|
found_count += 1
|
|
print(f"\n{'#' * 80}")
|
|
print(f"FOUND MESSAGE {msg_type} (#{found_count})")
|
|
print(f"{'#' * 80}")
|
|
debug_parse_1005(payload)
|
|
|
|
i += msg_total_len
|
|
continue
|
|
i += 1
|
|
|
|
except KeyboardInterrupt:
|
|
print("\nInterrupted")
|
|
except Exception as e:
|
|
print(f"ERROR: {e}")
|
|
import traceback
|
|
traceback.print_exc()
|
|
finally:
|
|
if 'sock' in locals():
|
|
sock.close()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|