299 lines
9.8 KiB
Python
299 lines
9.8 KiB
Python
"""Decode calibration-relevant Wheeltec G90 logs from a V2 capture.
|
|
|
|
GNSS-owned measurement time is preserved for every record. Host receive time
|
|
only identifies the chunk that completed the line and must not be substituted
|
|
for the measurement timestamp.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import bisect
|
|
import math
|
|
from dataclasses import dataclass
|
|
|
|
from .capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
|
|
|
|
|
|
def nmea_checksum_valid(line: str) -> bool:
|
|
star = line.rfind("*")
|
|
if star < 0:
|
|
return False
|
|
try:
|
|
expected = int(line[star + 1 : star + 3], 16)
|
|
except ValueError:
|
|
return False
|
|
value = 0
|
|
for char in line[1:star]:
|
|
value ^= ord(char)
|
|
return value == expected
|
|
|
|
|
|
def unicore_checksum_valid(line: str) -> bool:
|
|
"""Validate the CRC32 suffix used by Unicore hash-prefixed logs."""
|
|
|
|
star = line.rfind("*")
|
|
if star < 0:
|
|
return False
|
|
try:
|
|
expected = int(line[star + 1 : star + 9], 16)
|
|
except ValueError:
|
|
return False
|
|
crc = 0
|
|
for value in line[1:star].encode("ascii", "replace"):
|
|
crc ^= value
|
|
for _ in range(8):
|
|
crc = (crc >> 1) ^ (0xEDB88320 if crc & 1 else 0)
|
|
return (crc & 0xFFFFFFFF) == expected
|
|
|
|
|
|
def g90_checksum_valid(line: str) -> bool:
|
|
if line.startswith("$"):
|
|
return nmea_checksum_valid(line)
|
|
if line.startswith("#"):
|
|
return unicore_checksum_valid(line)
|
|
return False
|
|
|
|
|
|
def _safe_float(value: str):
|
|
try:
|
|
return float(value)
|
|
except (TypeError, ValueError):
|
|
return None
|
|
|
|
|
|
def _safe_int(value: str):
|
|
try:
|
|
return int(value)
|
|
except (TypeError, ValueError):
|
|
return None
|
|
|
|
|
|
def parse_nmea_latlon(value: str, hemisphere: str):
|
|
raw = _safe_float(value)
|
|
if raw is None:
|
|
return None
|
|
degrees = math.floor(raw / 100.0)
|
|
result = degrees + (raw - degrees * 100.0) / 60.0
|
|
if hemisphere.upper() in ("S", "W"):
|
|
result = -result
|
|
return result
|
|
|
|
|
|
def parse_gga(line: str) -> dict:
|
|
fields = line[: line.rfind("*")].split(",")
|
|
if len(fields) < 10:
|
|
raise ValueError("GGA has too few fields")
|
|
return {
|
|
"type": "GGA",
|
|
"position_time_utc": fields[1],
|
|
"lat_deg": parse_nmea_latlon(fields[2], fields[3]),
|
|
"lon_deg": parse_nmea_latlon(fields[4], fields[5]),
|
|
"fix_quality": _safe_int(fields[6]),
|
|
"satellites": _safe_int(fields[7]),
|
|
"hdop": _safe_float(fields[8]),
|
|
"altitude_m": _safe_float(fields[9]),
|
|
}
|
|
|
|
|
|
def parse_gnhpr(line: str) -> dict:
|
|
fields = line[: line.rfind("*")].split(",")
|
|
if len(fields) < 7:
|
|
raise ValueError("GNHPR has too few fields")
|
|
quality = _safe_int(fields[5])
|
|
return {
|
|
"type": "GNHPR",
|
|
"position_time_utc": fields[1],
|
|
"heading_deg": _safe_float(fields[2]),
|
|
"pitch_deg": _safe_float(fields[3]),
|
|
"roll_deg": _safe_float(fields[4]),
|
|
"heading_quality": quality,
|
|
"heading_satellites": _safe_int(fields[6]),
|
|
"heading_age_s": _safe_float(fields[7]) if len(fields) > 7 else None,
|
|
"heading_station_id": fields[8] if len(fields) > 8 else None,
|
|
"heading_valid": quality == 4,
|
|
}
|
|
|
|
|
|
def _split_unicore(line: str) -> tuple[list[str], list[str]]:
|
|
before_checksum = line[: line.rfind("*")]
|
|
header, payload = before_checksum.split(";", 1)
|
|
return header[1:].split(","), payload.split(",")
|
|
|
|
|
|
def _parse_unicore_header(fields: list[str]) -> dict:
|
|
if len(fields) < 9:
|
|
raise ValueError("Unicore ASCII header is incomplete")
|
|
return {
|
|
"gnss_week": _safe_int(fields[4]),
|
|
"gnss_tow_ms": _safe_int(fields[5]),
|
|
"leap_seconds": _safe_int(fields[8]),
|
|
}
|
|
|
|
|
|
def parse_bestnava(line: str) -> dict:
|
|
"""Parse BESTNAVA position and Doppler-velocity fields."""
|
|
|
|
header, fields = _split_unicore(line)
|
|
if len(fields) < 30:
|
|
raise ValueError("BESTNAVA has too few fields")
|
|
result = {
|
|
"type": "BESTNAVA",
|
|
**_parse_unicore_header(header),
|
|
"position_status": fields[0],
|
|
"position_type": fields[1],
|
|
"lat_deg": _safe_float(fields[2]),
|
|
"lon_deg": _safe_float(fields[3]),
|
|
"altitude_m": _safe_float(fields[4]),
|
|
"undulation_m": _safe_float(fields[5]),
|
|
"lat_std_m": _safe_float(fields[7]),
|
|
"lon_std_m": _safe_float(fields[8]),
|
|
"altitude_std_m": _safe_float(fields[9]),
|
|
"station_id": fields[10].strip('"'),
|
|
"differential_age_s": _safe_float(fields[11]),
|
|
"solution_age_s": _safe_float(fields[12]),
|
|
"satellites": _safe_int(fields[13]),
|
|
"solution_satellites": _safe_int(fields[14]),
|
|
"velocity_status": fields[21],
|
|
"velocity_type": fields[22],
|
|
"velocity_latency_s": _safe_float(fields[23]),
|
|
"velocity_age_s": _safe_float(fields[24]),
|
|
"horizontal_speed_m_s": _safe_float(fields[25]),
|
|
"track_ground_deg": _safe_float(fields[26]),
|
|
"vertical_speed_m_s": _safe_float(fields[27]),
|
|
"vertical_speed_std_m_s": _safe_float(fields[28]),
|
|
"horizontal_speed_std_m_s": _safe_float(fields[29]),
|
|
}
|
|
speed = result["horizontal_speed_m_s"]
|
|
track = result["track_ground_deg"]
|
|
if speed is not None and track is not None:
|
|
angle = math.radians(track)
|
|
result["velocity_east_m_s"] = speed * math.sin(angle)
|
|
result["velocity_north_m_s"] = speed * math.cos(angle)
|
|
else:
|
|
result["velocity_east_m_s"] = None
|
|
result["velocity_north_m_s"] = None
|
|
result["position_fixed"] = (
|
|
result["position_status"] == "SOL_COMPUTED"
|
|
and result["position_type"] == "NARROW_INT"
|
|
)
|
|
result["doppler_velocity_valid"] = (
|
|
result["velocity_status"] == "SOL_COMPUTED"
|
|
and result["velocity_type"] == "DOPPLER_VELOCITY"
|
|
)
|
|
return result
|
|
|
|
|
|
def parse_pvtslna(line: str) -> dict:
|
|
"""Parse PVTSLNA as a quality-rich fallback/diagnostic record."""
|
|
|
|
header, fields = _split_unicore(line)
|
|
if len(fields) < 34:
|
|
raise ValueError("PVTSLNA has too few fields")
|
|
speed_north = _safe_float(fields[17])
|
|
speed_east = _safe_float(fields[18])
|
|
return {
|
|
"type": "PVTSLNA",
|
|
**_parse_unicore_header(header),
|
|
"position_type": fields[0],
|
|
"altitude_m": _safe_float(fields[1]),
|
|
"lat_deg": _safe_float(fields[2]),
|
|
"lon_deg": _safe_float(fields[3]),
|
|
"altitude_std_m": _safe_float(fields[4]),
|
|
"lat_std_m": _safe_float(fields[5]),
|
|
"lon_std_m": _safe_float(fields[6]),
|
|
"differential_age_s": _safe_float(fields[7]),
|
|
"psr_position_type": fields[8],
|
|
"undulation_m": _safe_float(fields[12]),
|
|
"satellites": _safe_int(fields[13]),
|
|
"solution_satellites": _safe_int(fields[14]),
|
|
"velocity_north_m_s": speed_north,
|
|
"velocity_east_m_s": speed_east,
|
|
"horizontal_speed_m_s": (
|
|
None if speed_north is None or speed_east is None
|
|
else math.hypot(speed_north, speed_east)
|
|
),
|
|
"vertical_speed_m_s": _safe_float(fields[19]),
|
|
"heading_type": fields[20],
|
|
"baseline_length_m": _safe_float(fields[21]),
|
|
"heading_deg": _safe_float(fields[22]),
|
|
"pitch_deg": _safe_float(fields[23]),
|
|
"heading_satellites": _safe_int(fields[24]),
|
|
"heading_solution_satellites": _safe_int(fields[25]),
|
|
"gdop": _safe_float(fields[28]),
|
|
"pdop": _safe_float(fields[29]),
|
|
"hdop": _safe_float(fields[30]),
|
|
"htdop": _safe_float(fields[31]),
|
|
"tdop": _safe_float(fields[32]),
|
|
"position_fixed": fields[0] == "NARROW_INT",
|
|
}
|
|
|
|
|
|
def _chunk_starts(chunks: list[RawChunk]) -> list[int]:
|
|
starts = []
|
|
cursor = 0
|
|
for chunk in chunks:
|
|
starts.append(cursor)
|
|
cursor += len(chunk.raw)
|
|
return starts
|
|
|
|
|
|
def _host_ticks_for_span(chunks: list[RawChunk], starts: list[int], end: int) -> int:
|
|
end_index = max(0, min(len(chunks) - 1, bisect.bisect_left(starts, end) - 1))
|
|
return chunks[end_index].receive_utc_ticks
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class RtkSentence:
|
|
sentence_type: str
|
|
receive_utc_ticks: int
|
|
checksum_valid: bool
|
|
fields: dict
|
|
raw_line: str
|
|
|
|
|
|
def iter_g90_sentences(capture: CaptureFile) -> list[RtkSentence]:
|
|
"""Parse native asynchronous GGA/GNHPR/BESTNAVA/PVTSLNA records."""
|
|
|
|
rows: list[RtkSentence] = []
|
|
for _segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
|
stream = b"".join(chunk.raw for chunk in chunks)
|
|
starts = _chunk_starts(chunks)
|
|
cursor = 0
|
|
while cursor < len(stream):
|
|
newline = stream.find(b"\n", cursor)
|
|
if newline < 0:
|
|
break
|
|
end = newline + 1
|
|
raw_line = stream[cursor:end].rstrip(b"\r\n")
|
|
cursor = end
|
|
if not raw_line:
|
|
continue
|
|
line = raw_line.decode("ascii", "replace")
|
|
parser = None
|
|
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
|
parser = parse_gga
|
|
elif line.startswith("$GNHPR"):
|
|
parser = parse_gnhpr
|
|
elif line.startswith("#BESTNAVA"):
|
|
parser = parse_bestnava
|
|
elif line.startswith("#PVTSLNA"):
|
|
parser = parse_pvtslna
|
|
if parser is None:
|
|
continue
|
|
ticks = _host_ticks_for_span(chunks, starts, end)
|
|
try:
|
|
fields = parser(line)
|
|
except ValueError:
|
|
continue
|
|
rows.append(
|
|
RtkSentence(
|
|
sentence_type=str(fields["type"]),
|
|
receive_utc_ticks=int(ticks),
|
|
checksum_valid=g90_checksum_valid(line),
|
|
fields=fields,
|
|
raw_line=line,
|
|
)
|
|
)
|
|
rows.sort(key=lambda row: row.receive_utc_ticks)
|
|
return rows
|