Fix pps clock when not connected to gps

code.py

@@ -6,7 +6,8 @@ import adafruit_gps
 import digitalio
 from car_os.network_handler import NetworkHandler
 import math
-from car_os.pps import PPS
+from car_os.pps_clock import PPSClock
+from car_os.system_clock import SystemClock
 from car_os.gps import GPS
 from car_os.imu import IMU
 from car_os.timer import Timer
@@ -27,40 +28,35 @@ def main():
     network = NetworkHandler(SensorClass.VEHICLE, 256, 0.1, 5000, 0.001, 5001)
     network.initialize()
     network_timer = Timer(1)
-    network_reset_timer = Timer(600)
-    prind(board.__dict__)
 
-    pps = PPS(board.GP22)
+    pps_clock = PPSClock(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:
-        delta = pps.update()
+        delta = pps_clock.update()
         if delta > 60:
             continue
         heartbeat_timer.update(delta)
         network_timer.update(delta)
-        network_reset_timer.update(delta)
+        gps.update(pps_clock.utc_time)
-        gps.update(pps.utc)
+        imu.update(pps_clock.utc_time)
-        imu.update(pps.utc)
         
         if network_timer.check_timer():
-            network.update(pps.utc, network_reset_timer)
+            network.update()
             
         if not gps.has_fix:
-            print(time.time())
+            pps_clock.update_gps_fixed(gps.has_fix)
-            print("waiting for gps fix")
-            print()
             continue
         
         if gps.time_updated:
-            pps.update_from_gps(gps)
+            pps_clock.update_utc_from_gps(gps)
         
         #if heartbeat_timer.check_timer():
         #    message = bytearray(b'\xd4\x53\x6e\x4e\x00\x00\x00')
-        #    message[4] = pps.utc[0]
+        #    message[4] = pps_clock.utc[0]
-        #    message[5] = pps.utc[1]
+        #    message[5] = pps_clock.utc[1]
-        #    message[6] = pps.utc[2]
+        #    message[6] = pps_clock.utc[2]
         #    message += b'\x00\x00'
         #    network.send(message)
         

lib/car_os/pps.py

@@ -1,62 +0,0 @@
-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]
-        self._previous_timestamp = 0
-        self._previous_utc_h = 0
-        
-    def update(self):
-        current_time = time.monotonic_ns()
-        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]
-        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])
-    
-    @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/pps_clock.py

@@ -0,0 +1,66 @@
+import digitalio
+import time
+
+from car_os.system_clock import SystemClock
+
+    
+class PPSClock:
+    def __init__(self, pin):
+        self._system_clock = SystemClock()
+        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._pps_timestamp = 0
+        self._pps_time_ns = time.monotonic_ns()
+        self.gps_set = False
+        self._pps_offset = [0, 0, 0, 0]
+        self._gps_utc = [0, 0, 0]
+        self._system_time = (0, 0, 0, 0)
+        self._previous_timestamp = 0
+        self._previous_utc_h = 0
+        
+    def update(self):
+        self._system_clock.update()
+        self.current_value = self.gpio.value
+        if not self.current_value and self.previous_value:
+            self._pps_timestamp = self._system_clock.utc_timestamp
+            self._gps_utc[2] = self._gps_utc[2] + 1
+            self._pps_time_ns = time.monotonic_ns()
+        self.previous_value = self.current_value
+        current_timestamp = self.utc_timestamp
+        if self._previous_utc_h == 23 and self.utc_time[0] == 0:
+            current_timestamp += 86400
+        delta = current_timestamp - self._previous_timestamp
+        self._previous_timestamp = current_timestamp
+        self._previous_utc_h = self.utc_time[0]
+        return max(0, delta)
+    
+    def update_utc_from_gps(self, gps):
+        self._gps_utc[0] = gps.time[0]
+        self._gps_utc[1] = gps.time[1]
+        self._gps_utc[2] = gps.time[2]
+        gps.time_updated = False
+        self.gps_set = True
+    
+    def update_gps_fixed(self, has_fix):
+        self.gps_set = has_fix
+    
+    @property
+    def utc_time(self):
+        s = self._system_clock.utc_timestamp - self._pps_timestamp
+        ns = s % 1
+        s = int(s - ns)
+        m, s = divmod(self._gps_utc[2] + s, 60)
+        h, m = divmod(self._gps_utc[1] + m, 60)
+        _, h = divmod(self._gps_utc[0] + h, 24)
+        return (h, m, s, ns*1e9)
+    
+    @property
+    def utc_timestamp(self):
+        return self.utc_time[0] * 3600 + self.utc_time[1] * 60 + self.utc_time[2] + (self.utc_time[3] / 1_000_000_000)
+
+    @property
+    def update_time_ns(self):
+        return self._pps_time_ns

lib/car_os/system_clock.py

@@ -0,0 +1,47 @@
+import time
+
+class SystemClock:
+    def __init__(self):
+        self._update_time_ns = time.monotonic_ns()
+        self._time = (0, 0, 0, 0)
+        self._previous_timestamp = 0
+        self._previous_utc_h = 0
+
+    def update(self):
+        self._update_time()
+        current_timestamp = self.utc_timestamp
+        if self._previous_utc_h == 23 and self.utc_time[0] == 0:
+            current_timestamp += 86400
+        delta = current_timestamp - self._previous_timestamp
+        self._previous_timestamp = current_timestamp
+        self._previous_utc_h = self.utc_time[0]
+        return delta
+
+    def _update_time(self):
+        current_time = time.monotonic_ns()
+        time_diff = current_time - self._update_time_ns
+        self._update_time_ns = current_time
+        s, ns = divmod(self._time[3]+time_diff, 1e9)
+        m, s = divmod(self._time[2]+s, 60)
+        h, m = divmod(self._time[1]+m, 60)
+        _, h = divmod(self._time[0]+h, 24)
+        self._time = (h, m, s, ns)
+
+    def update_from_pps_clock(self, pps_time, pps_ref_monotonic):
+        self._time = pps_time[:]
+        self._update_time_ns = pps_ref_monotonic
+        self._update_time()
+        self._previous_timestamp = self.utc_timestamp
+
+    @property
+    def utc_time(self):
+        self._update_time()
+        return self._time
+
+    @property
+    def utc_timestamp(self):
+        return self._time[0] * 3600 + self._time[1] * 60 + self._time[2] + (self._time[3] / 1_000_000_000)
+
+    @property
+    def update_time_ns(self):
+        return self._update_time_ns