402 lines
15 KiB
Python
402 lines
15 KiB
Python
"""
|
|
Enhanced RTCM3 parser with detailed field extraction and CSV export.
|
|
Supports parsing common RTCM message types with human-readable output.
|
|
"""
|
|
import csv
|
|
import math
|
|
from datetime import datetime
|
|
from typing import Any
|
|
|
|
|
|
class RTCMDetailedParser:
|
|
"""Enhanced RTCM parser that extracts detailed fields from messages."""
|
|
|
|
def __init__(self):
|
|
self.messages: list[dict[str, Any]] = []
|
|
self.message_count = 0
|
|
|
|
def parse_messages(self, data: bytes, timestamp: str | None = None) -> list[dict[str, Any]]:
|
|
"""Parse RTCM3 messages and extract detailed fields."""
|
|
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:
|
|
msg_type = (data[i+3] << 4) | (data[i+4] >> 4)
|
|
msg_data = data[i+3:i+3+length]
|
|
|
|
self.message_count += 1
|
|
|
|
msg_info: dict[str, Any] = {
|
|
"timestamp": timestamp or datetime.utcnow().isoformat(),
|
|
"message_number": self.message_count,
|
|
"message_type": msg_type,
|
|
"length_bytes": length,
|
|
}
|
|
|
|
# Parse specific message types
|
|
if msg_type == 1005:
|
|
msg_info.update(self._parse_1005(msg_data))
|
|
elif msg_type == 1006:
|
|
msg_info.update(self._parse_1006(msg_data))
|
|
elif msg_type == 1007:
|
|
msg_info.update(self._parse_1007(msg_data))
|
|
elif msg_type == 1008:
|
|
msg_info.update(self._parse_1008(msg_data))
|
|
elif msg_type == 1033:
|
|
msg_info.update(self._parse_1033(msg_data))
|
|
elif msg_type in [1074, 1084, 1094, 1124]:
|
|
msg_info.update(self._parse_msm4(msg_data, msg_type))
|
|
elif msg_type in [1077, 1087, 1097, 1127]:
|
|
msg_info.update(self._parse_msm7(msg_data, msg_type))
|
|
elif msg_type == 1019:
|
|
msg_info.update(self._parse_1019(msg_data))
|
|
elif msg_type == 1020:
|
|
msg_info.update(self._parse_1020(msg_data))
|
|
|
|
messages.append(msg_info)
|
|
self.messages.append(msg_info)
|
|
i += msg_total_len
|
|
continue
|
|
i += 1
|
|
|
|
return messages
|
|
|
|
def _parse_1005(self, data: bytes) -> dict[str, Any]:
|
|
"""Parse RTCM 1005: Stationary RTK Reference Station ARP."""
|
|
result = {"message_name": "Stationary RTK Reference Station ARP"}
|
|
|
|
try:
|
|
if len(data) < 19:
|
|
return result
|
|
|
|
station_id = ((data[1] & 0x0F) << 8) | data[2]
|
|
itrf_year = (data[3] >> 2) & 0x3F
|
|
gps_ind = (data[3] >> 1) & 0x01
|
|
glonass_ind = data[3] & 0x01
|
|
galileo_ind = (data[4] >> 7) & 0x01
|
|
ref_station_ind = (data[4] >> 6) & 0x01
|
|
|
|
# ECEF-X (38 bits signed)
|
|
ecef_x_raw = ((data[3] & 0x03) << 36) | (data[4] << 28) | (data[5] << 20) | (data[6] << 12) | (data[7] << 4) | (data[8] >> 4)
|
|
if ecef_x_raw & (1 << 37):
|
|
ecef_x_raw -= (1 << 38)
|
|
ecef_x = ecef_x_raw * 0.0001
|
|
|
|
# ECEF-Y (38 bits signed)
|
|
ecef_y_raw = ((data[8] & 0x0F) << 34) | (data[9] << 26) | (data[10] << 18) | (data[11] << 10) | (data[12] << 2) | (data[13] >> 6)
|
|
if ecef_y_raw & (1 << 37):
|
|
ecef_y_raw -= (1 << 38)
|
|
ecef_y = ecef_y_raw * 0.0001
|
|
|
|
# ECEF-Z (38 bits signed)
|
|
ecef_z_raw = ((data[13] & 0x3F) << 32) | (data[14] << 24) | (data[15] << 16) | (data[16] << 8) | data[17]
|
|
if ecef_z_raw & (1 << 37):
|
|
ecef_z_raw -= (1 << 38)
|
|
ecef_z = ecef_z_raw * 0.0001
|
|
|
|
lat, lon, alt = self._ecef_to_lla(ecef_x, ecef_y, ecef_z)
|
|
|
|
result.update({
|
|
"station_id": station_id,
|
|
"itrf_year": itrf_year + 1980 if itrf_year else None,
|
|
"gps_indicator": gps_ind,
|
|
"glonass_indicator": glonass_ind,
|
|
"galileo_indicator": galileo_ind,
|
|
"reference_station_indicator": ref_station_ind,
|
|
"ecef_x_m": ecef_x,
|
|
"ecef_y_m": ecef_y,
|
|
"ecef_z_m": ecef_z,
|
|
"latitude_deg": lat,
|
|
"longitude_deg": lon,
|
|
"altitude_m": alt,
|
|
})
|
|
except Exception as e:
|
|
result["parse_error"] = str(e)
|
|
|
|
return result
|
|
|
|
def _parse_1006(self, data: bytes) -> dict[str, Any]:
|
|
"""Parse RTCM 1006: Stationary RTK Reference Station ARP with Antenna Height."""
|
|
result = self._parse_1005(data)
|
|
result["message_name"] = "Stationary RTK Reference Station ARP + Antenna Height"
|
|
|
|
try:
|
|
if len(data) >= 21:
|
|
antenna_height_raw = (data[18] << 8) | data[19]
|
|
result["antenna_height_m"] = antenna_height_raw * 0.0001
|
|
except Exception as e:
|
|
result["parse_error"] = str(e)
|
|
|
|
return result
|
|
|
|
def _parse_1007(self, data: bytes) -> dict[str, Any]:
|
|
"""Parse RTCM 1007: Antenna Descriptor."""
|
|
result = {"message_name": "Antenna Descriptor"}
|
|
|
|
try:
|
|
if len(data) < 5:
|
|
return result
|
|
|
|
station_id = ((data[1] & 0x0F) << 8) | data[2]
|
|
descriptor_len = data[3]
|
|
|
|
if len(data) >= 4 + descriptor_len:
|
|
descriptor = data[4:4+descriptor_len].decode('ascii', errors='ignore')
|
|
result.update({
|
|
"station_id": station_id,
|
|
"antenna_descriptor": descriptor,
|
|
"descriptor_length": descriptor_len,
|
|
})
|
|
except Exception as e:
|
|
result["parse_error"] = str(e)
|
|
|
|
return result
|
|
|
|
def _parse_1008(self, data: bytes) -> dict[str, Any]:
|
|
"""Parse RTCM 1008: Antenna Descriptor & Serial Number."""
|
|
result = {"message_name": "Antenna Descriptor & Serial Number"}
|
|
|
|
try:
|
|
if len(data) < 5:
|
|
return result
|
|
|
|
station_id = ((data[1] & 0x0F) << 8) | data[2]
|
|
descriptor_len = data[3]
|
|
|
|
if len(data) >= 4 + descriptor_len:
|
|
descriptor = data[4:4+descriptor_len].decode('ascii', errors='ignore')
|
|
|
|
# Setup ID (8 bits)
|
|
setup_id_offset = 4 + descriptor_len
|
|
if len(data) > setup_id_offset:
|
|
setup_id = data[setup_id_offset]
|
|
|
|
# Serial number length and value
|
|
serial_len_offset = setup_id_offset + 1
|
|
if len(data) > serial_len_offset:
|
|
serial_len = data[serial_len_offset]
|
|
serial_offset = serial_len_offset + 1
|
|
|
|
if len(data) >= serial_offset + serial_len:
|
|
serial_number = data[serial_offset:serial_offset+serial_len].decode('ascii', errors='ignore')
|
|
|
|
result.update({
|
|
"station_id": station_id,
|
|
"antenna_descriptor": descriptor,
|
|
"antenna_setup_id": setup_id,
|
|
"antenna_serial_number": serial_number,
|
|
"descriptor_length": descriptor_len,
|
|
"serial_length": serial_len,
|
|
})
|
|
except Exception as e:
|
|
result["parse_error"] = str(e)
|
|
|
|
return result
|
|
|
|
def _parse_1033(self, data: bytes) -> dict[str, Any]:
|
|
"""Parse RTCM 1033: Receiver and Antenna Descriptors."""
|
|
result = {"message_name": "Receiver and Antenna Descriptors"}
|
|
|
|
try:
|
|
if len(data) < 6:
|
|
return result
|
|
|
|
station_id = ((data[1] & 0x0F) << 8) | data[2]
|
|
|
|
# Antenna descriptor
|
|
antenna_desc_len = data[3]
|
|
offset = 4
|
|
antenna_descriptor = ""
|
|
if len(data) >= offset + antenna_desc_len:
|
|
antenna_descriptor = data[offset:offset+antenna_desc_len].decode('ascii', errors='ignore')
|
|
offset += antenna_desc_len
|
|
|
|
# Antenna setup ID
|
|
antenna_setup_id = data[offset] if len(data) > offset else None
|
|
offset += 1
|
|
|
|
# Antenna serial number
|
|
antenna_serial_len = data[offset] if len(data) > offset else 0
|
|
offset += 1
|
|
antenna_serial = ""
|
|
if len(data) >= offset + antenna_serial_len:
|
|
antenna_serial = data[offset:offset+antenna_serial_len].decode('ascii', errors='ignore')
|
|
offset += antenna_serial_len
|
|
|
|
# Receiver descriptor
|
|
receiver_desc_len = data[offset] if len(data) > offset else 0
|
|
offset += 1
|
|
receiver_descriptor = ""
|
|
if len(data) >= offset + receiver_desc_len:
|
|
receiver_descriptor = data[offset:offset+receiver_desc_len].decode('ascii', errors='ignore')
|
|
offset += receiver_desc_len
|
|
|
|
# Receiver firmware
|
|
receiver_fw_len = data[offset] if len(data) > offset else 0
|
|
offset += 1
|
|
receiver_firmware = ""
|
|
if len(data) >= offset + receiver_fw_len:
|
|
receiver_firmware = data[offset:offset+receiver_fw_len].decode('ascii', errors='ignore')
|
|
offset += receiver_fw_len
|
|
|
|
# Receiver serial number
|
|
receiver_serial_len = data[offset] if len(data) > offset else 0
|
|
offset += 1
|
|
receiver_serial = ""
|
|
if len(data) >= offset + receiver_serial_len:
|
|
receiver_serial = data[offset:offset+receiver_serial_len].decode('ascii', errors='ignore')
|
|
|
|
result.update({
|
|
"station_id": station_id,
|
|
"antenna_descriptor": antenna_descriptor,
|
|
"antenna_setup_id": antenna_setup_id,
|
|
"antenna_serial_number": antenna_serial,
|
|
"receiver_descriptor": receiver_descriptor,
|
|
"receiver_firmware": receiver_firmware,
|
|
"receiver_serial_number": receiver_serial,
|
|
})
|
|
except Exception as e:
|
|
result["parse_error"] = str(e)
|
|
|
|
return result
|
|
|
|
def _parse_msm4(self, data: bytes, msg_type: int) -> dict[str, Any]:
|
|
"""Parse MSM4 messages (basic observation data)."""
|
|
constellation = {1074: "GPS", 1084: "GLONASS", 1094: "Galileo", 1124: "BeiDou"}
|
|
result = {
|
|
"message_name": f"{constellation.get(msg_type, 'Unknown')} MSM4 Observations",
|
|
"constellation": constellation.get(msg_type, "Unknown"),
|
|
}
|
|
|
|
try:
|
|
station_id = ((data[1] & 0x0F) << 8) | data[2]
|
|
epoch_time_raw = (data[3] << 22) | (data[4] << 14) | (data[5] << 6) | (data[6] >> 2)
|
|
epoch_time_ms = epoch_time_raw
|
|
|
|
result.update({
|
|
"station_id": station_id,
|
|
"epoch_time_ms": epoch_time_ms,
|
|
"multiple_message_bit": (data[6] >> 1) & 0x01,
|
|
})
|
|
except Exception as e:
|
|
result["parse_error"] = str(e)
|
|
|
|
return result
|
|
|
|
def _parse_msm7(self, data: bytes, msg_type: int) -> dict[str, Any]:
|
|
"""Parse MSM7 messages (full observation data)."""
|
|
constellation = {1077: "GPS", 1087: "GLONASS", 1097: "Galileo", 1127: "BeiDou"}
|
|
result = {
|
|
"message_name": f"{constellation.get(msg_type, 'Unknown')} MSM7 Observations",
|
|
"constellation": constellation.get(msg_type, "Unknown"),
|
|
}
|
|
|
|
try:
|
|
station_id = ((data[1] & 0x0F) << 8) | data[2]
|
|
epoch_time_raw = (data[3] << 22) | (data[4] << 14) | (data[5] << 6) | (data[6] >> 2)
|
|
epoch_time_ms = epoch_time_raw
|
|
|
|
result.update({
|
|
"station_id": station_id,
|
|
"epoch_time_ms": epoch_time_ms,
|
|
"multiple_message_bit": (data[6] >> 1) & 0x01,
|
|
})
|
|
except Exception as e:
|
|
result["parse_error"] = str(e)
|
|
|
|
return result
|
|
|
|
def _parse_1019(self, data: bytes) -> dict[str, Any]:
|
|
"""Parse RTCM 1019: GPS Ephemeris."""
|
|
result = {"message_name": "GPS Ephemeris"}
|
|
|
|
try:
|
|
if len(data) < 62:
|
|
return result
|
|
|
|
satellite_id = (data[1] & 0x0F) << 2 | (data[2] >> 6)
|
|
week_number = ((data[2] & 0x3F) << 4) | (data[3] >> 4)
|
|
|
|
result.update({
|
|
"satellite_id": satellite_id,
|
|
"gps_week_number": week_number,
|
|
})
|
|
except Exception as e:
|
|
result["parse_error"] = str(e)
|
|
|
|
return result
|
|
|
|
def _parse_1020(self, data: bytes) -> dict[str, Any]:
|
|
"""Parse RTCM 1020: GLONASS Ephemeris."""
|
|
result = {"message_name": "GLONASS Ephemeris"}
|
|
|
|
try:
|
|
if len(data) < 45:
|
|
return result
|
|
|
|
satellite_id = (data[1] & 0x0F) << 2 | (data[2] >> 6)
|
|
|
|
result.update({
|
|
"satellite_id": satellite_id,
|
|
})
|
|
except Exception as e:
|
|
result["parse_error"] = str(e)
|
|
|
|
return result
|
|
|
|
def _ecef_to_lla(self, x: float, y: float, z: float) -> tuple[float, float, float]:
|
|
"""Convert ECEF to latitude, longitude, altitude (WGS84)."""
|
|
a = 6378137.0
|
|
e2 = 6.69437999014e-3
|
|
|
|
lon = math.atan2(y, x) if (x != 0 or y != 0) else 0.0
|
|
p = math.sqrt(x * x + y * y)
|
|
lat = math.atan2(z, p * (1 - e2))
|
|
|
|
for _ in range(10):
|
|
N = a / math.sqrt(1 - e2 * math.sin(lat) ** 2)
|
|
alt = p / math.cos(lat) - N
|
|
lat_new = math.atan2(z, p * (1 - e2 * N / (N + alt)))
|
|
if abs(lat_new - lat) < 1e-12:
|
|
break
|
|
lat = lat_new
|
|
|
|
N = a / math.sqrt(1 - e2 * math.sin(lat) ** 2)
|
|
alt = p / math.cos(lat) - N if abs(math.cos(lat)) > 1e-10 else z / math.sin(lat) - N * (1 - e2)
|
|
|
|
return math.degrees(lat), math.degrees(lon), alt
|
|
|
|
def export_to_csv(self, filename: str, message_types: list[int] | None = None):
|
|
"""Export parsed messages to CSV file."""
|
|
if not self.messages:
|
|
return
|
|
|
|
# Filter by message type if specified
|
|
messages_to_export = self.messages
|
|
if message_types:
|
|
messages_to_export = [m for m in self.messages if m.get("message_type") in message_types]
|
|
|
|
if not messages_to_export:
|
|
return
|
|
|
|
# Collect all unique field names
|
|
fieldnames = set()
|
|
for msg in messages_to_export:
|
|
fieldnames.update(msg.keys())
|
|
|
|
# Sort fieldnames for consistent column order
|
|
fieldnames = sorted(fieldnames)
|
|
|
|
with open(filename, 'w', newline='', encoding='utf-8') as csvfile:
|
|
writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
|
|
writer.writeheader()
|
|
for msg in messages_to_export:
|
|
writer.writerow(msg)
|
|
|
|
print(f"Exported {len(messages_to_export)} messages to {filename}")
|