Split sensor types into files

2026-07-06 17:31:02 -06:00
parent e6cb6a32b9
commit 9e03767e1b
25 changed files with 918 additions and 0 deletions
@@ -0,0 +1,110 @@
+CRC_TABLE = [
+        0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3,
+        0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33,
+        0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8,
+        0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77,
+        0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55,
+        0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4,
+        0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f,
+        0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff,
+        0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2,
+        0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12,
+        0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99,
+        0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14,
+        0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36,
+        0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9,
+        0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72,
+        0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2,
+        0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1,
+        0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71,
+        0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa,
+        0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35,
+        0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17,
+        0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8
+    ]
+
+def angle_step(start_angle, end_angle, length):
+    if start_angle <= end_angle:
+        return (end_angle - start_angle) / length
+    return (36000 + end_angle - start_angle) / length
+
+def angle_from_step(start_angle, step, index):
+    return (start_angle + (step * index)) % 36000
+
+def calc_crc_from_buffer(buffer, buffer_size):
+    crc = 0xD8
+    for i in range(0, buffer_size):
+        crc = CRC_TABLE[(crc ^ buffer[i]) & 0xff]
+    return crc
+
+def time_diff(time_1, time_2):
+    if time_1 > time_2:
+        time_1 -= 30000
+    return time_2 - time_1
+
+class Lidar2d:
+    def __init__(self, tx_pin, rx_pin, baudrate=230400):
+        self._tx_pin = tx_pin
+        self._rx_pin = rx_pin
+        self._baudrate = baudrate
+        self._uart = busio.UART(self._tx_pin, self._rx_pin, baudrate=baudrate)
+        self._prev_time = 0
+        self._timestamp = 0
+        self.has_update = False
+        self._update_time = [0, 0, 0, 0]
+        self._latest_data = []
+
+    def update(self, utc):
+        data = self._uart.read(2)
+        header = data[0]
+        verlen = data[1]
+        if header != 84 and verlen != 44:
+            return
+        data = self._uart.read(45)
+        speed = int.from_bytes(data[0:2], "little")  # deg / s
+        start_angle = int.from_bytes(data[2:4], "little")  # deg
+        points = [
+            (int.from_bytes(data[4:6], "little"), data[6]),
+            (int.from_bytes(data[7:9], "little"), data[9]),
+            (int.from_bytes(data[10:12], "little"), data[12]),
+            (int.from_bytes(data[13:15], "little"), data[15]),
+            (int.from_bytes(data[16:18], "little"), data[18]),
+            (int.from_bytes(data[19:21], "little"), data[21]),
+            (int.from_bytes(data[22:24], "little"), data[24]),
+            (int.from_bytes(data[25:27], "little"), data[27]),
+            (int.from_bytes(data[28:30], "little"), data[30]),
+            (int.from_bytes(data[31:33], "little"), data[33]),
+            (int.from_bytes(data[34:36], "little"), data[36]),
+            (int.from_bytes(data[37:39], "little"), data[39]),
+        ]
+        end_angle = int.from_bytes(data[40:42], "little")
+        self._prev_time = self._timestamp
+        self._timestamp = int.from_bytes(data[42:44], "little")
+        crc_check = data[44]
+        if calc_crc_from_buffer(data, 44) != crc_check:
+            return
+        self.has_update = True
+        self._latest_data = []
+        self._update_time = utc
+        step = angle_step(start_angle, end_angle, 11)
+        for idx, distance in enumerate(points):
+            angle = int(angle_from_step(start_angle, step, idx) * 100)
+            self._latest_data.append((angle, distance[0], distance[1]))
+
+    def get_lidar_message(self, utc):
+        if not self.has_update:
+            return None
+        self.has_update = False
+        message = bytearray(b'\xd4\x53\x2a\x6f')
+        message += self._update_time[0].to_bytes(1, 'little')
+        message += self._update_time[1].to_bytes(1, 'little')
+        message += self._update_time[2].to_bytes(1, 'little')
+        message += self._update_time[3].to_bytes(4, 'little')
+        message += (len(self._latest_data)).to_bytes(1, 'little')
+        for idx in range(len(self._latest_data)):
+            angle, distance, intensity = self._latest_data[idx]
+            message += angle.to_bytes(4, 'little')
+            message += distance.to_bytes(2, 'little')
+            message += intensity.to_bytes(1, 'little')
+        message += b'\x00\x00'
+        return message