6FPS/car-os-sensor
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix pps clock when not connected to gps

Browse Source
This commit is contained in:
ssmith
2026-08-02 14:25:21 -06:00
parent ca817909a7
commit cedb2cfaa1
4 changed files with 126 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
code.py
+13 -17
View File
@@ -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)
lib/car_os/pps.py
-62
View File
@@ -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
+66
View File
@@ -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
+47
View File
@@ -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