diff --git a/openpilot/starpilot/gps/car_external.py b/openpilot/starpilot/gps/car_external.py new file mode 100644 index 0000000000..268c20bc6d --- /dev/null +++ b/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 diff --git a/openpilot/starpilot/gps/tests/test_car_external.py b/openpilot/starpilot/gps/tests/test_car_external.py new file mode 100644 index 0000000000..b2f868052b --- /dev/null +++ b/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' diff --git a/openpilot/system/ubloxd/ubloxd.py b/openpilot/system/ubloxd/ubloxd.py old mode 100755 new mode 100644 index 3365e9c693..e609e65cf0 --- a/openpilot/system/ubloxd/ubloxd.py +++ b/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: diff --git a/prebuilt.json b/prebuilt.json index 65777dda67..085fade2d2 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "2b69cabc7e00554545e4aac4d826a9f8b3add4ad0eba1fb5abaab576f6fd6185", "schema_version": 1} +{"protected_inventory_sha256": "6280d986d0926d36f2778b58bb7b4aeca599470e332812731d900181d6906431", "schema_version": 1}