179 lines
7.1 KiB
Python
179 lines
7.1 KiB
Python
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)) |