Split sensor types into files
This commit is contained in:
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,4 @@
|
||||
|
||||
class SensorClass:
|
||||
VEHICLE = 0xf1
|
||||
LIDAR_2D = 0x23
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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])
|
||||
@@ -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])
|
||||
Reference in New Issue
Block a user