新增原始数据一步导出到 combined：对齐 Lidar-IMU 导出入口，适配 H32/G90/N300
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
+20
-3
@@ -2,10 +2,27 @@
|
||||
|
||||
| 文件 | 输入→输出 |
|
||||
|---|---|
|
||||
| `frontlidar_dlog_export.py` | LiDAR dlog → 逐帧原始点云NPZ；时间来自DObject post tick |
|
||||
| `rscap_v2/parse_rtk_imu_v2.py` | RTK/IMU rscap → JSONL，保存校验状态、主机时间、GNSS/IMU设备字段和原始报文 |
|
||||
| **`export_raw_to_combined.py`** | **一步导出**：逐站 H32 + 全程 G90/N300 `.rscap` → `combined/`（标定直接入口，对标 Lidar-IMU `export_rscap_to_v1`） |
|
||||
| `export_h32_rscap_station.py` | 内部零件：单站 H32 → 雷达帧 NPZ（一般不必单独跑） |
|
||||
| `frontlidar_dlog_export.py` | **旧数据** LiDAR dlog → 逐帧 NPZ；由一步导出在遇到 dlog 站时自动调用 |
|
||||
| `rscap_v2/parse_rtk_imu_v2.py` | 单独解析 RTK/IMU（调试用）；一步导出已内嵌同等逻辑 |
|
||||
| `rscap_v2/h32_msop.py` | H32 MSOP 解码（XYZ / 极坐标 `points_raw`） |
|
||||
| `rscap_v2/n300_imu.py` | N300 FDILink 采样解码 |
|
||||
| `rscap_v2/audit_capture_v2.py` | 检查rscap结构、时间范围和记录统计 |
|
||||
| `build_multisensor_npz.py` | 按LiDAR帧最近邻关联GGA/heading，并附加IMU时间窗 → combined NPZ |
|
||||
| `build_multisensor_npz.py` | 关联雷达帧与 RTK/IMU → combined；一步导出内部调用 |
|
||||
| `prepare_multisensor_station_dataset.py` | combined NPZ → 每站一帧`frames_all`和`reference_poses_*.csv` |
|
||||
|
||||
推荐用法：
|
||||
|
||||
```powershell
|
||||
python tools\export_raw_to_combined.py `
|
||||
--stations-root path\to\stations `
|
||||
--rtk-rscap path\to\rtk.rscap `
|
||||
--imu-rscap path\to\imu.rscap `
|
||||
--out path\to\exported `
|
||||
--overwrite
|
||||
```
|
||||
|
||||
当前标定只使用LiDAR和RTK；IMU保持原始传感器坐标，不参与点云去畸变或外参求解。prepared阶段对站内有效RTK取平均、对heading取圆均值，并选择有效帧序列的中间LiDAR帧。
|
||||
|
||||
G90 `#PVTSLNA` 没有 NMEA `fix_quality` 字段时，解析会写入合成值 `4`，以便沿用 prepare 的固定解筛选（`{4,5}`）。
|
||||
|
||||
+136
-47
@@ -1,9 +1,15 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Build one LiDAR-centric NPZ per frame with matched RTK and an IMU window.
|
||||
|
||||
Inputs are LiDAR frame NPZ files from frontlidar_dlog_export.py and parsed
|
||||
RTK/IMU JSONL files from parse_rtk_imu_v2.py. Raw .rscap files remain the
|
||||
traceability source; this script never modifies them.
|
||||
Inputs are LiDAR frame NPZ files from ``export_h32_rscap_station.py`` (or legacy
|
||||
``frontlidar_dlog_export.py``) and parsed RTK/IMU JSONL from
|
||||
``parse_rtk_imu_v2.py``. Raw ``.rscap`` files remain the traceability source;
|
||||
this script never modifies them.
|
||||
|
||||
Position rows may be NMEA ``GGA`` or G90 ``PVTSLNA`` (both expose ``lat_deg`` /
|
||||
``lon_deg`` / ``altitude_m``). Default time basis is LiDAR device time vs GNSS
|
||||
week/TOW; ``--time-basis host`` keeps the legacy host-receive nearest-neighbour
|
||||
association for old dlog datasets.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
@@ -18,6 +24,7 @@ import numpy as np
|
||||
|
||||
|
||||
GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000
|
||||
POSITION_TYPES = {"GGA", "PVTSLNA"}
|
||||
|
||||
|
||||
def parse_named_path(text: str) -> tuple[str, Path]:
|
||||
@@ -46,6 +53,12 @@ def parse_args() -> argparse.Namespace:
|
||||
parser.add_argument("--imu-before-ms", type=float, default=100.0)
|
||||
parser.add_argument("--imu-after-ms", type=float, default=100.0)
|
||||
parser.add_argument("--gps-utc-leap-seconds", type=int, default=18)
|
||||
parser.add_argument(
|
||||
"--time-basis",
|
||||
choices=("device_gnss", "host"),
|
||||
default="device_gnss",
|
||||
help="device_gnss: LiDAR unix_time_ns ↔ GNSS week/TOW; host: legacy host-receive association.",
|
||||
)
|
||||
parser.add_argument("--overwrite", action="store_true")
|
||||
return parser.parse_args()
|
||||
|
||||
@@ -83,7 +96,7 @@ def nearest_index(times: np.ndarray, target: int) -> int:
|
||||
|
||||
|
||||
def estimate_imu_times(rows: list[dict[str, Any]]) -> list[dict[str, Any]]:
|
||||
"""Recover 100 Hz timing inside each serial chunk from device timestamps.
|
||||
"""Recover timing inside each serial chunk from device timestamps.
|
||||
|
||||
A capture chunk has one host receive timestamp but may contain several IMU
|
||||
frames. The last frame is anchored to the chunk receive time and earlier
|
||||
@@ -119,6 +132,17 @@ def gnss_utc_ns(row: dict[str, Any], leap_seconds: int) -> int | None:
|
||||
return GPS_EPOCH_UNIX_NS + int(round(seconds * 1_000_000_000))
|
||||
|
||||
|
||||
def association_time_ns(row: dict[str, Any], time_basis: str, leap_seconds: int) -> int | None:
|
||||
if time_basis == "host":
|
||||
host = row.get("host_receive_utc_ns")
|
||||
return int(host) if host is not None else None
|
||||
device = gnss_utc_ns(row, leap_seconds)
|
||||
if device is not None:
|
||||
return device
|
||||
host = row.get("host_receive_utc_ns")
|
||||
return int(host) if host is not None else None
|
||||
|
||||
|
||||
def numeric_array(rows: list[dict[str, Any]], key: str, dtype: Any, default: Any) -> np.ndarray:
|
||||
return np.asarray([row.get(key, default) if row.get(key) is not None else default for row in rows], dtype=dtype)
|
||||
|
||||
@@ -168,34 +192,66 @@ def initialize_rtk_measurements(values: dict[str, np.ndarray]) -> None:
|
||||
values["rtk_heading_gnss_utc_ns"] = np.asarray([0], dtype=np.int64)
|
||||
values["rtk_heading_host_minus_gnss_ns"] = np.asarray([0], dtype=np.int64)
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
if args.out.exists() and any(args.out.iterdir()) and not args.overwrite:
|
||||
raise FileExistsError(f"{args.out} is non-empty; pass --overwrite")
|
||||
frames_out = args.out / "frames"
|
||||
|
||||
def build_combined(
|
||||
lidar_segments: list[tuple[str, Path]],
|
||||
rtk_paths: list[Path],
|
||||
imu_paths: list[Path],
|
||||
out: Path,
|
||||
*,
|
||||
rtk_max_dt_ms: float = 150.0,
|
||||
imu_before_ms: float = 100.0,
|
||||
imu_after_ms: float = 100.0,
|
||||
gps_utc_leap_seconds: int = 18,
|
||||
time_basis: str = "device_gnss",
|
||||
overwrite: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""Associate LiDAR frames with RTK/IMU and write ``out/`` combined package."""
|
||||
|
||||
if out.exists() and any(out.iterdir()) and not overwrite:
|
||||
raise FileExistsError(f"{out} is non-empty; pass overwrite=True")
|
||||
frames_out = out / "frames"
|
||||
frames_out.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
rtk_rows = load_jsonl(args.rtk)
|
||||
gga = sorted(
|
||||
[row for row in rtk_rows if row.get("type") == "GGA" and row.get("checksum_valid") and row.get("lat_deg") is not None],
|
||||
key=lambda row: int(row["host_receive_utc_ns"]),
|
||||
)
|
||||
heading = sorted(
|
||||
[row for row in rtk_rows if row.get("type") == "UNIHEADINGA" and row.get("checksum_valid") and row.get("heading_valid")],
|
||||
key=lambda row: int(row["host_receive_utc_ns"]),
|
||||
)
|
||||
imu = estimate_imu_times(load_jsonl(args.imu))
|
||||
gga_times = np.asarray([int(row["host_receive_utc_ns"]) for row in gga], dtype=np.int64)
|
||||
heading_times = np.asarray([int(row["host_receive_utc_ns"]) for row in heading], dtype=np.int64)
|
||||
rtk_rows = load_jsonl(rtk_paths)
|
||||
positions = []
|
||||
for row in rtk_rows:
|
||||
if row.get("type") not in POSITION_TYPES or not row.get("checksum_valid"):
|
||||
continue
|
||||
if row.get("lat_deg") is None or row.get("lon_deg") is None:
|
||||
continue
|
||||
assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds)
|
||||
if assoc is None:
|
||||
continue
|
||||
copied = dict(row)
|
||||
copied["_assoc_time_ns"] = assoc
|
||||
positions.append(copied)
|
||||
positions.sort(key=lambda row: int(row["_assoc_time_ns"]))
|
||||
|
||||
heading = []
|
||||
for row in rtk_rows:
|
||||
if row.get("type") != "UNIHEADINGA" or not row.get("checksum_valid") or not row.get("heading_valid"):
|
||||
continue
|
||||
assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds)
|
||||
if assoc is None:
|
||||
continue
|
||||
copied = dict(row)
|
||||
copied["_assoc_time_ns"] = assoc
|
||||
heading.append(copied)
|
||||
heading.sort(key=lambda row: int(row["_assoc_time_ns"]))
|
||||
|
||||
imu = estimate_imu_times(load_jsonl(imu_paths))
|
||||
position_times = np.asarray([int(row["_assoc_time_ns"]) for row in positions], dtype=np.int64)
|
||||
heading_times = np.asarray([int(row["_assoc_time_ns"]) for row in heading], dtype=np.int64)
|
||||
imu_times = np.asarray([int(row["estimated_time_ns"]) for row in imu], dtype=np.int64)
|
||||
|
||||
manifest: list[dict[str, Any]] = []
|
||||
global_index = 0
|
||||
max_rtk_ns = int(args.rtk_max_dt_ms * 1_000_000)
|
||||
before_ns = int(args.imu_before_ms * 1_000_000)
|
||||
after_ns = int(args.imu_after_ms * 1_000_000)
|
||||
max_rtk_ns = int(rtk_max_dt_ms * 1_000_000)
|
||||
before_ns = int(imu_before_ms * 1_000_000)
|
||||
after_ns = int(imu_after_ms * 1_000_000)
|
||||
|
||||
for segment_name, frame_dir in args.lidar:
|
||||
for segment_name, frame_dir in lidar_segments:
|
||||
frame_paths = sorted(frame_dir.glob("*.npz"))
|
||||
if not frame_paths:
|
||||
raise FileNotFoundError(f"no NPZ frames under {frame_dir}")
|
||||
@@ -204,28 +260,31 @@ def main() -> int:
|
||||
values = {key: np.asarray(frame[key]) for key in frame.files}
|
||||
lidar_time_ns = int(scalar(values["unix_time_ns"]))
|
||||
|
||||
gga_index = nearest_index(gga_times, lidar_time_ns)
|
||||
position_index = nearest_index(position_times, lidar_time_ns)
|
||||
heading_index = nearest_index(heading_times, lidar_time_ns)
|
||||
gga_row = gga[gga_index] if gga_index >= 0 else None
|
||||
position_row = positions[position_index] if position_index >= 0 else None
|
||||
heading_row = heading[heading_index] if heading_index >= 0 else None
|
||||
gga_dt = int(gga_times[gga_index]) - lidar_time_ns if gga_index >= 0 else None
|
||||
position_dt = int(position_times[position_index]) - lidar_time_ns if position_index >= 0 else None
|
||||
heading_dt = int(heading_times[heading_index]) - lidar_time_ns if heading_index >= 0 else None
|
||||
gga_ok = gga_row is not None and abs(gga_dt or 0) <= max_rtk_ns
|
||||
position_ok = position_row is not None and abs(position_dt or 0) <= max_rtk_ns
|
||||
heading_ok = heading_row is not None and abs(heading_dt or 0) <= max_rtk_ns
|
||||
add_rtk(values, "rtk_gga", gga_row if gga_ok else None, gga_dt)
|
||||
add_rtk(values, "rtk_gga", position_row if position_ok else None, position_dt)
|
||||
add_rtk(values, "rtk_heading", heading_row if heading_ok else None, heading_dt)
|
||||
initialize_rtk_measurements(values)
|
||||
|
||||
if gga_ok and gga_row:
|
||||
if position_ok and position_row:
|
||||
for key, dtype, default in (
|
||||
("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan),
|
||||
("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan),
|
||||
("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1),
|
||||
("differential_age_s", np.float64, np.nan),
|
||||
):
|
||||
values[f"rtk_{key}"] = np.asarray([gga_row.get(key, default)], dtype=dtype)
|
||||
values["rtk_gga_satellites"] = np.asarray([gga_row.get("satellites", -1)], dtype=np.int32)
|
||||
values["rtk_fixed"] = np.asarray([int(gga_row.get("fix_quality", -1)) in {4, 5}], dtype=np.uint8)
|
||||
values[f"rtk_{key}"] = np.asarray([position_row.get(key, default)], dtype=dtype)
|
||||
values["rtk_gga_satellites"] = np.asarray([position_row.get("satellites", -1)], dtype=np.int32)
|
||||
if position_row.get("gnss_week") is not None:
|
||||
values["rtk_gnss_week"] = np.asarray([position_row.get("gnss_week", -1)], dtype=np.int32)
|
||||
values["rtk_gnss_tow_ms"] = np.asarray([position_row.get("gnss_tow_ms", -1)], dtype=np.int64)
|
||||
values["rtk_fixed"] = np.asarray([int(position_row.get("fix_quality", -1)) in {4, 5}], dtype=np.uint8)
|
||||
if heading_ok and heading_row:
|
||||
for key, dtype, default in (
|
||||
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
|
||||
@@ -237,7 +296,7 @@ def main() -> int:
|
||||
values[f"rtk_{key}"] = np.asarray([heading_row.get(key, default)], dtype=dtype)
|
||||
values["rtk_heading_satellites"] = np.asarray([heading_row.get("satellites", -1)], dtype=np.int32)
|
||||
values["rtk_heading_solution_utf8"] = utf8_array(heading_row.get("heading_solution", ""))
|
||||
device_ns = gnss_utc_ns(heading_row, args.gps_utc_leap_seconds)
|
||||
device_ns = gnss_utc_ns(heading_row, gps_utc_leap_seconds)
|
||||
values["rtk_heading_gnss_utc_ns"] = np.asarray([device_ns or 0], dtype=np.int64)
|
||||
values["rtk_heading_host_minus_gnss_ns"] = np.asarray(
|
||||
[int(heading_row["host_receive_utc_ns"]) - device_ns if device_ns is not None else 0], dtype=np.int64
|
||||
@@ -263,7 +322,9 @@ def main() -> int:
|
||||
raw_matrix, raw_lengths = raw_frame_matrix(window)
|
||||
values["imu_raw_frame_bytes"] = raw_matrix
|
||||
values["imu_raw_frame_length"] = raw_lengths
|
||||
values["imu_source_files_json_utf8"] = utf8_array(json.dumps([str(path.resolve()) for path in args.imu], ensure_ascii=False))
|
||||
values["imu_source_files_json_utf8"] = utf8_array(
|
||||
json.dumps([str(path.resolve()) for path in imu_paths], ensure_ascii=False)
|
||||
)
|
||||
values["source_lidar_file_utf8"] = utf8_array(source.resolve())
|
||||
values["segment_name_utf8"] = utf8_array(segment_name)
|
||||
|
||||
@@ -273,37 +334,65 @@ def main() -> int:
|
||||
"global_index": global_index,
|
||||
"segment": segment_name,
|
||||
"segment_index": segment_index,
|
||||
"output": str(output.relative_to(args.out)),
|
||||
"output": str(output.relative_to(out)),
|
||||
"source_lidar": str(source.resolve()),
|
||||
"lidar_time_ns": lidar_time_ns,
|
||||
"rtk_gga_dt_ns": gga_dt,
|
||||
"rtk_gga_dt_ns": position_dt,
|
||||
"rtk_heading_dt_ns": heading_dt,
|
||||
"rtk_valid": gga_ok,
|
||||
"rtk_valid": position_ok,
|
||||
"heading_valid": heading_ok,
|
||||
"rtk_fix_quality": gga_row.get("fix_quality") if gga_ok and gga_row else None,
|
||||
"rtk_fixed": bool(gga_ok and gga_row and int(gga_row.get("fix_quality", -1)) in {4, 5}),
|
||||
"rtk_fix_quality": position_row.get("fix_quality") if position_ok and position_row else None,
|
||||
"rtk_fixed": bool(position_ok and position_row and int(position_row.get("fix_quality", -1)) in {4, 5}),
|
||||
"imu_window_count": len(window),
|
||||
})
|
||||
global_index += 1
|
||||
|
||||
fields = sorted({key for row in manifest for key in row})
|
||||
with (args.out / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
|
||||
with (out / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=fields)
|
||||
writer.writeheader()
|
||||
writer.writerows(manifest)
|
||||
if time_basis == "device_gnss":
|
||||
time_basis_text = (
|
||||
"LiDAR MSOP/device unix_time_ns ↔ RTK GNSS week/TOW (fallback host receive); "
|
||||
"IMU still windowed on host-anchored device deltas"
|
||||
)
|
||||
else:
|
||||
time_basis_text = (
|
||||
"LiDAR and serial host UTC; RTK GNSS time and IMU device time are retained for clock-model refinement"
|
||||
)
|
||||
summary = {
|
||||
"frames": len(manifest),
|
||||
"segments": {name: sum(row["segment"] == name for row in manifest) for name, _ in args.lidar},
|
||||
"segments": {name: sum(row["segment"] == name for row in manifest) for name, _ in lidar_segments},
|
||||
"rtk_valid": sum(bool(row["rtk_valid"]) for row in manifest),
|
||||
"heading_valid": sum(bool(row["heading_valid"]) for row in manifest),
|
||||
"rtk_fixed": sum(bool(row["rtk_fixed"]) for row in manifest),
|
||||
"imu_window_nonempty": sum(int(row["imu_window_count"]) > 0 for row in manifest),
|
||||
"rtk_max_dt_ms": args.rtk_max_dt_ms,
|
||||
"imu_window_ms": [-args.imu_before_ms, args.imu_after_ms],
|
||||
"time_basis": "LiDAR and serial host UTC; RTK GNSS time and IMU device time are retained for clock-model refinement",
|
||||
"rtk_max_dt_ms": rtk_max_dt_ms,
|
||||
"imu_window_ms": [-imu_before_ms, imu_after_ms],
|
||||
"time_basis": time_basis_text,
|
||||
"time_basis_mode": time_basis,
|
||||
"position_message_types": sorted(POSITION_TYPES),
|
||||
"imu_orientation_warning": "IMU values are in the raw IMU sensor frame; no LiDAR/body extrinsic is applied",
|
||||
}
|
||||
(args.out / "dataset_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
(out / "dataset_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
return summary
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
summary = build_combined(
|
||||
args.lidar,
|
||||
args.rtk,
|
||||
args.imu,
|
||||
args.out,
|
||||
rtk_max_dt_ms=args.rtk_max_dt_ms,
|
||||
imu_before_ms=args.imu_before_ms,
|
||||
imu_after_ms=args.imu_after_ms,
|
||||
gps_utc_leap_seconds=args.gps_utc_leap_seconds,
|
||||
time_basis=args.time_basis,
|
||||
overwrite=args.overwrite,
|
||||
)
|
||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
@@ -0,0 +1,172 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Export one static-station H32 V2 .rscap into LiDAR frame NPZs.
|
||||
|
||||
This is an **internal** helper used by ``export_raw_to_combined.py``.
|
||||
For RTK–LiDAR calibration, prefer the one-shot exporter that writes ``combined/``.
|
||||
|
||||
Output frame contract (consumed by ``build_multisensor_npz.py``):
|
||||
|
||||
- ``points_raw``: (N, 5) polar ``d_mm, azimuth_deg, altitude_deg, intensity, progression``
|
||||
- ``unix_time_ns``: H32 MSOP device timestamp (seconds+us → ns)
|
||||
- ``frame_counter``, ``point_count``, optional host receive stamp
|
||||
|
||||
Raw ``.rscap`` files are never modified.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parent
|
||||
sys.path.insert(0, str(ROOT / "rscap_v2"))
|
||||
|
||||
from capture_format_v2 import file_summary, read_capture # noqa: E402
|
||||
from h32_msop import iter_h32_frames_polar # noqa: E402
|
||||
|
||||
|
||||
def resolve_lidar_rscap(station_dir: Path, capture_name: str = "h32.rscap") -> Path:
|
||||
candidates = [
|
||||
station_dir / capture_name,
|
||||
station_dir / "h32.rscap",
|
||||
station_dir / "lidar.rscap",
|
||||
]
|
||||
for path in candidates:
|
||||
if path.is_file():
|
||||
return path
|
||||
raise FileNotFoundError(
|
||||
f"no LiDAR .rscap under {station_dir}; tried {[str(p.name) for p in candidates]}"
|
||||
)
|
||||
|
||||
|
||||
def export_station_h32(
|
||||
station: Path,
|
||||
out: Path,
|
||||
*,
|
||||
capture_name: str = "h32.rscap",
|
||||
stride: int = 1,
|
||||
min_frame_points: int = 100,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
compress: bool = True,
|
||||
write_reports: bool = False,
|
||||
resume: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""Decode one station H32 capture into ``out/frames/*.npz``. Returns metadata."""
|
||||
|
||||
rscap = station if station.is_file() and station.suffix.lower() == ".rscap" else resolve_lidar_rscap(station, capture_name)
|
||||
frames_dir = out / "frames"
|
||||
frames_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
capture = read_capture(rscap)
|
||||
frames = iter_h32_frames_polar(
|
||||
capture,
|
||||
min_frame_points=min_frame_points,
|
||||
frame_stride=max(1, stride),
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
)
|
||||
if not frames:
|
||||
raise RuntimeError(f"no H32 frames decoded from {rscap}")
|
||||
|
||||
saver = np.savez_compressed if compress else np.savez
|
||||
manifest_rows: list[dict[str, Any]] = []
|
||||
written = 0
|
||||
for index, frame in enumerate(frames):
|
||||
unix_time_ns = int(round(frame.t_start_s * 1_000_000_000))
|
||||
name = f"h32_{index:06d}_{unix_time_ns}_frame{index}.npz"
|
||||
destination = frames_dir / name
|
||||
if resume and destination.exists():
|
||||
continue
|
||||
points = np.asarray(frame.points_raw, dtype=np.float32)
|
||||
payload = {
|
||||
"points_raw": points,
|
||||
"frame_counter": np.asarray([index], dtype=np.int32),
|
||||
"point_count": np.asarray([points.shape[0]], dtype=np.int32),
|
||||
"unix_time_ns": np.asarray([unix_time_ns], dtype=np.int64),
|
||||
"device_time_s": np.asarray([frame.t_start_s], dtype=np.float64),
|
||||
"device_time_end_s": np.asarray([frame.t_end_s], dtype=np.float64),
|
||||
"host_receive_utc_ns": np.asarray([frame.host_receive_utc_ns], dtype=np.int64),
|
||||
"source_file_utf8": np.frombuffer(str(rscap.resolve()).encode("utf-8"), dtype=np.uint8),
|
||||
}
|
||||
saver(destination, **payload)
|
||||
written += 1
|
||||
manifest_rows.append(
|
||||
{
|
||||
"index": index,
|
||||
"output": name,
|
||||
"unix_time_ns": unix_time_ns,
|
||||
"point_count": int(points.shape[0]),
|
||||
"host_receive_utc_ns": int(frame.host_receive_utc_ns),
|
||||
}
|
||||
)
|
||||
|
||||
metadata: dict[str, Any] = {
|
||||
"source_rscap": str(rscap.resolve()),
|
||||
"capture": file_summary(capture),
|
||||
"frames_decoded": len(frames),
|
||||
"frames_written": written,
|
||||
"frames_dir": str(frames_dir.resolve()),
|
||||
"time_basis": "H32 MSOP device timestamp (packet seconds+microseconds)",
|
||||
"points_raw_columns": ["d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"],
|
||||
}
|
||||
(out / "metadata.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
(out / "README.md").write_text(
|
||||
"# H32 station export (internal)\n\n"
|
||||
f"- source: `{rscap}`\n"
|
||||
f"- frames: `{frames_dir}`\n"
|
||||
"- Prefer ``tools/export_raw_to_combined.py`` for the full RTK–LiDAR package.\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
if write_reports:
|
||||
reports = out / "reports"
|
||||
reports.mkdir(parents=True, exist_ok=True)
|
||||
with (reports / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=list(manifest_rows[0].keys()) if manifest_rows else ["index"])
|
||||
writer.writeheader()
|
||||
writer.writerows(manifest_rows)
|
||||
(reports / "export_summary.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
return metadata
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--station", type=Path, required=True, help="Station directory or .rscap file")
|
||||
parser.add_argument("--out", type=Path, required=True)
|
||||
parser.add_argument("--capture-name", default="h32.rscap")
|
||||
parser.add_argument("--stride", type=int, default=1)
|
||||
parser.add_argument("--min-frame-points", type=int, default=100)
|
||||
parser.add_argument("--min-range-m", type=float, default=0.3)
|
||||
parser.add_argument("--max-range-m", type=float, default=120.0)
|
||||
parser.add_argument("--compress", action="store_true", default=True)
|
||||
parser.add_argument("--write-reports", action="store_true")
|
||||
parser.add_argument("--resume", action="store_true", help="Skip frames that already exist")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
metadata = export_station_h32(
|
||||
args.station,
|
||||
args.out,
|
||||
capture_name=args.capture_name,
|
||||
stride=args.stride,
|
||||
min_frame_points=args.min_frame_points,
|
||||
min_range_m=args.min_range_m,
|
||||
max_range_m=args.max_range_m,
|
||||
compress=args.compress,
|
||||
write_reports=args.write_reports,
|
||||
resume=args.resume,
|
||||
)
|
||||
print(json.dumps(metadata, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,322 @@
|
||||
#!/usr/bin/env python3
|
||||
"""One-shot export: raw H32/G90/N300 captures → RTK–LiDAR ``combined/`` package.
|
||||
|
||||
Analogous to Lidar-IMU ``tools/export_rscap_to_v1.py``: raw ``.rscap`` in,
|
||||
calibration-ready intermediate out. Downstream prepare/solve consume ``combined/``
|
||||
only (``manifest.csv`` + associated frame NPZs).
|
||||
|
||||
Expected raw layout:
|
||||
|
||||
stations/
|
||||
001/h32.rscap
|
||||
002/h32.rscap
|
||||
...
|
||||
captures/ (paths passed explicitly)
|
||||
rtk.rscap # G90: #PVTSLNA + #UNIHEADINGA
|
||||
imu.rscap # N300 (associated only; not used in AX=XB)
|
||||
|
||||
Output under ``--out``:
|
||||
|
||||
export/<station>/frames/*.npz # internal LiDAR frames
|
||||
parsed/rtk.jsonl, imu.jsonl
|
||||
combined/frames/*.npz + manifest.csv + dataset_summary.json
|
||||
export_summary.json
|
||||
|
||||
Legacy dlog stations (``dobject`` + ``dobject_recording``) are still accepted;
|
||||
use ``--time-basis host`` for those datasets.
|
||||
|
||||
Raw ``.rscap`` / dlog files are never modified.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import shutil
|
||||
import subprocess
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
ROOT = Path(__file__).resolve().parent
|
||||
REPO = ROOT.parent
|
||||
sys.path.insert(0, str(ROOT))
|
||||
sys.path.insert(0, str(ROOT / "rscap_v2"))
|
||||
|
||||
from build_multisensor_npz import build_combined # noqa: E402
|
||||
from capture_format_v2 import file_summary, read_capture # noqa: E402
|
||||
from export_h32_rscap_station import export_station_h32, resolve_lidar_rscap # noqa: E402
|
||||
from pipeline_common_corrected import ( # noqa: E402
|
||||
parse_imu_capture,
|
||||
parse_rtk_capture,
|
||||
write_json,
|
||||
write_jsonl,
|
||||
)
|
||||
|
||||
|
||||
def is_h32_station(station: Path, capture_name: str) -> bool:
|
||||
try:
|
||||
resolve_lidar_rscap(station, capture_name)
|
||||
return True
|
||||
except FileNotFoundError:
|
||||
return False
|
||||
|
||||
|
||||
def is_dlog_station(station: Path) -> bool:
|
||||
return (station / "dobject").is_dir() and (station / "dobject_recording").is_dir()
|
||||
|
||||
|
||||
def discover_stations(stations_root: Path, names: list[str], capture_name: str) -> list[Path]:
|
||||
if names:
|
||||
stations = [stations_root / name for name in names]
|
||||
missing = [str(path) for path in stations if not path.is_dir()]
|
||||
if missing:
|
||||
raise FileNotFoundError(f"station directories missing: {missing}")
|
||||
return stations
|
||||
stations = sorted(
|
||||
[
|
||||
path
|
||||
for path in stations_root.iterdir()
|
||||
if path.is_dir() and (is_h32_station(path, capture_name) or is_dlog_station(path))
|
||||
],
|
||||
key=lambda path: path.name,
|
||||
)
|
||||
if not stations:
|
||||
raise FileNotFoundError(
|
||||
f"no station with {capture_name}/lidar.rscap or dobject+dobject_recording under {stations_root}"
|
||||
)
|
||||
return stations
|
||||
|
||||
|
||||
def export_legacy_dlog_station(
|
||||
station: Path,
|
||||
out: Path,
|
||||
*,
|
||||
lidar_object: str,
|
||||
timezone: str,
|
||||
stride: int,
|
||||
) -> None:
|
||||
exporter = ROOT / "frontlidar_dlog_export.py"
|
||||
command = [
|
||||
sys.executable,
|
||||
str(exporter),
|
||||
"--dlog",
|
||||
str(station),
|
||||
"--out",
|
||||
str(out),
|
||||
"--object",
|
||||
lidar_object,
|
||||
"--format",
|
||||
"npz",
|
||||
"--timezone",
|
||||
timezone,
|
||||
"--stride",
|
||||
str(stride),
|
||||
"--compress",
|
||||
"--skip-rtk",
|
||||
"--write-reports",
|
||||
"--resume",
|
||||
]
|
||||
completed = subprocess.run(command, check=False)
|
||||
if completed.returncode != 0:
|
||||
raise RuntimeError(f"legacy dlog export failed for {station} (exit {completed.returncode})")
|
||||
|
||||
|
||||
def parse_serial(rtk_rscap: Path, imu_rscap: Path, parsed_root: Path) -> dict[str, Any]:
|
||||
parsed_root.mkdir(parents=True, exist_ok=True)
|
||||
rtk_capture = read_capture(rtk_rscap)
|
||||
imu_capture = read_capture(imu_rscap)
|
||||
rtk_rows = parse_rtk_capture(rtk_capture)
|
||||
imu_rows = parse_imu_capture(imu_capture)
|
||||
write_jsonl(parsed_root / "rtk.jsonl", rtk_rows)
|
||||
write_jsonl(parsed_root / "imu.jsonl", imu_rows)
|
||||
summary = {
|
||||
"rtk_capture": file_summary(rtk_capture),
|
||||
"imu_capture": file_summary(imu_capture),
|
||||
"rtk_records": len(rtk_rows),
|
||||
"rtk_checksum_valid": sum(bool(row.get("checksum_valid")) for row in rtk_rows),
|
||||
"rtk_pvtslna": sum(row.get("type") == "PVTSLNA" and row.get("checksum_valid") for row in rtk_rows),
|
||||
"rtk_gga": sum(row.get("type") == "GGA" and row.get("checksum_valid") for row in rtk_rows),
|
||||
"rtk_heading_valid": sum(row.get("type") == "UNIHEADINGA" and row.get("heading_valid") for row in rtk_rows),
|
||||
"imu_frames": len(imu_rows),
|
||||
"imu_crc_valid": sum(bool(row.get("crc_valid")) for row in imu_rows),
|
||||
"imu_types": sorted({str(row.get("type")) for row in imu_rows}),
|
||||
}
|
||||
write_json(parsed_root / "parse_summary.json", summary)
|
||||
return summary
|
||||
|
||||
|
||||
def export_raw_to_combined(
|
||||
*,
|
||||
stations_root: Path,
|
||||
rtk_rscap: Path,
|
||||
imu_rscap: Path,
|
||||
out: Path,
|
||||
station_names: list[str] | None = None,
|
||||
lidar_capture_name: str = "h32.rscap",
|
||||
lidar_object: str = "frontlidar",
|
||||
timezone: str = "+08:00",
|
||||
stride: int = 1,
|
||||
rtk_max_dt_ms: float = 150.0,
|
||||
imu_before_ms: float = 100.0,
|
||||
imu_after_ms: float = 100.0,
|
||||
time_basis: str = "device_gnss",
|
||||
overwrite: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""Full raw → combined export. Returns ``export_summary`` dict."""
|
||||
|
||||
if not stations_root.is_dir():
|
||||
raise FileNotFoundError(f"stations root does not exist: {stations_root}")
|
||||
if not rtk_rscap.is_file():
|
||||
raise FileNotFoundError(f"RTK capture missing: {rtk_rscap}")
|
||||
if not imu_rscap.is_file():
|
||||
raise FileNotFoundError(f"IMU capture missing: {imu_rscap}")
|
||||
if out.exists() and any(out.iterdir()) and not overwrite:
|
||||
raise FileExistsError(f"{out} is non-empty; pass --overwrite")
|
||||
if overwrite and out.exists():
|
||||
# Keep out root but clear known children so rebuild is deterministic.
|
||||
for child in ("export", "parsed", "combined", "export_summary.json", "capture_audit.json"):
|
||||
target = out / child
|
||||
if target.is_dir():
|
||||
shutil.rmtree(target)
|
||||
elif target.is_file():
|
||||
target.unlink()
|
||||
|
||||
out.mkdir(parents=True, exist_ok=True)
|
||||
export_root = out / "export"
|
||||
parsed_root = out / "parsed"
|
||||
combined_root = out / "combined"
|
||||
|
||||
stations = discover_stations(stations_root, station_names or [], lidar_capture_name)
|
||||
parse_summary = parse_serial(rtk_rscap, imu_rscap, parsed_root)
|
||||
|
||||
station_meta: list[dict[str, Any]] = []
|
||||
lidar_segments: list[tuple[str, Path]] = []
|
||||
saw_dlog = False
|
||||
for station in stations:
|
||||
station_out = export_root / station.name
|
||||
if is_h32_station(station, lidar_capture_name):
|
||||
meta = export_station_h32(
|
||||
station,
|
||||
station_out,
|
||||
capture_name=lidar_capture_name,
|
||||
stride=stride,
|
||||
write_reports=True,
|
||||
resume=False,
|
||||
)
|
||||
kind = "h32_rscap"
|
||||
elif is_dlog_station(station):
|
||||
saw_dlog = True
|
||||
export_legacy_dlog_station(
|
||||
station,
|
||||
station_out,
|
||||
lidar_object=lidar_object,
|
||||
timezone=timezone,
|
||||
stride=stride,
|
||||
)
|
||||
meta = {"source": str(station.resolve()), "kind": "legacy_dlog"}
|
||||
kind = "legacy_dlog"
|
||||
else:
|
||||
raise RuntimeError(f"station {station.name} has neither H32 .rscap nor dlog layout")
|
||||
frames_dir = station_out / "frames"
|
||||
if not frames_dir.is_dir() or not any(frames_dir.glob("*.npz")):
|
||||
raise RuntimeError(f"no exported frames for station {station.name}: {frames_dir}")
|
||||
lidar_segments.append((station.name, frames_dir))
|
||||
station_meta.append({"station": station.name, "kind": kind, "frames_dir": str(frames_dir), **meta})
|
||||
|
||||
if saw_dlog and time_basis == "device_gnss":
|
||||
print(
|
||||
"[warn] legacy dlog stations use host/DObject time; prefer --time-basis host",
|
||||
file=sys.stderr,
|
||||
)
|
||||
|
||||
combined_summary = build_combined(
|
||||
lidar_segments,
|
||||
[parsed_root / "rtk.jsonl"],
|
||||
[parsed_root / "imu.jsonl"],
|
||||
combined_root,
|
||||
rtk_max_dt_ms=rtk_max_dt_ms,
|
||||
imu_before_ms=imu_before_ms,
|
||||
imu_after_ms=imu_after_ms,
|
||||
time_basis=time_basis,
|
||||
overwrite=True,
|
||||
)
|
||||
|
||||
summary = {
|
||||
"role": "RTK-LiDAR one-shot raw export (like Lidar-IMU export_rscap_to_v1)",
|
||||
"stations_root": str(stations_root.resolve()),
|
||||
"rtk_rscap": str(rtk_rscap.resolve()),
|
||||
"imu_rscap": str(imu_rscap.resolve()),
|
||||
"out": str(out.resolve()),
|
||||
"station_count": len(stations),
|
||||
"stations": station_meta,
|
||||
"parsed": parse_summary,
|
||||
"combined": combined_summary,
|
||||
"outputs": {
|
||||
"combined": str(combined_root.resolve()),
|
||||
"manifest": str((combined_root / "manifest.csv").resolve()),
|
||||
"parsed": str(parsed_root.resolve()),
|
||||
"export": str(export_root.resolve()),
|
||||
},
|
||||
"timestamp_policy": {
|
||||
"default_time_basis": time_basis,
|
||||
"lidar_h32": "MSOP device timestamp → unix_time_ns",
|
||||
"rtk": "GNSS week/TOW when time_basis=device_gnss; else host_receive_utc_ns",
|
||||
"imu": "associated only; host-anchored device deltas in combined window",
|
||||
"host_utc": "kept for audit; not the default calibration timeline for new captures",
|
||||
},
|
||||
"next_step": "run/run_direct_rtk_lidar.ps1 -CombinedRoot <out>/combined ...",
|
||||
}
|
||||
(out / "export_summary.json").write_text(
|
||||
json.dumps(summary, ensure_ascii=False, indent=2) + "\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
return summary
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
|
||||
parser.add_argument("--stations-root", type=Path, required=True, help="Directory of per-station folders")
|
||||
parser.add_argument("--rtk-rscap", type=Path, required=True, help="Continuous G90/RTK V2 .rscap")
|
||||
parser.add_argument("--imu-rscap", type=Path, required=True, help="Continuous N300/IMU V2 .rscap")
|
||||
parser.add_argument("--out", type=Path, required=True, help="Output package root (contains combined/)")
|
||||
parser.add_argument("--station", action="append", default=[], help="Optional station name filter; repeatable")
|
||||
parser.add_argument("--lidar-capture-name", default="h32.rscap")
|
||||
parser.add_argument("--lidar-object", default="frontlidar", help="Legacy dlog DObject name")
|
||||
parser.add_argument("--timezone", default="+08:00", help="Legacy dlog tick timezone")
|
||||
parser.add_argument("--stride", type=int, default=1)
|
||||
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
|
||||
parser.add_argument("--imu-before-ms", type=float, default=100.0)
|
||||
parser.add_argument("--imu-after-ms", type=float, default=100.0)
|
||||
parser.add_argument("--time-basis", choices=("device_gnss", "host"), default="device_gnss")
|
||||
parser.add_argument("--overwrite", action="store_true")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
if args.stride < 1:
|
||||
raise SystemExit("stride must be >= 1")
|
||||
summary = export_raw_to_combined(
|
||||
stations_root=args.stations_root,
|
||||
rtk_rscap=args.rtk_rscap,
|
||||
imu_rscap=args.imu_rscap,
|
||||
out=args.out,
|
||||
station_names=args.station,
|
||||
lidar_capture_name=args.lidar_capture_name,
|
||||
lidar_object=args.lidar_object,
|
||||
timezone=args.timezone,
|
||||
stride=args.stride,
|
||||
rtk_max_dt_ms=args.rtk_max_dt_ms,
|
||||
imu_before_ms=args.imu_before_ms,
|
||||
imu_after_ms=args.imu_after_ms,
|
||||
time_basis=args.time_basis,
|
||||
overwrite=args.overwrite,
|
||||
)
|
||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
||||
print(f"\nCombined package ready: {summary['outputs']['combined']}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,371 @@
|
||||
"""Decode RoboSense H32 MSOP V2 .rscap into Cartesian frames (metres).
|
||||
|
||||
Angle / distance conventions follow ``RSLidarH32_3D_RawCaptureNet48``:
|
||||
azimuth = normalize(-(block_az + horizontal[ch])), altitude = vertical[ch],
|
||||
distance_mm = raw * distance_unit_mm, then:
|
||||
|
||||
x = d_m * cos(alt) * cos(az)
|
||||
y = d_m * cos(alt) * sin(az)
|
||||
z = d_m * sin(alt)
|
||||
|
||||
MSOP-only captures do not include DIFOP; vertical angles default to a uniform
|
||||
-16°…+16° fan, horizontal channel offsets default to 0.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from capture_format_v2 import CaptureFile
|
||||
|
||||
PACKET_LENGTH = 1248
|
||||
DATA_START = 42
|
||||
BLOCKS = 12
|
||||
BLOCK_LENGTH = 100
|
||||
CHANNELS = 32
|
||||
MIN_FRAME_POINTS_DEFAULT = 100
|
||||
DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
|
||||
|
||||
|
||||
def ticks_to_unix_ns(ticks: int) -> int:
|
||||
return (ticks - DOTNET_UNIX_EPOCH_TICKS) * 100
|
||||
|
||||
|
||||
def default_vertical_deg() -> np.ndarray:
|
||||
return -16.0 + np.arange(CHANNELS, dtype=np.float64) * (32.0 / (CHANNELS - 1))
|
||||
|
||||
|
||||
def default_horizontal_deg() -> np.ndarray:
|
||||
return np.zeros(CHANNELS, dtype=np.float64)
|
||||
|
||||
|
||||
def read_u16_be(packet: bytes, index: int) -> int:
|
||||
return (packet[index] << 8) | packet[index + 1]
|
||||
|
||||
|
||||
def device_timestamp_ms(packet: bytes) -> int:
|
||||
seconds = int.from_bytes(packet[20:26], "big")
|
||||
microseconds = int.from_bytes(packet[26:30], "big")
|
||||
return seconds * 1000 + microseconds // 1000
|
||||
|
||||
|
||||
def distance_unit_mm(packet: bytes, *, auto: bool = True, fallback: float = 2.5) -> float:
|
||||
if not auto:
|
||||
return float(fallback)
|
||||
return 2.5 if packet[17] == 1 else 0.5
|
||||
|
||||
|
||||
def normalize_azimuth_deg(angle: float) -> float:
|
||||
while angle > 180.0:
|
||||
angle -= 360.0
|
||||
while angle < -180.0:
|
||||
angle += 360.0
|
||||
return angle
|
||||
|
||||
|
||||
@dataclass
|
||||
class LidarFrameExport:
|
||||
t_start_s: float
|
||||
t_end_s: float
|
||||
points_xyz: np.ndarray # (N, 3) metres
|
||||
|
||||
|
||||
@dataclass
|
||||
class LidarFramePolarExport:
|
||||
"""One H32 frame in the calibration ``points_raw`` polar contract.
|
||||
|
||||
Columns: ``d_mm, azimuth_deg, altitude_deg, intensity, progression``.
|
||||
Azimuth already includes the H32 channel horizontal offset and sign flip so
|
||||
``rigorous_calibration.load_npz_xyz`` reproduces the same Cartesian points.
|
||||
"""
|
||||
|
||||
t_start_s: float
|
||||
t_end_s: float
|
||||
points_raw: np.ndarray # (N, 5) float32
|
||||
host_receive_utc_ns: int
|
||||
|
||||
|
||||
def decode_packet_points(
|
||||
packet: bytes,
|
||||
vertical_deg: np.ndarray,
|
||||
horizontal_deg: np.ndarray,
|
||||
*,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
) -> tuple[list[float], np.ndarray]:
|
||||
"""Decode one MSOP packet into block azimuths and concatenated XYZ points."""
|
||||
|
||||
if len(packet) != PACKET_LENGTH:
|
||||
return [], np.zeros((0, 3), dtype=np.float64)
|
||||
unit = distance_unit_mm(packet)
|
||||
az_list: list[float] = []
|
||||
chunks: list[np.ndarray] = []
|
||||
idx = DATA_START
|
||||
for _block in range(BLOCKS):
|
||||
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
|
||||
break
|
||||
az = read_u16_be(packet, idx + 2) * 0.01
|
||||
az_list.append(az)
|
||||
pts = _block_points(
|
||||
packet,
|
||||
idx,
|
||||
az,
|
||||
unit,
|
||||
vertical_deg,
|
||||
horizontal_deg,
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
)
|
||||
if pts.shape[0]:
|
||||
chunks.append(pts)
|
||||
idx += BLOCK_LENGTH
|
||||
if not chunks:
|
||||
return az_list, np.zeros((0, 3), dtype=np.float64)
|
||||
return az_list, np.vstack(chunks)
|
||||
|
||||
|
||||
def _block_points(
|
||||
packet: bytes,
|
||||
block_offset: int,
|
||||
az_deg: float,
|
||||
unit_mm: float,
|
||||
vertical_deg: np.ndarray,
|
||||
horizontal_deg: np.ndarray,
|
||||
*,
|
||||
min_range_m: float,
|
||||
max_range_m: float,
|
||||
) -> np.ndarray:
|
||||
xs: list[float] = []
|
||||
ys: list[float] = []
|
||||
zs: list[float] = []
|
||||
idx = block_offset + 4 # after FF EE + azimuth
|
||||
for ch in range(CHANNELS):
|
||||
raw = read_u16_be(packet, idx)
|
||||
idx += 3
|
||||
if raw == 0:
|
||||
continue
|
||||
d_m = (raw * unit_mm) * 0.001
|
||||
if d_m < min_range_m or d_m > max_range_m:
|
||||
continue
|
||||
az_ch = np.deg2rad(normalize_azimuth_deg(-(az_deg + float(horizontal_deg[ch]))))
|
||||
alt = np.deg2rad(float(vertical_deg[ch]))
|
||||
cos_alt = np.cos(alt)
|
||||
xs.append(d_m * cos_alt * np.cos(az_ch))
|
||||
ys.append(d_m * cos_alt * np.sin(az_ch))
|
||||
zs.append(d_m * np.sin(alt))
|
||||
if not xs:
|
||||
return np.zeros((0, 3), dtype=np.float64)
|
||||
return np.column_stack([xs, ys, zs]).astype(np.float64, copy=False)
|
||||
|
||||
|
||||
def _block_points_raw(
|
||||
packet: bytes,
|
||||
block_offset: int,
|
||||
az_deg: float,
|
||||
unit_mm: float,
|
||||
vertical_deg: np.ndarray,
|
||||
horizontal_deg: np.ndarray,
|
||||
*,
|
||||
min_range_m: float,
|
||||
max_range_m: float,
|
||||
) -> np.ndarray:
|
||||
"""Return polar ``points_raw`` rows compatible with ``load_npz_xyz``."""
|
||||
|
||||
rows: list[list[float]] = []
|
||||
idx = block_offset + 4
|
||||
for ch in range(CHANNELS):
|
||||
raw = read_u16_be(packet, idx)
|
||||
intensity = float(packet[idx + 2])
|
||||
idx += 3
|
||||
if raw == 0:
|
||||
continue
|
||||
d_mm = float(raw) * unit_mm
|
||||
d_m = d_mm * 0.001
|
||||
if d_m < min_range_m or d_m > max_range_m:
|
||||
continue
|
||||
az_ch = normalize_azimuth_deg(-(az_deg + float(horizontal_deg[ch])))
|
||||
rows.append([d_mm, az_ch, float(vertical_deg[ch]), intensity, float(ch)])
|
||||
if not rows:
|
||||
return np.zeros((0, 5), dtype=np.float32)
|
||||
return np.asarray(rows, dtype=np.float32)
|
||||
|
||||
|
||||
def iter_h32_frames_polar(
|
||||
capture: CaptureFile,
|
||||
*,
|
||||
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
||||
frame_stride: int = 1,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
max_points_per_frame: int | None = None,
|
||||
vertical_deg: np.ndarray | None = None,
|
||||
horizontal_deg: np.ndarray | None = None,
|
||||
) -> list[LidarFramePolarExport]:
|
||||
"""Assemble MSOP packets into polar frames for the RTK–LiDAR combined contract."""
|
||||
|
||||
vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64)
|
||||
horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64)
|
||||
if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,):
|
||||
raise ValueError(f"vertical/horizontal must have shape ({CHANNELS},)")
|
||||
|
||||
frames: list[LidarFramePolarExport] = []
|
||||
point_chunks: list[np.ndarray] = []
|
||||
t_start: float | None = None
|
||||
t_end: float | None = None
|
||||
host_ns = 0
|
||||
prev_az: float | None = None
|
||||
kept = 0
|
||||
stride = max(1, int(frame_stride))
|
||||
|
||||
def emit() -> None:
|
||||
nonlocal point_chunks, t_start, t_end, host_ns, kept
|
||||
if not point_chunks or t_start is None or t_end is None:
|
||||
point_chunks = []
|
||||
t_start = t_end = None
|
||||
return
|
||||
points = np.vstack(point_chunks)
|
||||
point_chunks = []
|
||||
start_s, end_s = t_start, t_end
|
||||
frame_host = host_ns
|
||||
t_start = t_end = None
|
||||
if points.shape[0] < min_frame_points:
|
||||
return
|
||||
if kept % stride != 0:
|
||||
kept += 1
|
||||
return
|
||||
kept += 1
|
||||
if max_points_per_frame is not None and points.shape[0] > max_points_per_frame:
|
||||
select = np.linspace(0, points.shape[0] - 1, max_points_per_frame, dtype=int)
|
||||
points = points[select]
|
||||
if end_s <= start_s:
|
||||
end_s = start_s + 0.1
|
||||
frames.append(
|
||||
LidarFramePolarExport(
|
||||
t_start_s=start_s,
|
||||
t_end_s=end_s,
|
||||
points_raw=points.astype(np.float32, copy=False),
|
||||
host_receive_utc_ns=int(frame_host),
|
||||
)
|
||||
)
|
||||
|
||||
for chunk in capture.chunks:
|
||||
packet = chunk.raw
|
||||
if len(packet) != PACKET_LENGTH:
|
||||
continue
|
||||
packet_t = device_timestamp_ms(packet) * 1e-3
|
||||
unit = distance_unit_mm(packet)
|
||||
chunk_host = ticks_to_unix_ns(chunk.receive_utc_ticks)
|
||||
idx = DATA_START
|
||||
for _block in range(BLOCKS):
|
||||
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
|
||||
break
|
||||
az = read_u16_be(packet, idx + 2) * 0.01
|
||||
if prev_az is not None and prev_az > 270.0 and az < 90.0:
|
||||
emit()
|
||||
prev_az = az
|
||||
pts = _block_points_raw(
|
||||
packet,
|
||||
idx,
|
||||
az,
|
||||
unit,
|
||||
vertical,
|
||||
horizontal,
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
)
|
||||
if pts.shape[0]:
|
||||
if t_start is None:
|
||||
t_start = packet_t
|
||||
t_end = packet_t
|
||||
host_ns = chunk_host
|
||||
point_chunks.append(pts)
|
||||
idx += BLOCK_LENGTH
|
||||
|
||||
emit()
|
||||
return frames
|
||||
|
||||
|
||||
def iter_h32_frames(
|
||||
capture: CaptureFile,
|
||||
*,
|
||||
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
||||
frame_stride: int = 1,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
max_points_per_frame: int | None = None,
|
||||
vertical_deg: np.ndarray | None = None,
|
||||
horizontal_deg: np.ndarray | None = None,
|
||||
) -> list[LidarFrameExport]:
|
||||
"""Assemble MSOP packets into frames using the 270°→90° azimuth wrap."""
|
||||
|
||||
vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64)
|
||||
horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64)
|
||||
if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,):
|
||||
raise ValueError(f"vertical/horizontal must have shape ({CHANNELS},)")
|
||||
|
||||
frames: list[LidarFrameExport] = []
|
||||
point_chunks: list[np.ndarray] = []
|
||||
t_start: float | None = None
|
||||
t_end: float | None = None
|
||||
prev_az: float | None = None
|
||||
kept = 0
|
||||
stride = max(1, int(frame_stride))
|
||||
|
||||
def emit() -> None:
|
||||
nonlocal point_chunks, t_start, t_end, kept
|
||||
if not point_chunks or t_start is None or t_end is None:
|
||||
point_chunks = []
|
||||
t_start = t_end = None
|
||||
return
|
||||
points = np.vstack(point_chunks)
|
||||
point_chunks = []
|
||||
start_s, end_s = t_start, t_end
|
||||
t_start = t_end = None
|
||||
if points.shape[0] < min_frame_points:
|
||||
return
|
||||
if kept % stride != 0:
|
||||
kept += 1
|
||||
return
|
||||
kept += 1
|
||||
if max_points_per_frame is not None and points.shape[0] > max_points_per_frame:
|
||||
select = np.linspace(0, points.shape[0] - 1, max_points_per_frame, dtype=int)
|
||||
points = points[select]
|
||||
if end_s <= start_s:
|
||||
end_s = start_s + 0.1
|
||||
frames.append(LidarFrameExport(t_start_s=start_s, t_end_s=end_s, points_xyz=points))
|
||||
|
||||
for chunk in capture.chunks:
|
||||
packet = chunk.raw
|
||||
if len(packet) != PACKET_LENGTH:
|
||||
continue
|
||||
packet_t = device_timestamp_ms(packet) * 1e-3
|
||||
unit = distance_unit_mm(packet)
|
||||
idx = DATA_START
|
||||
for _block in range(BLOCKS):
|
||||
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
|
||||
break
|
||||
az = read_u16_be(packet, idx + 2) * 0.01
|
||||
if prev_az is not None and prev_az > 270.0 and az < 90.0:
|
||||
emit()
|
||||
prev_az = az
|
||||
pts = _block_points(
|
||||
packet,
|
||||
idx,
|
||||
az,
|
||||
unit,
|
||||
vertical,
|
||||
horizontal,
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
)
|
||||
if pts.shape[0]:
|
||||
if t_start is None:
|
||||
t_start = packet_t
|
||||
t_end = packet_t
|
||||
point_chunks.append(pts)
|
||||
idx += BLOCK_LENGTH
|
||||
|
||||
emit()
|
||||
return frames
|
||||
@@ -0,0 +1,113 @@
|
||||
"""Decode Wheeltec N300 FDILink IMU frames from a V2 .rscap capture."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ImuSample:
|
||||
t_s: float
|
||||
gyro_rad_s: tuple[float, float, float]
|
||||
accel_m_s2: tuple[float, float, float]
|
||||
host_receive_utc_ticks: int
|
||||
device_timestamp_us: int
|
||||
|
||||
|
||||
def crc8_fdilink(data: bytes) -> int:
|
||||
crc = 0
|
||||
for value in data:
|
||||
crc ^= value
|
||||
for _ in range(8):
|
||||
crc = ((crc << 1) ^ 0x07) & 0xFF if crc & 0x80 else (crc << 1) & 0xFF
|
||||
return crc
|
||||
|
||||
|
||||
def crc16_fdilink(data: bytes) -> int:
|
||||
crc = 0
|
||||
for value in data:
|
||||
crc ^= value << 8
|
||||
for _ in range(8):
|
||||
crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF
|
||||
return crc
|
||||
|
||||
|
||||
def _host_ticks_for_span(chunks: list[RawChunk], start: int, end: int) -> int:
|
||||
stream_offset = 0
|
||||
last = chunks[0]
|
||||
for chunk in chunks:
|
||||
next_offset = stream_offset + len(chunk.raw)
|
||||
if start < next_offset and end > stream_offset:
|
||||
last = chunk
|
||||
stream_offset = next_offset
|
||||
return last.receive_utc_ticks
|
||||
|
||||
|
||||
def iter_n300_imu_samples(capture: CaptureFile) -> list[ImuSample]:
|
||||
"""Return CRC-valid MSG_IMU (0x40) samples sorted by device timestamp."""
|
||||
|
||||
samples: list[ImuSample] = []
|
||||
expected_lengths = {0x40: 56, 0x41: 48}
|
||||
for _segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while cursor < len(stream):
|
||||
start = stream.find(b"\xFC", cursor)
|
||||
if start < 0:
|
||||
break
|
||||
if start + 8 > len(stream):
|
||||
break
|
||||
payload_length = stream[start + 2]
|
||||
end = start + payload_length + 8
|
||||
if end > len(stream):
|
||||
if stream.find(b"\xFC", start + 1) < 0:
|
||||
break
|
||||
cursor = start + 1
|
||||
continue
|
||||
frame = stream[start:end]
|
||||
if frame[-1] != 0xFD:
|
||||
cursor = start + 1
|
||||
continue
|
||||
packet_id = frame[1]
|
||||
payload = frame[7:-1]
|
||||
header_ok = crc8_fdilink(frame[:4]) == frame[4]
|
||||
payload_ok = crc16_fdilink(payload) == int.from_bytes(frame[5:7], "big")
|
||||
expected = expected_lengths.get(packet_id)
|
||||
length_ok = expected is None or len(payload) == expected
|
||||
if not (header_ok and payload_ok and length_ok):
|
||||
cursor = start + 1
|
||||
continue
|
||||
if packet_id == 0x40:
|
||||
gyro = struct.unpack_from("<3f", payload, 0)
|
||||
accel = struct.unpack_from("<3f", payload, 12)
|
||||
device_us = struct.unpack_from("<q", payload, 48)[0]
|
||||
samples.append(
|
||||
ImuSample(
|
||||
t_s=float(device_us) * 1e-6,
|
||||
gyro_rad_s=gyro,
|
||||
accel_m_s2=accel,
|
||||
host_receive_utc_ticks=_host_ticks_for_span(chunks, start, end),
|
||||
device_timestamp_us=int(device_us),
|
||||
)
|
||||
)
|
||||
cursor = end
|
||||
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
|
||||
return samples
|
||||
|
||||
|
||||
def samples_to_arrays(samples: list[ImuSample]) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
if not samples:
|
||||
return (
|
||||
np.zeros(0, dtype=np.float64),
|
||||
np.zeros((0, 3), dtype=np.float64),
|
||||
np.zeros((0, 3), dtype=np.float64),
|
||||
)
|
||||
t = np.asarray([sample.t_s for sample in samples], dtype=np.float64)
|
||||
gyro = np.asarray([sample.gyro_rad_s for sample in samples], dtype=np.float64)
|
||||
accel = np.asarray([sample.accel_m_s2 for sample in samples], dtype=np.float64)
|
||||
return t, gyro, accel
|
||||
@@ -131,6 +131,39 @@ def parse_heading(line: str) -> dict:
|
||||
}
|
||||
|
||||
|
||||
def parse_pvtslna(line: str) -> dict:
|
||||
"""Parse Unicore/G90 ``#PVTSLNA`` into GGA-compatible position fields.
|
||||
|
||||
``fix_quality`` is synthesized as 4 when checksum-valid coordinates exist so
|
||||
the existing prepare gate (accepted fixes {4,5}) keeps working. Position
|
||||
stddevs are retained for audits.
|
||||
"""
|
||||
star = line.rfind("*")
|
||||
fields = line[1:star if star >= 0 else None].split(",")
|
||||
if len(fields) < 16:
|
||||
raise ValueError("PVTSLNA has too few fields")
|
||||
tow = safe_float(fields[5])
|
||||
return {
|
||||
"type": "PVTSLNA",
|
||||
"gnss_week": safe_int(fields[4]),
|
||||
"gnss_tow_ms": int(tow) if tow is not None else None,
|
||||
"altitude_m": safe_float(fields[10]),
|
||||
"lat_deg": safe_float(fields[11]),
|
||||
"lon_deg": safe_float(fields[12]),
|
||||
"height_std_m": safe_float(fields[13]),
|
||||
"latitude_std_m": safe_float(fields[14]),
|
||||
"longitude_std_m": safe_float(fields[15]),
|
||||
# Downstream prepare still filters on NMEA-style fix quality.
|
||||
"fix_quality": 4,
|
||||
"satellites": -1,
|
||||
"hdop": None,
|
||||
"differential_age_s": None,
|
||||
"position_time_utc": "",
|
||||
"geoid_separation_m": None,
|
||||
"station_id": "",
|
||||
}
|
||||
|
||||
|
||||
def chunk_source(chunks: list[RawChunk], offset: int, end: int) -> dict:
|
||||
first = chunks[0]
|
||||
last = chunks[-1]
|
||||
@@ -184,6 +217,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
try:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
row.update(parse_gga(line))
|
||||
elif line.startswith("#PVTSLNA"):
|
||||
row.update(parse_pvtslna(line))
|
||||
elif line.startswith("#UNIHEADINGA"):
|
||||
row.update(parse_heading(line))
|
||||
except ValueError as ex:
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import bisect
|
||||
import struct
|
||||
|
||||
from pipeline_common import *
|
||||
from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
|
||||
@@ -39,6 +40,7 @@ def source_for_span(chunks: list[RawChunk], start: int, end: int, segment_id: in
|
||||
"host_receive_monotonic_ticks": end_chunk.receive_monotonic_ticks,
|
||||
}
|
||||
|
||||
|
||||
def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
@@ -60,6 +62,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
try:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
row.update(parse_gga(line))
|
||||
elif line.startswith("#PVTSLNA"):
|
||||
row.update(parse_pvtslna(line))
|
||||
elif line.startswith("#UNIHEADINGA"):
|
||||
row.update(parse_heading(line))
|
||||
except ValueError as ex:
|
||||
@@ -68,7 +72,103 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
return rows
|
||||
|
||||
|
||||
def parse_imu_capture(capture: CaptureFile) -> list[dict]:
|
||||
def crc8_fdilink(data: bytes) -> int:
|
||||
crc = 0
|
||||
for value in data:
|
||||
crc ^= value
|
||||
for _ in range(8):
|
||||
crc = ((crc << 1) ^ 0x07) & 0xFF if crc & 0x80 else (crc << 1) & 0xFF
|
||||
return crc
|
||||
|
||||
|
||||
def crc16_fdilink(data: bytes) -> int:
|
||||
crc = 0
|
||||
for value in data:
|
||||
crc ^= value << 8
|
||||
for _ in range(8):
|
||||
crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF
|
||||
return crc
|
||||
|
||||
|
||||
def parse_n300_imu_capture(capture: CaptureFile) -> list[dict]:
|
||||
"""Parse Wheeltec N300 FDILink IMU frames; normalize to HI13-like keys."""
|
||||
|
||||
rows = []
|
||||
expected_lengths = {0x40: 56, 0x41: 48}
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while cursor < len(stream):
|
||||
start = stream.find(b"\xFC", cursor)
|
||||
if start < 0:
|
||||
break
|
||||
if start + 8 > len(stream):
|
||||
break
|
||||
payload_length = stream[start + 2]
|
||||
end = start + payload_length + 8
|
||||
if end > len(stream):
|
||||
if stream.find(b"\xFC", start + 1) < 0:
|
||||
break
|
||||
cursor = start + 1
|
||||
continue
|
||||
frame = stream[start:end]
|
||||
if frame[-1] != 0xFD:
|
||||
cursor = start + 1
|
||||
continue
|
||||
packet_id = frame[1]
|
||||
payload = frame[7:-1]
|
||||
header_ok = crc8_fdilink(frame[:4]) == frame[4]
|
||||
payload_ok = crc16_fdilink(payload) == int.from_bytes(frame[5:7], "big")
|
||||
expected = expected_lengths.get(packet_id)
|
||||
length_ok = expected is None or len(payload) == expected
|
||||
row = {
|
||||
"type": "N300",
|
||||
"tag": int(packet_id),
|
||||
"frame_length": len(frame),
|
||||
"crc_valid": bool(header_ok and payload_ok and length_ok),
|
||||
"raw_frame_hex": frame.hex(),
|
||||
}
|
||||
row.update(source_for_span(chunks, start, end, segment_id))
|
||||
if row["crc_valid"] and packet_id == 0x40:
|
||||
try:
|
||||
gyro = struct.unpack_from("<3f", payload, 0)
|
||||
accel = struct.unpack_from("<3f", payload, 12)
|
||||
device_us = struct.unpack_from("<q", payload, 48)[0]
|
||||
row.update(
|
||||
{
|
||||
"device_timestamp_us": int(device_us),
|
||||
# build_multisensor_npz.estimate_imu_times uses ms.
|
||||
"device_timestamp_ms": int(device_us) // 1000,
|
||||
"gyro_x_radps": gyro[0],
|
||||
"gyro_y_radps": gyro[1],
|
||||
"gyro_z_radps": gyro[2],
|
||||
"accel_x_mps2": accel[0],
|
||||
"accel_y_mps2": accel[1],
|
||||
"accel_z_mps2": accel[2],
|
||||
"pps_sync_stamp_ms": -1,
|
||||
}
|
||||
)
|
||||
except (IndexError, struct.error, ValueError) as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
row["crc_valid"] = False
|
||||
elif row["crc_valid"] and packet_id == 0x41:
|
||||
try:
|
||||
device_us = struct.unpack_from("<q", payload, 40)[0]
|
||||
row.update(
|
||||
{
|
||||
"device_timestamp_us": int(device_us),
|
||||
"device_timestamp_ms": int(device_us) // 1000,
|
||||
"pps_sync_stamp_ms": -1,
|
||||
}
|
||||
)
|
||||
except (IndexError, struct.error, ValueError) as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
cursor = end if row["crc_valid"] else start + 1
|
||||
return rows
|
||||
|
||||
|
||||
def parse_hi13_imu_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
@@ -104,3 +204,12 @@ def parse_imu_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows.append(row)
|
||||
cursor = end
|
||||
return rows
|
||||
|
||||
|
||||
def parse_imu_capture(capture: CaptureFile) -> list[dict]:
|
||||
"""Prefer N300 FDILink when present; fall back to legacy HI13."""
|
||||
|
||||
n300 = parse_n300_imu_capture(capture)
|
||||
if any(row.get("crc_valid") and row.get("type") == "N300" for row in n300):
|
||||
return n300
|
||||
return parse_hi13_imu_capture(capture)
|
||||
|
||||
Reference in New Issue
Block a user