105 lines
3.3 KiB
Python
105 lines
3.3 KiB
Python
"""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("<q", payload, 48, device_us)
|
|
header = bytearray([0xFC, 0x40, 56, 7])
|
|
header.append(crc8_fdilink(header))
|
|
crc = crc16_fdilink(payload)
|
|
frame = bytes(header) + crc.to_bytes(2, "big") + bytes(payload) + b"\xFD"
|
|
return frame
|
|
|
|
|
|
def test_n300_imu_sample_from_capture_chunks():
|
|
frame = _n300_imu_frame(654321)
|
|
header = CaptureHeader(
|
|
sensor_kind="wheeltec-n300",
|
|
session_id="test",
|
|
session_start_utc_ticks=0,
|
|
session_start_monotonic_ticks=0,
|
|
monotonic_frequency=10_000_000,
|
|
port="COM1",
|
|
baud=921600,
|
|
file_start_utc_ticks=0,
|
|
)
|
|
chunk = RawChunk(
|
|
sequence=1,
|
|
receive_utc_ticks=100,
|
|
receive_monotonic_ticks=1,
|
|
raw=frame,
|
|
record_file_offset=0,
|
|
raw_file_offset=0,
|
|
record_crc32=0,
|
|
crc_valid=True,
|
|
)
|
|
capture = CaptureFile(path="mem", header=header, chunks=[chunk], footer=None)
|
|
samples = iter_n300_imu_samples(capture)
|
|
assert len(samples) == 1
|
|
assert samples[0].device_timestamp_us == 654321
|
|
assert abs(samples[0].t_s - 654321e-6) < 1e-12
|
|
assert abs(samples[0].gyro_rad_s[0] - 0.1) < 1e-6
|
|
assert abs(samples[0].accel_m_s2[2] - 9.81) < 1e-5
|