41 lines
1.2 KiB
Python
41 lines
1.2 KiB
Python
"""Parse RoboSense H32 DIFOP channel calibration angles."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
|
|
import numpy as np
|
|
|
|
CHANNELS = 32
|
|
VERTICAL_START = 468
|
|
HORIZONTAL_START = 564
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class DifopAngles:
|
|
vertical_deg: np.ndarray # (32,)
|
|
horizontal_deg: np.ndarray # (32,)
|
|
|
|
|
|
def _read_u16_be(packet: bytes, index: int) -> int:
|
|
return (packet[index] << 8) | packet[index + 1]
|
|
|
|
|
|
def signed_angle_deg(packet: bytes, index: int) -> float:
|
|
"""Match RSLidarH32 plugin SignedAngle: sign byte + BE u16 * 0.01 deg."""
|
|
|
|
sign = -1.0 if packet[index] > 0 else 1.0
|
|
return sign * _read_u16_be(packet, index + 1) * 0.01
|
|
|
|
|
|
def parse_difop_angles(packet: bytes) -> DifopAngles:
|
|
needed = HORIZONTAL_START + CHANNELS * 3
|
|
if len(packet) < needed:
|
|
raise ValueError(f"DIFOP packet too short: {len(packet)} < {needed}")
|
|
vertical = np.empty(CHANNELS, dtype=np.float64)
|
|
horizontal = np.empty(CHANNELS, dtype=np.float64)
|
|
for channel in range(CHANNELS):
|
|
vertical[channel] = signed_angle_deg(packet, VERTICAL_START + channel * 3)
|
|
horizontal[channel] = signed_angle_deg(packet, HORIZONTAL_START + channel * 3)
|
|
return DifopAngles(vertical_deg=vertical, horizontal_deg=horizontal)
|