Sensor network stuff works now

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):
+        if network_timer.check_timer():
-            network.update(pps.utc, network_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]

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._acceleration = self._imu.acceleration[:]
-            self._linear_acceleration = self._imu.linear_acceleration
+            self._linear_acceleration = self._imu.linear_acceleration[:]
-            self._gravity = self._imu.gravity
+            self._gravity = self._imu.gravity[:]
             self._accel_update_time = utc
             self.accel_updated = True
     

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,11 +44,11 @@ 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)
+            reset_timer.reset()
-            timer.set_interval(10)
+            return
-        if not timer.check_timer(utc):
+        if not reset_timer.check_timer():
             return
         if self._send_socket:
             self._send_socket.close()
@@ -58,6 +57,7 @@ class NetworkHandler:
 
     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)
 

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]
@@ -47,3 +56,7 @@ class PPS:
         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])
+    
+    @property
+    def utc_timestamp(self):
+        return self.utc[0] * 3600 + self.utc[1] * 60 + self.utc[2] + (self.utc[3] / 1_000_000_000)

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._delta = 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):
+    def update(self, delta):
-        ns = utc[3] * 0.000000001
+        self._delta += delta
-        timestamp = utc[0] * 3600 + utc[1] * 60 + utc[2] + ns
+    
-        shifted_timestamp = timestamp
+    def check_timer(self):
-        if self._last_utc[0] == 23 and utc[0] == 0:
+        if self._delta >= self._interval_s:
-            shifted_timestamp += 24*3600
+            self._delta = 0
-        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):
+    def reset(self):
-        ns = utc[3] * 0.000000001
+        self._delta = 0
-        self._last_time = utc[0] * 3600 + utc[1] * 60 + utc[2] + ns
-        self._last_utc = (utc[0], utc[1], utc[2])