Split sensor types into files

code.py

@@ -0,0 +1,78 @@
+import time
+import board
+import busio
+import adafruit_bno055
+import adafruit_gps
+import digitalio
+from car_os.network_handler import NetworkHandler
+import math
+from car_os.pps import PPS
+from car_os.gps import GPS
+from car_os.imu import IMU
+from car_os.timer import Timer
+from car_os.enums import SensorClass
+
+def main():
+    heartbeat_timer = Timer(15)
+    
+    network = NetworkHandler(SensorClass.VEHICLE, 256, 0.001, 5000, 0.1, 5001)
+    network.initialize()
+    network_timer = Timer(0.5)
+    
+    pps = PPS(board.GP22)
+    imu = IMU(board.GP21, board.GP20, 0x0C, (44, 66, -109), (-1, 3, -1), (-7, -16, -39))
+    gps = GPS(board.GP27, board.GP26, 0x42)
+    
+    while True:
+        pps.update()
+        gps.update(pps.utc)
+        imu.update(pps.utc)
+            
+        if not gps.has_fix:
+            print(time.time())
+            print("waiting for gps fix")
+            print()
+            continue
+        
+        if gps.time_updated:
+            pps.update_from_gps(gps)
+        
+        if network_timer.check_timer(pps.utc):
+            network.update(pps.utc, network_timer)
+        
+        #if heartbeat_timer.check_timer(pps.utc):
+        #    message = bytearray(b'\xd4\x53\x6e\x4e\x00\x00\x00')
+        #    message[4] = pps.utc[0]
+        #    message[5] = pps.utc[1]
+        #    message[6] = pps.utc[2]
+        #    message += b'\x00\x00'
+        #    network.send(message)
+        
+        if not gps.ready:
+            continue
+        try:
+            message = gps.get_position_message()
+            if message is not None:
+                network.send(message)
+        except:
+            pass
+        try:
+            message = gps.get_sat_message()
+            if message is not None:
+                network.send(message)
+        except:
+            pass
+        try:
+            message = imu.get_accel_message()
+            if message is not None:
+                network.send(message)
+        except:
+            pass
+        try:
+            message = imu.get_mag_message()
+            if message is not None:
+                network.send(message)
+        except:
+            pass
+            
+main()

lib/car_os/enums.py

@@ -0,0 +1,4 @@
+
+class SensorClass:
+    VEHICLE = 0xf1
+    LIDAR_2D = 0x23

lib/car_os/gps.py

