Split sensor types into files
This commit is contained in:
@@ -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