car-os-sensor/lidar_code.py

import board
import busio
import digitalio
import time
from data_broadcast import DataBroadcast



class SensorClass:
    VEHICLE = 0xf1
    LIDAR_2D = 0x23
    
class PPS:
    def __init__(self, pin):
        self.pin = pin
        self.gpio = digitalio.DigitalInOut(self.pin)
        self.gpio.direction = digitalio.Direction.INPUT
        self.previous_value = self.gpio.value
        self.current_value = self.gpio.value
        self._update_time = time.monotonic_ns()
        self.gps_set = False
        self._utc = [0, 0, 0, 0]
        
    def update(self):
        ns = self.get_ns()
        s, ns = divmod(ns + self._utc[3], 1_000_000_000)
        m, s = divmod(self._utc[2] + s, 60)
        h, m = divmod(self._utc[1] + m, 60)
        _, h = divmod(self._utc[0] + h, 24)
        self._utc = [h, m, s, ns]
        self._update_time = time.monotonic_ns()
    
    def update_from_gps(self, gps):
        self._utc[0] = gps.utc[0]
        self._utc[1] = gps.utc[1]
        self._utc[2] = gps.utc[2]
        gps.time_updated = False
    
    
    def on_gps_fixed(self):
        self.gps_set = True
        self._update_time = time.monotonic_ns()
    
    def get_ns(self):
        return (time.monotonic_ns() - self._update_time)
    
    @property
    def utc(self):
        return self._utc

class Lidar2d:
    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 __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._latest_data = []
    
    def _angle_step(self, 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(self, start_angle, step, index):
        return (start_angle + (step * index)) % 36000

    def _calc_crc_from_buffer(self, buffer, buffer_size):
        crc = 0xD8
        for i in range(0, buffer_size):
            crc = self.CRC_TABLE[(crc ^ buffer[i]) & 0xff]
        return crc

    def _time_diff(self, time_1, time_2):
        if time_1 > time_2:
            time_1 -= 30000
        return time_2 - time_1
    
    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 self._calc_crc_from_buffer(data, 44) != crc_check:
            return
        self.has_update = True
        self._latest_data = [utc[:]]
        step = self._angle_step(start_angle, end_angle, 11)
        for idx, distance in enumerate(points):
            angle = int(self._angle_from_step(start_angle, step, idx) * 100)
            self._latest_data.append((angle, distance[0], distance[1]))
    
    def get_lidar_message(self, utc):
        self.has_update = False
        message = bytearray(b'\xd4\x53\x2a\x6f\x00\x00\x00')
        message[4] = utc[0]
        message[5] = utc[1]
        message[6] = utc[2]
        message += utc[3].to_bytes(4, 'little')
        message += self._latest_data[0][0].to_bytes(1, 'little')
        message += self._latest_data[0][1].to_bytes(1, 'little')
        message += self._latest_data[0][2].to_bytes(1, 'little')
        message += self._latest_data[0][3].to_bytes(4, 'little')
        message += (len(self._latest_data)-1).to_bytes(1, 'little')
        for idx in range(len(self._latest_data)-1):
            angle, distance, intensity = self._latest_data[idx+1]
            message += angle.to_bytes(4, 'little')
            message += distance.to_bytes(2, 'little')
            message += intensity.to_bytes(1, 'little')
        message += b'\x00\x00'
        return message
    

if __name__ == "__main__":
    
    network = DataBroadcast(SensorClass.LIDAR_2D, 256, 0.001, 5000, 0.1, 5001)
    network.initialize()
    network_timer = Timer(0.5)
    
    pps = PPS(board.GP22)
    lidar_2d = Lidar2d(board.GP0, board.GP1, baudrate=230400)
    while True:
        pps.update()
        
        if network_timer.check_timer(pps.utc):
            network.update(pps.utc, network_timer)
            
        lidar_2d.update(pps.utc)
        if lidar_2d.has_update:
            print(lidar_2d.get_lidar_message(pps.utc))