Restore car GPS fixes on the standard logging channel

openpilot/starpilot/gps/car_external.py

@@ -0,0 +1,45 @@
+"""Car GPS fallback on the existing ubloxd-owned standard logging channel."""
+import math
+
+from openpilot.cereal import messaging
+from openpilot.starpilot.gps.publisher import FIELDS
+from openpilot.starpilot.gps.source import GPS_MAX_AGE_NS, observation
+
+
+class CarGpsExternal:
+  def __init__(self):
+    self.native_ns = 0
+    self.last_car_ns = 0
+    self.last_publish_ns = 0
+
+  def select(self, sm, now_ns, native=None):
+    """Keep native fixes first; repeat car fixes at the standard 10Hz cadence.
+
+    The source stamp is retained as the standard Event stamp. Repeated polling
+    cannot refresh an old CAN fix, and the source remains explicitly 'car'.
+    """
+    if native is not None:
+      self.native_ns = int(native.logMonoTime) if native.valid and native.gpsLocationExternal.hasFix else 0
+    if 0 < self.native_ns <= now_ns <= self.native_ns + GPS_MAX_AGE_NS:
+      return native
+    fix = observation(sm, 'starpilotCarState', now_ns)
+    if fix is None:
+      return native
+    stamp, gps = fix
+    # Preserve the existing decoder's real metric motion and uncertainty fields.
+    numbers = (gps.altitude, gps.speed, gps.bearingDeg, gps.verticalAccuracy,
+               gps.bearingAccuracyDeg, gps.speedAccuracy, *gps.vNED)
+    if (len(gps.vNED) != 3 or not all(math.isfinite(v) for v in numbers) or
+        not 0 <= gps.speed <= 200 or not 0 <= gps.bearingDeg < 360 or
+        min(gps.verticalAccuracy, gps.bearingAccuracyDeg, gps.speedAccuracy) < 0):
+      return native
+    if stamp < self.last_car_ns or stamp == self.last_car_ns and now_ns - self.last_publish_ns < 100_000_000:
+      # Keep the standard channel alive without refreshing the source stamp.
+      return None
+    event = messaging.new_message('gpsLocationExternal', valid=True, logMonoTime=stamp)
+    event.gpsLocationExternal = {key: getattr(gps, key) for key in FIELDS}
+    event.gpsLocationExternal.source = 'car'
+    # The companion has no satellite-count field: zero means unknown.
+    event.gpsLocationExternal.satelliteCount = 0
+    self.last_car_ns, self.last_publish_ns = stamp, now_ns
+    return event

openpilot/starpilot/gps/tests/test_car_external.py

