Initial commit
This commit is contained in:
brentperteet
317
debug_1005.py
Normal file
317
debug_1005.py
Normal file
@@ -0,0 +1,317 @@
|
||||
#!/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()
|
||||
Reference in New Issue
Block a user