@@ -0,0 +1,135 @@
+import busio
+import adafruit_gps
+import time
+
+SAT_CODE_MAP = {
+    "GA": "Galileo",
+    "GB": "BeiDou",
+    "GI": "NavIC",
+    "GL": "GLONASS",
+    "GP": "GPS",
+    "GQ": "QZSS",
+    "GN": "GNSS",
+}
+    
+class GPS:
+    def __init__(self, scl_pin, sda_pin, address):
+        self._gps_i2c = busio.I2C(scl_pin, sda_pin)
+        self._gps = adafruit_gps.GPS_GtopI2C(self._gps_i2c, address=address, debug=False)
+        time.sleep(1)
+        self._gps.send_command(b"PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0", add_checksum=True)
+        self._gps.send_command(b"PMTK220,500", add_checksum=True)
+        self._gps_time = (0, 0, 0)
+        self._previous_gps_time = (0, 0, 0)
+        self.time_updated = False
+        self.gps_updated = False
+        self.sats_updated = False
+        self._gps_update_time = [0, 0, 0, 0]
+        self._sats_update_time = [0, 0, 0, 0]
+        self._previous_latitude = 999999
+        self._previous_sats = []
+        
+    
+    def update(self, utc):
+        if not self._gps.update() or not self._gps.has_fix:
+            return False
+        self._gps_time = (self._gps.timestamp_utc.tm_hour, self._gps.timestamp_utc.tm_min, self._gps.timestamp_utc.tm_sec)
+        if self._gps_time != self._previous_gps_time:
+            self.time_updated = True
+            self._previous_gps_time = self._gps_time[:]
+        if self._gps.latitude != self._previous_latitude:
+            self.gps_updated = True
+            self._gps_update_time = utc
+            self._previous_latitude = self._gps.latitude
+        if self._gps._sats is not None and (self._gps.sats != self._previous_sats):
+            self._previous_sats = self._gps._sats[:]
+            self.sats_updated =True
+            self._sats_update_time = utc
+        return True
+    
+    def get_position_message(self):
+        if not self.ready:
+            return None
+        if not self.gps_updated:
+            return None
+        self.gps_updated = False
+        message = bytearray(b'\xd4\x53\x2a\x7c')
+        message += self._gps_update_time[0].to_bytes(1, 'little')
+        message += self._gps_update_time[1].to_bytes(1, 'little')
+        message += self._gps_update_time[2].to_bytes(1, 'little')
+        message += self._gps_update_time[3].to_bytes(4, 'little')
+        message += round((self._gps.latitude + 90) * 1_000_000_000).to_bytes(8, 'little')
+        message += round((self._gps.longitude + 180) * 1_000_000_000).to_bytes(8, 'little')
+        message += int(self._gps.altitude_m < 0).to_bytes(1, 'little')
+        message += round(abs(self._gps.altitude_m) * 1_000).to_bytes(4, 'little')
+        message += int(self._gps.height_geoid < 0).to_bytes(1, 'little')
+        message += round(abs(self._gps.height_geoid) * 100).to_bytes(2, 'little')
+        message += int(self._gps.speed_kmh < 0).to_bytes(1, 'little')
+        message += round(abs(self._gps.speed_kmh) * 1_000_000).to_bytes(4, 'little')
+        message += round(self._gps.pdop * 1_000).to_bytes(4, 'little')
+        message += round(self._gps.hdop * 1_000).to_bytes(4, 'little')
+        message += round(self._gps.vdop * 1_000).to_bytes(4, 'little')
+        message += b'\x00\x00'
+        return message
+    
+    def get_sat_message(self, utc):
+        if not self.ready:
+            return None
+        if not self.sats_updated:
+            return None
+        self.sats_updated = False
+        message = bytearray(b'\xd4\x53\x2a\x7b')
+        message += self._sats_update_time[0].to_bytes(1, 'little')
+        message += self._sats_update_time[1].to_bytes(1, 'little')
+        message += self._sats_update_time[2].to_bytes(1, 'little')
+        message += self._sats_update_time[3].to_bytes(4, 'little')
+        talkers = {
+            "GA": 0,
+            "GB": 0,
+            "GI": 0,
+            "GL": 0,
+            "GP": 0,
+            "GQ": 0,
+            "GN": 0,
+        }
+        if self._gps.sats:
+            for sat in self._gps.sats:
+                talkers[sat[:2]] += 1
+        for talker in ["GA", "GB", "GI", "GL", "GP", "GQ", "GN"]:
+            message += talkers[talker].to_bytes(1, 'little')
+        message += len(self._gps.sats).to_bytes(1, 'little')
+        for gps_id in self._gps.sats:
+            string_encode = bytearray(self._gps.sats[gps_id][0].encode('ascii'))
+            while len(string_encode) < 4:
+                string_encode += b'\x00'
+            message += string_encode
+            message += max(0, self._gps.sats[gps_id][1]).to_bytes(1, 'little')
+            message += self._gps.sats[gps_id][2].to_bytes(2, 'little')
+        message += b'\x00\x00'
+        return message
+    
+    @property
+    def time(self):
+        return self._gps_time
+    
+    @property
+    def has_fix(self):
+        return self._gps.has_fix
+    
+    @property
+    def ready(self):
+        if self._gps.latitude_degrees is None:
+            return False
+        if self._gps.longitude_degrees is None:
+            return False
+        if self._gps.altitude_m is None:
+            return False
+        if self._gps.speed_kmh is None:
+            return False
+        if self._gps.pdop is None:
+            return False
+        if self._gps.hdop is None:
+            return False
+        if self._gps.vdop is None:
+            return False
+        return True

lib/car_os/imu.py

