重构RTK-IMU标定链路并完成机械先验工程验证
This commit is contained in:
lichun.qu
+163
-10
@@ -1,4 +1,9 @@
|
||||
"""Decode Wheeltec G90 NMEA (GGA / GNHPR) from a V2 .rscap capture."""
|
||||
"""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
|
||||
|
||||
@@ -23,6 +28,32 @@ def nmea_checksum_valid(line: str) -> bool:
|
||||
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)
|
||||
@@ -76,8 +107,124 @@ def parse_gnhpr(line: str) -> dict:
|
||||
"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},
|
||||
"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",
|
||||
}
|
||||
|
||||
|
||||
@@ -105,7 +252,7 @@ class RtkSentence:
|
||||
|
||||
|
||||
def iter_g90_sentences(capture: CaptureFile) -> list[RtkSentence]:
|
||||
"""Parse GGA/GNHPR lines; host time comes from the containing serial chunk."""
|
||||
"""Parse native asynchronous GGA/GNHPR/BESTNAVA/PVTSLNA records."""
|
||||
|
||||
rows: list[RtkSentence] = []
|
||||
for _segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
@@ -122,21 +269,27 @@ def iter_g90_sentences(capture: CaptureFile) -> list[RtkSentence]:
|
||||
if not raw_line:
|
||||
continue
|
||||
line = raw_line.decode("ascii", "replace")
|
||||
if not (line.startswith("$GNGGA") or line.startswith("$GPGGA") or line.startswith("$GNHPR")):
|
||||
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:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
fields = parse_gga(line)
|
||||
else:
|
||||
fields = parse_gnhpr(line)
|
||||
fields = parser(line)
|
||||
except ValueError:
|
||||
continue
|
||||
rows.append(
|
||||
RtkSentence(
|
||||
sentence_type=str(fields["type"]),
|
||||
receive_utc_ticks=int(ticks),
|
||||
checksum_valid=nmea_checksum_valid(line),
|
||||
checksum_valid=g90_checksum_valid(line),
|
||||
fields=fields,
|
||||
raw_line=line,
|
||||
)
|
||||
|
||||
Reference in New Issue
Block a user