diff --git a/code.py b/code.py new file mode 100644 index 0000000..5be4f55 --- /dev/null +++ b/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() diff --git a/lib/adafruit_bno055.mpy b/lib/adafruit_bno055.mpy new file mode 100644 index 0000000..d889c16 Binary files /dev/null and b/lib/adafruit_bno055.mpy differ diff --git a/lib/adafruit_bus_device/__init__.py b/lib/adafruit_bus_device/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/lib/adafruit_bus_device/i2c_device.mpy b/lib/adafruit_bus_device/i2c_device.mpy new file mode 100644 index 0000000..561f277 Binary files /dev/null and b/lib/adafruit_bus_device/i2c_device.mpy differ diff --git a/lib/adafruit_bus_device/spi_device.mpy b/lib/adafruit_bus_device/spi_device.mpy new file mode 100644 index 0000000..34e296f Binary files /dev/null and b/lib/adafruit_bus_device/spi_device.mpy differ diff --git a/lib/adafruit_gps.mpy b/lib/adafruit_gps.mpy new file mode 100644 index 0000000..1cb5213 Binary files /dev/null and b/lib/adafruit_gps.mpy differ diff --git a/lib/adafruit_register/__init__.py b/lib/adafruit_register/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/lib/adafruit_register/i2c_bcd_alarm.mpy b/lib/adafruit_register/i2c_bcd_alarm.mpy new file mode 100644 index 0000000..cd17a7c Binary files /dev/null and b/lib/adafruit_register/i2c_bcd_alarm.mpy differ diff --git a/lib/adafruit_register/i2c_bcd_datetime.mpy b/lib/adafruit_register/i2c_bcd_datetime.mpy new file mode 100644 index 0000000..747b084 Binary files /dev/null and b/lib/adafruit_register/i2c_bcd_datetime.mpy differ diff --git a/lib/adafruit_register/i2c_bit.mpy b/lib/adafruit_register/i2c_bit.mpy new file mode 100644 index 0000000..be27c0f Binary files /dev/null and b/lib/adafruit_register/i2c_bit.mpy differ diff --git a/lib/adafruit_register/i2c_bits.mpy b/lib/adafruit_register/i2c_bits.mpy new file mode 100644 index 0000000..6b156ab Binary files /dev/null and b/lib/adafruit_register/i2c_bits.mpy differ diff --git a/lib/adafruit_register/i2c_struct.mpy b/lib/adafruit_register/i2c_struct.mpy new file mode 100644 index 0000000..d2fd738 Binary files /dev/null and b/lib/adafruit_register/i2c_struct.mpy differ diff --git a/lib/adafruit_register/i2c_struct_array.mpy b/lib/adafruit_register/i2c_struct_array.mpy new file mode 100644 index 0000000..3db663d Binary files /dev/null and b/lib/adafruit_register/i2c_struct_array.mpy differ diff --git a/lib/adafruit_register/register_accessor.mpy b/lib/adafruit_register/register_accessor.mpy new file mode 100644 index 0000000..6deb546 Binary files /dev/null and b/lib/adafruit_register/register_accessor.mpy differ diff --git a/lib/adafruit_register/register_bit.mpy b/lib/adafruit_register/register_bit.mpy new file mode 100644 index 0000000..2981ccf Binary files /dev/null and b/lib/adafruit_register/register_bit.mpy differ diff --git a/lib/adafruit_register/register_bits.mpy b/lib/adafruit_register/register_bits.mpy new file mode 100644 index 0000000..9aa3f61 Binary files /dev/null and b/lib/adafruit_register/register_bits.mpy differ diff --git a/lib/car_os/__init__.py b/lib/car_os/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/lib/car_os/enums.py b/lib/car_os/enums.py new file mode 100644 index 0000000..132f85d --- /dev/null +++ b/lib/car_os/enums.py @@ -0,0 +1,4 @@ + +class SensorClass: + VEHICLE = 0xf1 + LIDAR_2D = 0x23 \ No newline at end of file diff --git a/lib/car_os/gps.py b/lib/car_os/gps.py new file mode 100644 index 0000000..8244477 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/lib/car_os/imu.py b/lib/car_os/imu.py new file mode 100644 index 0000000..3b03e7a --- /dev/null +++ b/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 diff --git a/lib/car_os/lidar_2d.py b/lib/car_os/lidar_2d.py new file mode 100644 index 0000000..1f77c24 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/lib/car_os/network_handler.py b/lib/car_os/network_handler.py new file mode 100644 index 0000000..6938e49 --- /dev/null +++ b/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 diff --git a/lib/car_os/pps.py b/lib/car_os/pps.py new file mode 100644 index 0000000..a4f5ed2 --- /dev/null +++ b/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]) \ No newline at end of file diff --git a/lib/car_os/timer.py b/lib/car_os/timer.py new file mode 100644 index 0000000..bed7600 --- /dev/null +++ b/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]) \ No newline at end of file diff --git a/lidar_code.py b/lidar_code.py new file mode 100644 index 0000000..8eabf5a --- /dev/null +++ b/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)) \ No newline at end of file