@@ -0,0 +1,211 @@
+import busio
+import adafruit_bno055
+import time
+
+
+class ImuMode:
+    CONFIG_MODE = 0x00
+    ACCONLY_MODE = 0x01
+    MAGONLY_MODE = 0x02
+    GYRONLY_MODE = 0x03
+    ACCMAG_MODE = 0x04
+    ACCGYRO_MODE = 0x05
+    MAGGYRO_MODE = 0x06
+    AMG_MODE = 0x07
+    IMUPLUS_MODE = 0x08
+    COMPASS_MODE = 0x09
+    M4G_MODE = 0x0A
+    NDOF_FMC_OFF_MODE = 0x0B
+    NDOF_MODE = 0x0C
+
+
+class IMU:
+    def __init__(self, scl_pin, sda_pin, mode, mag_offset=None, gyro_offset=None, accel_offset=None):
+        i2c = busio.I2C(scl_pin, sda_pin)
+        self._imu = adafruit_bno055.BNO055_I2C(i2c)
+        print(self._imu.axis_remap)
+        self._imu.axis_remap = (0, 1, 2, 0, -1, -1)
+        self._imu.mode = adafruit_bno055.ACCMAG_MODE
+        time.sleep(1)
+        self._imu.mode = adafruit_bno055.NDOF_MODE
+        if mag_offset:
+            self._imu.offsets_magnetometer = mag_offset
+        if gyro_offset:
+            self._imu.offsets_gyroscope = gyro_offset
+        if accel_offset:
+            self._imu.offsets_accelerometer = accel_offset
+        self._accel_update_time = [0, 0, 0, 0]
+        self.accel_updated = False
+        self._acceleration = [0, 0, 0]
+        self._linear_acceleration = [0, 0, 0]
+        self._gravity = [0, 0, 0]
+
+        self._mag_update_time = [0, 0, 0, 0]
+        self.mag_updated = False
+        self._mag = [0, 0, 0]
+
+        self._gyro_update_time = [0, 0, 0, 0]
+        self.gyro_updated = False
+        self._gyro = [0, 0, 0]
+
+        self._euler_update_time = [0, 0, 0, 0]
+        self.euler_updated = False
+        self._euler = [0, 0, 0]
+
+        self._quat_update_time = [0, 0, 0, 0]
+        self.quat_updated = False
+        self._quat = [0, 0, 0, 0]
+        #self._do_calibration()
+        
+    def _do_calibration(self):
+        print("Magnetometer: Perform the figure-eight calibration dance.")
+        while not self._imu.calibration_status[3] == 3:
+            # Calibration Dance Step One: Magnetometer
+            #   Move sensor away from magnetic interference or shields
+            #   Perform the figure-eight until calibrated
+            print(f"Mag Calib Status: {100 / 3 * self._imu.calibration_status[3]:3.0f}%")
+            time.sleep(1)
+        print("... CALIBRATED")
+        time.sleep(1)
+
+        print("Accelerometer: Perform the six-step calibration dance.")
+        while not self._imu.calibration_status[2] == 3:
+            # Calibration Dance Step Two: Accelerometer
+            #   Place sensor board into six stable positions for a few seconds each:
+            #    1) x-axis right, y-axis up,    z-axis away
+            #    2) x-axis up,    y-axis left,  z-axis away
+            #    3) x-axis left,  y-axis down,  z-axis away
+            #    4) x-axis down,  y-axis right, z-axis away
+            #    5) x-axis left,  y-axis right, z-axis up
+            #    6) x-axis right, y-axis left,  z-axis down
+            #   Repeat the steps until calibrated
+            print(f"Accel Calib Status: {100 / 3 * self._imu.calibration_status[2]:3.0f}%")
+            time.sleep(1)
+        print("... CALIBRATED")
+        time.sleep(1)
+
+        print("Gyroscope: Perform the hold-in-place calibration dance.")
+        while not self._imu.calibration_status[1] == 3:
+            # Calibration Dance Step Three: Gyroscope
+            #  Place sensor in any stable position for a few seconds
+            #  (Accelerometer calibration may also calibrate the gyro)
+            print(f"Gyro Calib Status: {100 / 3 * self._imu.calibration_status[1]:3.0f}%")
+            time.sleep(1)
+        print("... CALIBRATED")
+        time.sleep(1)
+
+        print("\nCALIBRATION COMPLETED")
+        print("Insert these preset offset values into project code:")
+        print(f"  Offsets_Magnetometer:  {self._imu.offsets_magnetometer}")
+        print(f"  Offsets_Gyroscope:     {self._imu.offsets_gyroscope}")
+        print(f"  Offsets_Accelerometer: {self._imu.offsets_accelerometer}")
+
+    def update(self, utc):
+        if self._acceleration != self._imu.acceleration:
+            self._acceleration = self._imu.acceleration
+            self._linear_acceleration = self._imu.linear_acceleration
+            self._gravity = self._imu.gravity
+            self._accel_update_time = utc
+            self.accel_updated = True
+    
+    def get_accel_message(self):
+        if not self.accel_updated:
+            return
+        self.accel_updated = False
+        message = bytearray(b'\xd4\x53\x2a\xfa')
+        message += self._accel_update_time[0].to_bytes(1, 'little')
+        message += self._accel_update_time[1].to_bytes(1, 'little')
+        message += self._accel_update_time[2].to_bytes(1, 'little')
+        message += self._accel_update_time[3].to_bytes(4, 'little')
+        value_list = list(self._acceleration)
+        for idx in range(len(value_list)):
+            neg = int(value_list[idx] < 0)
+            val = abs(round(value_list[idx]*1000000))
+            message += neg.to_bytes(1, 'little')
+            message += val.to_bytes(4, 'little')
+        value_list = list(self._linear_acceleration)
+        for idx in range(len(value_list)):
+            neg = int(value_list[idx] < 0)
+            val = abs(round(value_list[idx]*1000000))
+            message += neg.to_bytes(1, 'little')
+            message += val.to_bytes(4, 'little')
+        value_list = list(self._gravity)
+        for idx in range(len(value_list)):
+            neg = int(value_list[idx] < 0)
+            val = abs(round(value_list[idx]*1000000))
+            message += neg.to_bytes(1, 'little')
+            message += val.to_bytes(4, 'little')
+        message += b'\x00\x00'
+        return message
+    
+    def get_mag_message(self):
+        if not self.mag_updated:
+            return
+        self.mag_updated = False
+        message = bytearray(b'\xd4\x53\x2a\x39')
+        message += self._mag_update_time[0].to_bytes(1, 'little')
+        message += self._mag_update_time[1].to_bytes(1, 'little')
+        message += self._mag_update_time[2].to_bytes(1, 'little')
+        message += self._mag_update_time[3].to_bytes(4, 'little')
+        value_list = list(self._mag)
+        for idx in range(len(value_list)):
+            neg = int(value_list[idx] < 0)
+            message += neg.to_bytes(1, 'little')
+            val = abs(round(value_list[idx]*1000))
+            message += val.to_bytes(4, 'little')
+        message += b'\x00\x00'
+        return message
+    
+    def get_gyro_message(self):
+        if not self.gyro_updated:
+            return
+        self.gyro_updated = False
+        message = bytearray(b'\xd4\x53\x2a\xa7')
+        message += self._gyro_update_time[0].to_bytes(1, 'little')
+        message += self._gyro_update_time[1].to_bytes(1, 'little')
+        message += self._gyro_update_time[2].to_bytes(1, 'little')
+        message += self._gyro_update_time[3].to_bytes(4, 'little')
+        value_list = list(self._gyro)
+        for idx in range(len(value_list)):
+            neg = int(value_list[idx] < 0)
+            message += neg.to_bytes(1, 'little')
+            val = abs(round(value_list[idx]*1000000))
+            message += val.to_bytes(8, 'little')
+        message += b'\x00\x00'
+        return message
+    
+    def get_euler_message(self):
+        if not self.euler_updated:
+            return
+        self.euler_updated = False
+        message = bytearray(b'\xd4\x53\x2a\x5e')
+        message += self._euler_update_time[0].to_bytes(1, 'little')
+        message += self._euler_update_time[1].to_bytes(1, 'little')
+        message += self._euler_update_time[2].to_bytes(1, 'little')
+        message += self._euler_update_time[3].to_bytes(4, 'little')
+        value_list = list(self._euler)
+        for idx in range(len(value_list)):
+            neg = int(value_list[idx] < 0)
+            message += neg.to_bytes(1, 'little')
+            val = abs(round(value_list[idx]*1000))
+            message += val.to_bytes(4, 'little')
+        message += b'\x00\x00'
+        return message
+    
+    def get_quaternion_message(self):
+        if not self.quat_updated:
+            return
+        self.quat_updated = False
+        message = bytearray(b'\xd4\x53\x2a\xba')
+        message += self._quat_update_time[0].to_bytes(1, 'little')
+        message += self._quat_update_time[1].to_bytes(1, 'little')
+        message += self._quat_update_time[2].to_bytes(1, 'little')
+        message += self._quat_update_time[3].to_bytes(4, 'little')
+        value_list = list(self._quat)
+        for idx in range(len(value_list)):
+            neg = int(value_list[idx] < 0)
+            val = abs(round(value_list[idx]*1000000))
+            message += neg.to_bytes(1, 'little')
+            message += val.to_bytes(4, 'little')
+        message += b'\x00\x00'
+        return message

