新增独立RTK与IMU外参标定流程及质量验证

tools/rscap_v2/g90_rtk.py

@@ -0,0 +1,145 @@
+"""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