diff --git a/code.py b/code.py index 5be4f55..2ae4425 100644 --- a/code.py +++ b/code.py @@ -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] diff --git a/lib/car_os/imu.py b/lib/car_os/imu.py index 3b03e7a..2825745 100644 --- a/lib/car_os/imu.py +++ b/lib/car_os/imu.py @@ -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 diff --git a/lib/car_os/network_handler.py b/lib/car_os/network_handler.py index 6938e49..c9c1cfb 100644 --- a/lib/car_os/network_handler.py +++ b/lib/car_os/network_handler.py @@ -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) diff --git a/lib/car_os/pps.py b/lib/car_os/pps.py index a4f5ed2..ac8d696 100644 --- a/lib/car_os/pps.py +++ b/lib/car_os/pps.py @@ -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]) \ No newline at end of file + 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) \ No newline at end of file diff --git a/lib/car_os/timer.py b/lib/car_os/timer.py index bed7600..019627a 100644 --- a/lib/car_os/timer.py +++ b/lib/car_os/timer.py @@ -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]) \ No newline at end of file + def reset(self): + self._delta = 0 \ No newline at end of file