lib/car_os/lidar_2d.py

@@ -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

lib/car_os/network_handler.py

@@ -0,0 +1,125 @@
+from os import getenv
+import ipaddress
+import wifi
+import socketpool
+import time
+import random
+
+random.seed(time.time())
+random.random()
+
+
+class NetworkHandler:
+    def __init__(self, sensor_class, listen_max_buffer, listen_timeout, listen_port, send_timeout, send_port):
+        self._sensor_class = sensor_class
+        print(self._sensor_class)
+        self._wifi_connected = False
+        self._listen_socket = None
+        self._listen_max_buffer = listen_max_buffer
+        self._listen_timeout = listen_timeout
+        self._listen_port = listen_port
+        self._listen_buffer = bytearray(listen_max_buffer)
+        self._pool = None
+        self._host_addr = ''
+        self._listen_port = listen_port
+        self._send_addr = ''
+        self._send_port = send_port
+        self._send_socket = None
+        self._send_timeout = send_timeout
+        self._ssid = ''
+
+    def initialize(self):
+        self._connect_wifi()
+        while not self._wifi_connected:
+            time.sleep(15)
+            self._connect_wifi()
+        print('WiFi connected -', self._ssid)
+        self._pool = socketpool.SocketPool(wifi.radio)
+        self._host_addr = str(wifi.radio.ipv4_address)
+        print(self._host_addr)
+        self._setup_listen_socket()
+        print(f'Send addr: "{self._send_addr}"')
+        while len(self._send_addr) == 0:
+            print('Trying to connect to CarOS host', time.time())
+            if self._receive_udp():
+                break
+            time.sleep(random.randint(1, 10) / 10)
+
+    def update(self, utc, timer):
+        if self._receive_udp():
+            timer.reset(utc)
+            timer.set_interval(10)
+        if not timer.check_timer(utc):
+            return
+        if self._send_socket:
+            self._send_socket.close()
+            self._send_socket = None
+        self._send_addr = None
+
+    def send(self, message):
+        if self._send_addr is None or self._send_socket is None:
+            return
+        buffer = bytearray(message)
+
+        ck_a, ck_b = self._calculate_checksum(buffer)
+        buffer[-2] = ck_a
+        buffer[-1] = ck_b
+        try:
+            self._send_socket.sendto(buffer, (self._send_addr, self._send_port))
+        except Exception as e:
+            print("Error while sending data to: '", self._send_addr, "'", self._send_port)
+            print(e)
+
+    def _send_peer_request(self):
+        print("Send peer request")
+        message = bytearray(b'\xd4\x53\x6E\x77\x00')
+        message[-1] = self._sensor_class
+        message += '\x00' + '\x00'
+        self.send(message)
+
+    def _calculate_checksum(self, packet):
+        ck_a = 0
+        ck_b = 0
+        for i in range(2, len(packet) - 2):
+            ck_a += packet[i]
+            ck_b += ck_a
+        return [ck_a % 0x100, ck_b % 0x100]
+
+    def _connect_wifi(self):
+        self._ssid = getenv("CIRCUITPY_WIFI_SSID")
+        print(f'Trying to connect to "{self._ssid}"')
+        password = getenv("CIRCUITPY_WIFI_PASSWORD")
+        try:
+            wifi.radio.connect(self._ssid, password)
+            self._wifi_connected = True
+        except Exception as e:
+            print("Could not find WiFi info. Check your settings.toml file!")
+            self._wifi_connected = False
+
+    def _setup_listen_socket(self):
+        self._listen_socket = self._pool.socket(self._pool.AF_INET, self._pool.SOCK_DGRAM)
+        self._listen_socket.settimeout(self._listen_timeout)
+        self._listen_socket.bind((self._host_addr, self._listen_port))
+
+    def _setup_send_socket(self):
+        self._send_socket = self._pool.socket(self._pool.AF_INET, self._pool.SOCK_DGRAM)
+        self._send_socket.settimeout(self._send_timeout)
+
+    def _receive_udp(self):
+        send_addr = None
+        try:
+            size, addr = self._listen_socket.recvfrom_into(self._listen_buffer)
+            data = self._listen_buffer[:size].decode('utf-8')
+            send_addr, time_str = data.split('|')
+            print('Data received:', data)
+        except OSError:
+            return False
+        if send_addr is None:
+            return False
+        if send_addr != self._send_addr:
+            self._send_addr = send_addr
+            if self._send_socket:
+                self._send_socket.close()
+            self._setup_send_socket()
+        self._send_peer_request()
+        return True

