diff --git a/tests/test_export_rscap_helpers.py b/tests/test_export_rscap_helpers.py new file mode 100644 index 0000000..1ae4404 --- /dev/null +++ b/tests/test_export_rscap_helpers.py @@ -0,0 +1,104 @@ +"""Unit tests for rscap → V1 export helpers (no large real captures).""" + +from __future__ import annotations + +import struct + +import numpy as np + +from tools.rscap_v2.h32_msop import ( + CHANNELS, + PACKET_LENGTH, + decode_packet_points, + default_vertical_deg, + default_horizontal_deg, + device_timestamp_ms, + normalize_azimuth_deg, +) +from tools.rscap_v2.n300_imu import crc8_fdilink, crc16_fdilink, iter_n300_imu_samples +from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk + + +def _make_msop_packet(*, seconds: int = 100, microseconds: int = 5000, az_deg: float = 10.0) -> bytes: + packet = bytearray(PACKET_LENGTH) + packet[17] = 1 # 2.5 mm unit + sec = seconds.to_bytes(6, "big") + packet[20:26] = sec + packet[26:30] = int(microseconds).to_bytes(4, "big") + az_raw = int(round(az_deg * 100)) + for block in range(12): + offset = 42 + block * 100 + packet[offset] = 255 + packet[offset + 1] = 238 + packet[offset + 2] = (az_raw >> 8) & 0xFF + packet[offset + 3] = az_raw & 0xFF + idx = offset + 4 + for _ch in range(CHANNELS): + # 4.0 m at 2.5 mm/unit => raw = 1600 + packet[idx] = (1600 >> 8) & 0xFF + packet[idx + 1] = 1600 & 0xFF + packet[idx + 2] = 10 + idx += 3 + return bytes(packet) + + +def test_h32_device_timestamp_and_points(): + packet = _make_msop_packet(seconds=1700000000, microseconds=123000) + assert device_timestamp_ms(packet) == 1700000000 * 1000 + 123 + az_list, pts = decode_packet_points( + packet, + default_vertical_deg(), + default_horizontal_deg(), + min_range_m=0.1, + max_range_m=50.0, + ) + assert len(az_list) == 12 + assert pts.shape[0] == 12 * CHANNELS + assert np.allclose(np.linalg.norm(pts, axis=1), 4.0, atol=1e-3) + + +def test_normalize_azimuth(): + assert abs(normalize_azimuth_deg(190.0) + 170.0) < 1e-9 + + +def _n300_imu_frame(device_us: int = 123456) -> bytes: + payload = bytearray(56) + struct.pack_into("<3f", payload, 0, 0.1, -0.2, 0.3) + struct.pack_into("<3f", payload, 12, 0.0, 0.0, 9.81) + struct.pack_into(" None: + path.parent.mkdir(parents=True, exist_ok=True) + with path.open("w", newline="", encoding="utf-8") as handle: + writer = csv.writer(handle) + writer.writerow(["t", "gx", "gy", "gz", "ax", "ay", "az"]) + for index in range(t.shape[0]): + writer.writerow( + [ + f"{t[index]:.9f}", + f"{gyro[index, 0]:.12g}", + f"{gyro[index, 1]:.12g}", + f"{gyro[index, 2]:.12g}", + f"{accel[index, 0]:.12g}", + f"{accel[index, 1]:.12g}", + f"{accel[index, 2]:.12g}", + ] + ) + + +def write_lidar_session(root: Path, frames) -> dict: + frames_dir = root / "frames" + frames_dir.mkdir(parents=True, exist_ok=True) + index_path = root / "frames_index.csv" + with index_path.open("w", newline="", encoding="utf-8") as handle: + writer = csv.writer(handle) + writer.writerow(["frame_id", "filename", "t_start", "t_end"]) + point_counts = [] + for index, frame in enumerate(frames): + rel = f"frames/frame_{index:05d}.npz" + np.savez_compressed(root / rel, points=np.asarray(frame.points_xyz, dtype=np.float32)) + writer.writerow( + [ + index, + rel, + f"{frame.t_start_s:.9f}", + f"{frame.t_end_s:.9f}", + ] + ) + point_counts.append(int(frame.points_xyz.shape[0])) + return { + "frames": len(frames), + "points_min": int(min(point_counts)) if point_counts else 0, + "points_max": int(max(point_counts)) if point_counts else 0, + "points_mean": float(np.mean(point_counts)) if point_counts else 0.0, + "t_start": float(frames[0].t_start_s) if frames else None, + "t_end": float(frames[-1].t_end_s) if frames else None, + } + + +def export_session( + *, + imu_rscap: Path, + lidar_rscap: Path, + out: Path, + frame_stride: int = 1, + max_points_per_frame: int | None = 80000, + min_range_m: float = 0.3, + max_range_m: float = 120.0, + min_frame_points: int = 100, +) -> dict: + out.mkdir(parents=True, exist_ok=True) + imu_capture = read_capture(imu_rscap) + lidar_capture = read_capture(lidar_rscap) + + samples = iter_n300_imu_samples(imu_capture) + t, gyro, accel = samples_to_arrays(samples) + imu_csv = out / "imu.csv" + write_imu_csv(imu_csv, t, gyro, accel) + + frames = iter_h32_frames( + lidar_capture, + min_frame_points=min_frame_points, + frame_stride=frame_stride, + min_range_m=min_range_m, + max_range_m=max_range_m, + max_points_per_frame=max_points_per_frame, + ) + lidar_dir = out / "lidar" + lidar_stats = write_lidar_session(lidar_dir, frames) + + summary = { + "imu_rscap": str(imu_rscap), + "lidar_rscap": str(lidar_rscap), + "out": str(out), + "timestamp_policy": { + "imu": "n300_device_timestamp_us -> seconds", + "lidar": "h32_msop_device_timestamp_ms -> seconds (t_start/t_end per frame)", + "host_utc": "not used as calibration timeline", + }, + "imu": { + "samples": int(t.shape[0]), + "t_start": float(t[0]) if t.size else None, + "t_end": float(t[-1]) if t.size else None, + "capture": file_summary(imu_capture), + }, + "lidar": { + **lidar_stats, + "frame_stride": int(frame_stride), + "max_points_per_frame": max_points_per_frame, + "capture": file_summary(lidar_capture), + "angle_source": "default_msop_only_vertical_-16_to_16_deg", + }, + "outputs": { + "imu_csv": str(imu_csv), + "lidar_session": str(lidar_dir), + }, + } + (out / "export_summary.json").write_text( + json.dumps(summary, indent=2, ensure_ascii=False) + "\n", + encoding="utf-8", + ) + return summary + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--imu-rscap", type=Path, required=True, help="N300 V2 .rscap") + parser.add_argument("--lidar-rscap", type=Path, required=True, help="H32 MSOP V2 .rscap") + parser.add_argument("--out", type=Path, required=True, help="Output session directory") + parser.add_argument("--frame-stride", type=int, default=1, help="Keep every N-th LiDAR frame") + parser.add_argument( + "--max-points-per-frame", + type=int, + default=80000, + help="Uniform downsample cap per frame; 0 disables", + ) + 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("--min-frame-points", type=int, default=100) + args = parser.parse_args() + max_points = None if args.max_points_per_frame <= 0 else args.max_points_per_frame + summary = export_session( + imu_rscap=args.imu_rscap, + lidar_rscap=args.lidar_rscap, + out=args.out, + frame_stride=args.frame_stride, + max_points_per_frame=max_points, + min_range_m=args.min_range_m, + max_range_m=args.max_range_m, + min_frame_points=args.min_frame_points, + ) + print(json.dumps({ + "imu_samples": summary["imu"]["samples"], + "lidar_frames": summary["lidar"]["frames"], + "imu_csv": summary["outputs"]["imu_csv"], + "lidar_session": summary["outputs"]["lidar_session"], + "export_summary": str(Path(args.out) / "export_summary.json"), + }, ensure_ascii=False, indent=2)) + if summary["imu"]["samples"] == 0: + raise SystemExit("no valid N300 IMU samples decoded") + if summary["lidar"]["frames"] == 0: + raise SystemExit("no valid H32 frames decoded") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/rscap_v2/__init__.py b/tools/rscap_v2/__init__.py new file mode 100644 index 0000000..a2715ac --- /dev/null +++ b/tools/rscap_v2/__init__.py @@ -0,0 +1 @@ +"""V2 .rscap readers and sensor decoders for export to V1 intermediate format.""" diff --git a/tools/rscap_v2/capture_format_v2.py b/tools/rscap_v2/capture_format_v2.py new file mode 100644 index 0000000..3812980 --- /dev/null +++ b/tools/rscap_v2/capture_format_v2.py @@ -0,0 +1,254 @@ +from __future__ import annotations + +import binascii +import io +import struct +from dataclasses import dataclass, asdict +from pathlib import Path +from typing import BinaryIO, Iterator + + +FILE_MAGIC = "RAW_SERIAL_CAPTURE_FILE_V2" +RECORD_MAGIC = "RAW_SERIAL_RECORD_V2" +FOOTER_MAGIC = "RAW_SERIAL_CAPTURE_FOOTER_V2" + + +def read_7bit_int(stream: BinaryIO) -> int: + value = 0 + shift = 0 + while True: + raw = stream.read(1) + if not raw: + raise EOFError("truncated .NET string length") + value |= (raw[0] & 0x7F) << shift + if not raw[0] & 0x80: + return value + shift += 7 + if shift > 35: + raise ValueError("invalid .NET string length") + + +def read_dotnet_string(stream: BinaryIO) -> str: + length = read_7bit_int(stream) + raw = stream.read(length) + if len(raw) != length: + raise EOFError("truncated .NET string") + return raw.decode("utf-8") + + +def read_i32(stream: BinaryIO) -> int: + raw = stream.read(4) + if len(raw) != 4: + raise EOFError("truncated int32") + return struct.unpack(" int: + raw = stream.read(8) + if len(raw) != 8: + raise EOFError("truncated int64") + return struct.unpack(" int: + raw = stream.read(4) + if len(raw) != 4: + raise EOFError("truncated uint32") + return struct.unpack(" CaptureHeader: + if read_dotnet_string(stream) != FILE_MAGIC: + raise ValueError("not a V2 raw capture file") + version = read_i32(stream) + if version != 2: + raise ValueError(f"unsupported capture version: {version}") + return CaptureHeader( + sensor_kind=read_dotnet_string(stream), + session_id=read_dotnet_string(stream), + session_start_utc_ticks=read_i64(stream), + session_start_monotonic_ticks=read_i64(stream), + monotonic_frequency=read_i64(stream), + port=read_dotnet_string(stream), + baud=read_i32(stream), + file_start_utc_ticks=read_i64(stream), + ) + + +def parse_record_body(body: bytes, record_file_offset: int, record_crc: int) -> RawChunk: + stream = io.BytesIO(body) + if read_dotnet_string(stream) != RECORD_MAGIC: + raise ValueError("invalid record magic") + sequence = read_i64(stream) + receive_utc_ticks = read_i64(stream) + receive_monotonic_ticks = read_i64(stream) + raw_length = read_i32(stream) + if raw_length < 0 or raw_length > 64 * 1024 * 1024: + raise ValueError(f"invalid raw length: {raw_length}") + raw_offset = record_file_offset + 4 + stream.tell() + raw = stream.read(raw_length) + if len(raw) != raw_length: + raise EOFError("truncated raw bytes") + crc_valid = (binascii.crc32(body) & 0xFFFFFFFF) == record_crc + return RawChunk( + sequence=sequence, + receive_utc_ticks=receive_utc_ticks, + receive_monotonic_ticks=receive_monotonic_ticks, + raw=raw, + record_file_offset=record_file_offset, + raw_file_offset=raw_offset, + record_crc32=record_crc, + crc_valid=crc_valid, + ) + + +def parse_footer(body: bytes, expected_crc: int) -> CaptureFooter: + stream = io.BytesIO(body) + if read_dotnet_string(stream) != FOOTER_MAGIC: + raise ValueError("invalid footer magic") + clean_close = stream.read(1) == b"\x01" + records = read_i64(stream) + raw_bytes = read_i64(stream) + first_sequence = read_i64(stream) + last_sequence = read_i64(stream) + dropped_chunks = read_i64(stream) + dropped_bytes = read_i64(stream) + return CaptureFooter( + clean_close=clean_close, + records=records, + bytes=raw_bytes, + first_sequence=first_sequence, + last_sequence=last_sequence, + dropped_chunks=dropped_chunks, + dropped_bytes=dropped_bytes, + crc_valid=(binascii.crc32(body) & 0xFFFFFFFF) == expected_crc, + ) + + +def read_capture(path: Path) -> CaptureFile: + chunks: list[RawChunk] = [] + footer = None + truncated = False + with path.open("rb") as stream: + header = read_header(stream) + while True: + record_offset = stream.tell() + length_raw = stream.read(4) + if not length_raw: + break + if len(length_raw) != 4: + truncated = True + break + length = struct.unpack(" 1024 * 1024: + raise ValueError("invalid footer length") + footer_body = stream.read(footer_length) + if len(footer_body) != footer_length: + raise EOFError("truncated footer") + footer = parse_footer(footer_body, read_u32(stream)) + break + if length <= 0 or length > 64 * 1024 * 1024: + raise ValueError("invalid record length") + body = stream.read(length) + if len(body) != length: + raise EOFError("truncated record body") + record_crc = read_u32(stream) + chunks.append(parse_record_body(body, record_offset, record_crc)) + except (EOFError, ValueError): + truncated = True + break + return CaptureFile(str(path), header, chunks, footer, truncated) + + +def sequence_gaps(chunks: list[RawChunk]) -> list[tuple[int, int, int]]: + result = [] + for previous, current in zip(chunks, chunks[1:]): + if current.sequence > previous.sequence + 1: + result.append((previous.sequence, current.sequence, current.sequence - previous.sequence - 1)) + return result + + +def file_summary(capture: CaptureFile) -> dict: + gaps = sequence_gaps(capture.chunks) + sequences = [chunk.sequence for chunk in capture.chunks] + return { + "path": capture.path, + "sensor": capture.header.sensor_kind, + "session_id": capture.header.session_id, + "port": capture.header.port, + "baud": capture.header.baud, + "chunks_read": len(capture.chunks), + "bytes_read": sum(len(chunk.raw) for chunk in capture.chunks), + "first_sequence": sequences[0] if sequences else None, + "last_sequence": sequences[-1] if sequences else None, + "missing_chunks": sum(gap[2] for gap in gaps), + "gap_count": len(gaps), + "bad_record_crc": sum(not chunk.crc_valid for chunk in capture.chunks), + "truncated_tail": capture.truncated_tail, + "footer": None if capture.footer is None else asdict(capture.footer), + "gaps": gaps[:100], + } + + +def iter_contiguous_segments(chunks: list[RawChunk]) -> Iterator[tuple[int, list[RawChunk]]]: + if not chunks: + return + segment_id = 0 + current = [chunks[0]] + for previous, chunk in zip(chunks, chunks[1:]): + if chunk.sequence != previous.sequence + 1: + yield segment_id, current + segment_id += 1 + current = [chunk] + else: + current.append(chunk) + yield segment_id, current + diff --git a/tools/rscap_v2/h32_msop.py b/tools/rscap_v2/h32_msop.py new file mode 100644 index 0000000..7e1ed72 --- /dev/null +++ b/tools/rscap_v2/h32_msop.py @@ -0,0 +1,224 @@ +"""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 + + +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 + + +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 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 diff --git a/tools/rscap_v2/n300_imu.py b/tools/rscap_v2/n300_imu.py new file mode 100644 index 0000000..0742071 --- /dev/null +++ b/tools/rscap_v2/n300_imu.py @@ -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(" 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