146 lines
4.2 KiB
Python
146 lines
4.2 KiB
Python
"""Decode Wheeltec G90 NMEA (GGA / GNHPR) from a V2 .rscap capture."""
|
|
|
|
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 _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,
|
|
"satellites": _safe_int(fields[6]),
|
|
"heading_valid": quality in {4, 5},
|
|
}
|
|
|
|
|
|
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 GGA/GNHPR lines; host time comes from the containing serial chunk."""
|
|
|
|
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")
|
|
if not (line.startswith("$GNGGA") or line.startswith("$GPGGA") or line.startswith("$GNHPR")):
|
|
continue
|
|
ticks = _host_ticks_for_span(chunks, starts, end)
|
|
try:
|
|
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
|
fields = parse_gga(line)
|
|
else:
|
|
fields = parse_gnhpr(line)
|
|
except ValueError:
|
|
continue
|
|
rows.append(
|
|
RtkSentence(
|
|
sentence_type=str(fields["type"]),
|
|
receive_utc_ticks=int(ticks),
|
|
checksum_valid=nmea_checksum_valid(line),
|
|
fields=fields,
|
|
raw_line=line,
|
|
)
|
|
)
|
|
rows.sort(key=lambda row: row.receive_utc_ticks)
|
|
return rows
|