lib/car_os/pps.py

@@ -0,0 +1,49 @@
+import digitalio
+import time
+
+    
+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._pps_offset = [0, 0, 0, 0]
+        self._utc = [0, 0, 0]
+        
+    def update(self):
+        current_time = time.monotonic_ns()
+        self.previous_value = self.current_value
+        self.current_value = self.gpio.value
+        if not self.current_value and self.previous_value:
+            self._update_time = current_time
+            self._pps_offset = [0, 0, 0, 0]
+            self._utc[2] = self._utc[2] + 1
+    
+    def update_from_gps(self, gps):
+        self._utc[0] = gps.time[0]
+        self._utc[1] = gps.time[1]
+        self._utc[2] = gps.time[2]
+        gps.time_updated = False
+    
+    def on_gps_fixed(self):
+        self.gps_set = True
+    
+    def update_pps_offset(self):
+        ns = time.monotonic_ns() - self._update_time
+        s, ns = divmod(ns, 1_000_000_000)
+        m, s = divmod(self._pps_offset[2] + s, 60)
+        h, m = divmod(self._pps_offset[1] + m, 60)
+        _, h = divmod(self._pps_offset[0] + h, 24)
+        self._pps_offset = [h, m, s, ns]
+    
+    @property
+    def utc(self):
+        self.update_pps_offset()
+        m, s = divmod(self._pps_offset[2] + self._utc[2], 60)
+        h, m = divmod(self._pps_offset[1] + self._utc[1] + m, 60)
+        _, h = divmod(self._pps_offset[0] + self._utc[0] + h, 24)
+        return (h, m, s, self._pps_offset[3])

lib/car_os/timer.py

@@ -0,0 +1,27 @@
+
+
+class Timer:
+    def __init__(self, interval_s):
+        self._interval_s = interval_s
+        self._last_time = 0
+        self._last_utc = (0,0,0)
+    
+    def set_interval(self, new_interval_s):
+        self._interval_s = new_interval_s
+    
+    def check_timer(self, utc):
+        ns = utc[3] * 0.000000001
+        timestamp = utc[0] * 3600 + utc[1] * 60 + utc[2] + ns
+        shifted_timestamp = timestamp
+        if self._last_utc[0] == 23 and utc[0] == 0:
+            shifted_timestamp += 24*3600
+        if (shifted_timestamp - self._last_time) >= self._interval_s:
+            self._last_time = timestamp
+            self._last_utc = (utc[0], utc[1], utc[2])
+            return True
+        return False
+    
+    def reset(self, utc):
+        ns = utc[3] * 0.000000001
+        self._last_time = utc[0] * 3600 + utc[1] * 60 + utc[2] + ns
+        self._last_utc = (utc[0], utc[1], utc[2])

lidar_code.py

@@ -0,0 +1,179 @@
+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))