From cedb2cfaa140b85597d57bfb901686148ff03d16 Mon Sep 17 00:00:00 2001 From: Sean Smith Date: Sun, 2 Aug 2026 14:25:21 -0600 Subject: [PATCH] Fix pps clock when not connected to gps --- code.py | 30 ++++++++--------- lib/car_os/pps.py | 62 ----------------------------------- lib/car_os/pps_clock.py | 66 ++++++++++++++++++++++++++++++++++++++ lib/car_os/system_clock.py | 47 +++++++++++++++++++++++++++ 4 files changed, 126 insertions(+), 79 deletions(-) delete mode 100644 lib/car_os/pps.py create mode 100644 lib/car_os/pps_clock.py create mode 100644 lib/car_os/system_clock.py diff --git a/code.py b/code.py index 92e61a4..ff92060 100644 --- a/code.py +++ b/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.utc) - imu.update(pps.utc) + gps.update(pps_clock.utc_time) + imu.update(pps_clock.utc_time) if network_timer.check_timer(): - network.update(pps.utc, network_reset_timer) + network.update() if not gps.has_fix: - print(time.time()) - print("waiting for gps fix") - print() + pps_clock.update_gps_fixed(gps.has_fix) 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[5] = pps.utc[1] - # message[6] = pps.utc[2] + # message[4] = pps_clock.utc[0] + # message[5] = pps_clock.utc[1] + # message[6] = pps_clock.utc[2] # message += b'\x00\x00' # network.send(message) diff --git a/lib/car_os/pps.py b/lib/car_os/pps.py deleted file mode 100644 index ac8d696..0000000 --- a/lib/car_os/pps.py +++ /dev/null @@ -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) \ No newline at end of file diff --git a/lib/car_os/pps_clock.py b/lib/car_os/pps_clock.py new file mode 100644 index 0000000..4c5bb39 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/lib/car_os/system_clock.py b/lib/car_os/system_clock.py new file mode 100644 index 0000000..954812c --- /dev/null +++ b/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