lichun.qu/calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

重构RTK-IMU标定链路并完成机械先验工程验证

Browse Source
This commit is contained in:
lichun.qu
2026-08-25 09:56:25 +08:00
parent c2da6dd192
commit d14ae74117
56 changed files with 118382 additions and 159 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/rscap_v2/g90_rtk.py
+163 -10
View File
@@ -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,
)
tools/rscap_v2/hi13_imu.py
+57 -18
View File
@@ -12,6 +12,7 @@ HI91 (preferred for calibration):
from __future__ import annotations
import struct
from dataclasses import dataclass
import numpy as np
@@ -22,6 +23,29 @@ G0 = 9.80665
DEG2RAD = np.pi / 180.0
@dataclass(frozen=True)
class Hi13Sample:
"""One CRC-valid native HI91 record.
t_s/system_time_ms are sensor-owned. Host receive ticks are retained only
for clock diagnostics and cross-clock fitting.
"""
t_s: float
system_time_ms: int
device_timestamp_us: int
host_receive_utc_ticks: int
gyro_rad_s: tuple[float, float, float]
accel_m_s2: tuple[float, float, float]
rpy_deg: tuple[float, float, float]
quaternion_wxyz: tuple[float, float, float, float]
mag_ut: tuple[float, float, float]
pps_sync_stamp_ms: int
temperature_c: int
air_pressure_pa: float
frame_tag: int = 0x91
def crc16_hi13(frame: bytes, payload_length: int) -> int:
crc = 0
for value in frame[:4]:
@@ -41,8 +65,8 @@ def _update_crc16(crc: int, value: int) -> int:
return crc
def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None:
"""Return (gyro_rad_s, accel_m_s2, device_timestamp_ms) for a CRC-valid HI91 frame."""
def parse_hi91_sample(raw: bytes, host_receive_utc_ticks: int = 0) -> Hi13Sample | None:
"""Decode every calibration-relevant field from a CRC-valid HI91 frame."""
if len(raw) < 6 + 76:
return None
@@ -54,12 +78,34 @@ def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[floa
expected = raw[4] | (raw[5] << 8)
if crc16_hi13(raw, payload_length) != expected:
return None
device_ms = struct.unpack_from("<I", raw, 14)[0]
device_ms = int(struct.unpack_from("<I", raw, 14)[0])
ax, ay, az = struct.unpack_from("<fff", raw, 18)
gx, gy, gz = struct.unpack_from("<fff", raw, 30)
gyro = (gx * DEG2RAD, gy * DEG2RAD, gz * DEG2RAD)
accel = (ax * G0, ay * G0, az * G0)
return gyro, accel, int(device_ms)
return Hi13Sample(
t_s=float(device_ms) * 1e-3,
system_time_ms=device_ms,
device_timestamp_us=device_ms * 1000,
host_receive_utc_ticks=int(host_receive_utc_ticks),
gyro_rad_s=gyro,
accel_m_s2=accel,
rpy_deg=struct.unpack_from("<fff", raw, 54),
quaternion_wxyz=struct.unpack_from("<ffff", raw, 66),
mag_ut=struct.unpack_from("<fff", raw, 42),
pps_sync_stamp_ms=int(struct.unpack_from("<H", raw, 7)[0]),
temperature_c=int(struct.unpack_from("<b", raw, 9)[0]),
air_pressure_pa=float(struct.unpack_from("<f", raw, 10)[0]),
)
def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None:
"""Backward-compatible compact HI91 decoder."""
sample = parse_hi91_sample(raw)
if sample is None:
return None
return sample.gyro_rad_s, sample.accel_m_s2, sample.system_time_ms
def iter_hi13_imu_samples(
@@ -67,14 +113,14 @@ def iter_hi13_imu_samples(
*,
host_utc_ticks_min: int | None = None,
host_utc_ticks_max: int | None = None,
) -> list[ImuSample]:
) -> list[Hi13Sample]:
"""Return CRC-valid HI91 samples sorted by device timestamp.
Streams chunk-by-chunk (no giant join) and can skip whole chunks outside the
host UTC receive window before parsing.
"""
samples: list[ImuSample] = []
samples: list[Hi13Sample] = []
carry = b""
for chunk in capture.chunks:
if host_utc_ticks_min is not None and chunk.receive_utc_ticks < host_utc_ticks_min:
@@ -102,25 +148,16 @@ def iter_hi13_imu_samples(
if end > len(stream):
carry = stream[sync:]
break
parsed = parse_hi91_frame(stream[sync:end])
host_ticks = chunk.receive_utc_ticks
parsed = parse_hi91_sample(stream[sync:end], host_ticks)
cursor = end
if parsed is None:
continue
gyro, accel, device_ms = parsed
host_ticks = chunk.receive_utc_ticks
if host_utc_ticks_min is not None and host_ticks < host_utc_ticks_min:
continue
if host_utc_ticks_max is not None and host_ticks > host_utc_ticks_max:
continue
samples.append(
ImuSample(
t_s=float(device_ms) * 1e-3,
gyro_rad_s=gyro,
accel_m_s2=accel,
host_receive_utc_ticks=host_ticks,
device_timestamp_us=int(device_ms) * 1000,
)
)
samples.append(parsed)
else:
carry = b""
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
@@ -129,8 +166,10 @@ def iter_hi13_imu_samples(
__all__ = [
"ImuSample",
"Hi13Sample",
"crc16_hi13",
"iter_hi13_imu_samples",
"parse_hi91_frame",
"parse_hi91_sample",
"samples_to_arrays",
]