Fix pps clock when not connected to gps

2026-08-02 14:25:21 -06:00
parent ca817909a7
commit cedb2cfaa1
4 changed files with 126 additions and 79 deletions
@@ -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