Sensor network stuff works now
This commit is contained in:
@@ -12,19 +12,34 @@ from car_os.imu import IMU
|
||||
from car_os.timer import Timer
|
||||
from car_os.enums import SensorClass
|
||||
|
||||
def wait(start):
|
||||
delta = time.monotonic_ns() - start
|
||||
print("delta:", delta)
|
||||
while 0 <= delta < 1000:
|
||||
delta = time.monotonic_ns() - start
|
||||
print("delta:", delta)
|
||||
continue
|
||||
return
|
||||
|
||||
def main():
|
||||
heartbeat_timer = Timer(15)
|
||||
|
||||
network = NetworkHandler(SensorClass.VEHICLE, 256, 0.001, 5000, 0.1, 5001)
|
||||
network = NetworkHandler(SensorClass.VEHICLE, 256, 0.1, 5000, 0.001, 5001)
|
||||
network.initialize()
|
||||
network_timer = Timer(0.5)
|
||||
network_timer = Timer(1)
|
||||
network_reset_timer = Timer(600)
|
||||
|
||||
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()
|
||||
delta = pps.update()
|
||||
if delta > 60:
|
||||
continue
|
||||
heartbeat_timer.update(delta)
|
||||
network_timer.update(delta)
|
||||
network_reset_timer.update(delta)
|
||||
gps.update(pps.utc)
|
||||
imu.update(pps.utc)
|
||||
|
||||
@@ -37,10 +52,10 @@ def main():
|
||||
if gps.time_updated:
|
||||
pps.update_from_gps(gps)
|
||||
|
||||
if network_timer.check_timer(pps.utc):
|
||||
network.update(pps.utc, network_timer)
|
||||
if network_timer.check_timer():
|
||||
network.update(pps.utc, network_reset_timer)
|
||||
|
||||
#if heartbeat_timer.check_timer(pps.utc):
|
||||
#if heartbeat_timer.check_timer():
|
||||
# message = bytearray(b'\xd4\x53\x6e\x4e\x00\x00\x00')
|
||||
# message[4] = pps.utc[0]
|
||||
# message[5] = pps.utc[1]
|
||||
|
||||
+3
-3
@@ -102,9 +102,9 @@ class IMU:
|
||||
|
||||
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._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
|
||||
|
||||
|
||||
@@ -12,7 +12,6 @@ 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
|
||||
@@ -45,19 +44,20 @@ class NetworkHandler:
|
||||
break
|
||||
time.sleep(random.randint(1, 10) / 10)
|
||||
|
||||
def update(self, utc, timer):
|
||||
def update(self, utc, reset_timer):
|
||||
if self._receive_udp():
|
||||
timer.reset(utc)
|
||||
timer.set_interval(10)
|
||||
if not timer.check_timer(utc):
|
||||
reset_timer.reset()
|
||||
return
|
||||
if not reset_timer.check_timer():
|
||||
return
|
||||
if self._send_socket:
|
||||
self._send_socket.close()
|
||||
self._send_socket = None
|
||||
self._send_addr = None
|
||||
self._send_addr = None
|
||||
|
||||
def send(self, message):
|
||||
if self._send_addr is None or self._send_socket is None:
|
||||
print(self._send_addr, self._send_socket)
|
||||
return
|
||||
buffer = bytearray(message)
|
||||
|
||||
|
||||
+15
-2
@@ -13,15 +13,24 @@ class PPS:
|
||||
self.gps_set = False
|
||||
self._pps_offset = [0, 0, 0, 0]
|
||||
self._utc = [0, 0, 0]
|
||||
self._previous_timestamp = 0
|
||||
self._previous_utc_h = 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
|
||||
self.previous_value = self.current_value
|
||||
current_timestamp = self.utc_timestamp
|
||||
if self._previous_utc_h == 23 and self.utc[0] == 0:
|
||||
current_timestamp += 86400
|
||||
delta = current_timestamp - self._previous_timestamp
|
||||
self._previous_timestamp = current_timestamp
|
||||
self._previous_utc_h = self.utc[0]
|
||||
return max(0, delta)
|
||||
|
||||
def update_from_gps(self, gps):
|
||||
self._utc[0] = gps.time[0]
|
||||
@@ -46,4 +55,8 @@ class PPS:
|
||||
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])
|
||||
return (h, m, s, self._pps_offset[3])
|
||||
|
||||
@property
|
||||
def utc_timestamp(self):
|
||||
return self.utc[0] * 3600 + self.utc[1] * 60 + self.utc[2] + (self.utc[3] / 1_000_000_000)
|
||||
+9
-15
@@ -3,25 +3,19 @@
|
||||
class Timer:
|
||||
def __init__(self, interval_s):
|
||||
self._interval_s = interval_s
|
||||
self._last_time = 0
|
||||
self._last_utc = (0,0,0)
|
||||
self._delta = 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])
|
||||
def update(self, delta):
|
||||
self._delta += delta
|
||||
|
||||
def check_timer(self):
|
||||
if self._delta >= self._interval_s:
|
||||
self._delta = 0
|
||||
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])
|
||||
def reset(self):
|
||||
self._delta = 0
|
||||
Reference in New Issue
Block a user