@@ -0,0 +1,141 @@
+"""Real serialized car fixes must reach the standard, singly owned GPS channel."""
+
+from types import SimpleNamespace
+from unittest.mock import patch
+
+import pytest
+
+from openpilot.cereal import messaging, log
+from openpilot.starpilot.gps.car_external import CarGpsExternal
+from openpilot.starpilot.gps.source import GPS_MAX_AGE_NS
+
+NOW = 100_000_000_000
+
+
+class Messages:
+  def __init__(self, *, stamp=NOW - 100_000_000):
+    self.event = messaging.new_message('starpilotCarState', valid=True, logMonoTime=NOW)
+    self.event.starpilotCarState.gps = {
+      'sourceMonoTime': stamp,
+      'latitude': 41.0,
+      'longitude': -90.0,
+      'altitude': 200.0,
+      'speed': 12.0,
+      'bearingDeg': 90.0,
+      'horizontalAccuracy': 5.0,
+      'verticalAccuracy': 10.0,
+      'bearingAccuracyDeg': 10.0,
+      'speedAccuracy': 0.5,
+      'unixTimestampMillis': 1_700_000_000_000,
+      'hasFix': True,
+      'vNED': [0.0, 12.0, 0.0],
+    }
+    self.valid = {'starpilotCarState': True}
+    self.logMonoTime = {'starpilotCarState': NOW}
+
+  def __getitem__(self, service):
+    assert service == 'starpilotCarState'
+    return self.event.starpilotCarState
+
+  def update(self, timeout):
+    assert timeout == 0
+
+
+def native(*, fixed=False):
+  event = messaging.new_message('gpsLocationExternal', valid=True, logMonoTime=NOW)
+  event.gpsLocationExternal = {'source': 'ublox', 'hasFix': fixed, 'latitude': 40.0, 'longitude': -80.0, 'speed': 3.0}
+  return event
+
+
+def test_car_fallback_keeps_true_source_units_fix_time_and_standard_wire():
+  sm, owner = Messages(), CarGpsExternal()
+  event = owner.select(sm, NOW, native())
+  with log.Event.from_bytes(event.to_bytes()) as decoded:
+    assert decoded.valid and decoded.logMonoTime == sm['starpilotCarState'].gps.sourceMonoTime
+    gps = decoded.gpsLocationExternal
+    assert str(gps.source) == 'car' and gps.hasFix and gps.satelliteCount == 0
+    for name in (
+      'latitude',
+      'longitude',
+      'altitude',
+      'speed',
+      'bearingDeg',
+      'horizontalAccuracy',
+      'verticalAccuracy',
+      'bearingAccuracyDeg',
+      'speedAccuracy',
+      'unixTimestampMillis',
+    ):
+      assert getattr(gps, name) == getattr(sm['starpilotCarState'].gps, name)
+    assert list(gps.vNED) == [0.0, 12.0, 0.0]
+  assert owner.select(sm, NOW + 1) is None
+  assert owner.select(sm, NOW + 1, native()) is None
+  repeated = owner.select(sm, NOW + 100_000_000)
+  assert repeated.logMonoTime == event.logMonoTime
+  # Polling cannot rejuvenate the old observation.
+  assert owner.select(sm, NOW + GPS_MAX_AGE_NS) is None
+  assert not owner.select(sm, NOW + GPS_MAX_AGE_NS, native()).gpsLocationExternal.hasFix
+
+
+def test_native_fix_priority_loss_recovery_and_stale_native_fallback():
+  sm, owner, good = Messages(), CarGpsExternal(), native(fixed=True)
+  assert owner.select(sm, NOW, good) is good
+  assert owner.select(sm, NOW + 1) is None
+  assert str(owner.select(sm, NOW + 1, native()).gpsLocationExternal.source) == 'car'
+  assert owner.select(sm, NOW + 2, good) is good
+  sm.event.starpilotCarState.gps.sourceMonoTime = NOW + GPS_MAX_AGE_NS
+  sm.logMonoTime['starpilotCarState'] = NOW + GPS_MAX_AGE_NS
+  assert str(owner.select(sm, NOW + GPS_MAX_AGE_NS + 1).gpsLocationExternal.source) == 'car'
+
+
+@pytest.mark.parametrize('fault', ['invalid', 'noFix', 'future', 'stale', 'accuracy', 'nanMotion', 'badVelocity'])
+def test_bad_car_fix_never_replaces_a_native_observation(fault):
+  sm, owner, absent = Messages(), CarGpsExternal(), native()
+  gps = sm.event.starpilotCarState.gps
+  if fault == 'invalid':
+    sm.valid['starpilotCarState'] = False
+  elif fault == 'noFix':
+    gps.hasFix = False
+  elif fault == 'future':
+    gps.sourceMonoTime = NOW + 1
+  elif fault == 'stale':
+    gps.sourceMonoTime = NOW - GPS_MAX_AGE_NS - 1
+  elif fault == 'accuracy':
+    gps.horizontalAccuracy = 500.0
+  elif fault == 'nanMotion':
+    gps.speed = float('nan')
+  else:
+    gps.vNED = [0.0, float('inf'), 0.0]
+  assert owner.select(sm, NOW, absent) is absent
+  assert owner.select(sm, NOW) is None
+
+
+def test_actual_ubloxd_loop_publishes_car_gps_even_without_ubx_bytes():
+  from openpilot.system.ubloxd import ubloxd
+
+  sm, sent, calls = Messages(), [], [0]
+
+  class StopLoop(Exception):
+    pass
+
+  def receive(_sock):
+    calls[0] += 1
+    if sent or calls[0] > 1:
+      raise StopLoop
+    return None
+
+  publisher = SimpleNamespace(send=lambda service, event: sent.append((service, event if isinstance(event, bytes) else event.to_bytes())))
+  with (
+    patch.object(ubloxd.messaging, 'SubMaster', return_value=sm),
+    patch.object(ubloxd.messaging, 'PubMaster', return_value=publisher) as pm,
+    patch.object(ubloxd.messaging, 'sub_sock'),
+    patch.object(ubloxd.messaging, 'recv_one', side_effect=receive),
+    patch.object(ubloxd, 'Params'),
+    patch.object(ubloxd.time, 'monotonic_ns', return_value=NOW),
+  ):
+    with pytest.raises(StopLoop):
+      ubloxd.main()
+  pm.assert_called_once_with(['ubloxGnss', 'gpsLocationExternal'])
+  assert len(sent) == 1 and sent[0][0] == 'gpsLocationExternal'
+  with log.Event.from_bytes(sent[0][1]) as event:
+    assert event.gpsLocationExternal.hasFix and str(event.gpsLocationExternal.source) == 'car'

openpilot/system/ubloxd/ubloxd.py

@@ -13,6 +13,7 @@ from openpilot.common.params import Params
 from openpilot.system.ubloxd.ubx import Ubx
 from openpilot.system.ubloxd.gps import Gps
 from openpilot.system.ubloxd.glonass import Glonass
+from openpilot.starpilot.gps.car_external import CarGpsExternal
 
 
 SECS_IN_MIN = 60
@@ -514,9 +515,19 @@ def main():
   sock = messaging.sub_sock('ubloxRaw', timeout=100, conflate=False)
   params = Params()
   last_position_save = float('-inf')
+  car_gps = messaging.SubMaster(['starpilotCarState'])
+  car_external = CarGpsExternal()
 
   while True:
     msg = messaging.recv_one(sock)
+    car_gps.update(0)
+    fallback = car_external.select(car_gps, time.monotonic_ns())
+    if fallback is not None:
+      payload = fallback.to_bytes()
+      pm.send('gpsLocationExternal', payload)
+      if time.monotonic() - last_position_save > 60:
+        params.put('LastGPSPosition', payload)
+        last_position_save = time.monotonic()
     if msg is None:
       continue
 
@@ -531,6 +542,10 @@ def main():
       if not res:
         continue
       service, dat = res
+      if service == 'gpsLocationExternal':
+        dat = car_external.select(car_gps, time.monotonic_ns(), dat)
+        if dat is None:
+          continue
       pm.send(service, dat)
 
       if service == 'gpsLocationExternal' and dat.gpsLocationExternal.hasFix and time.monotonic() - last_position_save > 60:

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "2b69cabc7e00554545e4aac4d826a9f8b3add4ad0eba1fb5abaab576f6fd6185", "schema_version": 1}
+{"protected_inventory_sha256": "6280d986d0926d36f2778b58bb7b4aeca599470e332812731d900181d6906431", "schema_version": 1}