From 01f4d23a509708f3be35d82a6b192ed4fc5e04ec Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 7 Jun 2025 11:45:11 -0400 Subject: [PATCH] use gps data directly --- selfdrive/controls/plannerd.py | 5 ++++- .../speed_limit_resolver.py | 22 +++++++++++++------ .../tests/test_speed_limit_resolver.py | 7 ++++-- 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index e9892368fb..e78a7ffdb1 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 from cereal import car +from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params from openpilot.common.realtime import Priority, config_realtime_process from openpilot.common.swaglog import cloudlog @@ -16,11 +17,13 @@ def main(): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("plannerd got CarParams: %s", CP.brand) + gps_location_service = get_gps_location_service(params) + ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState', - "liveMapDataSP", "carStateSP"], + "liveMapDataSP", "carStateSP", gps_location_service], poll='modelV2') while True: diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py index 6af23ef02c..e3d66827de 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py @@ -2,7 +2,8 @@ import time import numpy as np from cereal import messaging, custom - +from openpilot.common.gps import get_gps_location_service +from openpilot.common.params import Params from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import debug @@ -15,6 +16,7 @@ class SpeedLimitResolver: _current_speed_limit: float def __init__(self, policy: Policy): + self._gps_location_service = get_gps_location_service(Params()) self._limit_solutions = {} self._distance_solutions = {} @@ -58,10 +60,13 @@ class SpeedLimitResolver: # Load limits from map_data if sm.updated['liveMapDataSP']: self._reset_limit_sources(Source.map_data) - self._process_map_data(sm['liveMapDataSP']) + self._process_map_data(sm) - def _process_map_data(self, map_data: custom.LiveMapDataSP) -> None: - gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3 + def _process_map_data(self, sm: messaging.SubMaster) -> None: + gps_data = sm[self._gps_location_service] + map_data = sm['liveMapDataSP'] + + gps_fix_age = time.time() - gps_data.unixTimestampMillis * 1e-3 if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE: debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}') return @@ -69,10 +74,13 @@ class SpeedLimitResolver: speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0. next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0. - self._calculate_map_data_limits(speed_limit, next_speed_limit, map_data) + self._calculate_map_data_limits(sm, speed_limit, next_speed_limit) - def _calculate_map_data_limits(self, speed_limit: float, next_speed_limit: float, map_data: custom.LiveMapDataSP) -> None: - distance_since_fix = self._v_ego * (time.time() - map_data.lastGpsTimestamp * 1e-3) + def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None: + gps_data = sm[self._gps_location_service] + map_data = sm['liveMapDataSP'] + + distance_since_fix = self._v_ego * (time.time() - gps_data.unixTimestampMillis * 1e-3) distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix) self._limit_solutions[Source.map_data] = speed_limit diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py index f2a1455f7b..0dc5dd585c 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py @@ -36,13 +36,16 @@ def setup_sm_mock(mocker: MockerFixture): 'speedLimitAhead': 0., 'speedLimitAheadValid': 0., 'speedLimitAheadDistance': 0., - 'lastGpsTimestamp': time.time() * 1e3, + }, mocker) + gps_data = create_mock({ + 'unixTimestampMillis': time.time() * 1e3, }, mocker) sm_mock = mocker.MagicMock() sm_mock.__getitem__.side_effect = lambda key: { 'carState': car_state, 'liveMapDataSP': live_map_data, 'carStateSP': car_state_sp, + 'gpsLocation': gps_data, }[key] return sm_mock @@ -121,7 +124,7 @@ class TestSpeedLimitResolverValidation: def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture): resolver = resolver_class(policy) sm_mock = mocker.MagicMock() - sm_mock['liveMapDataSP'].lastGpsTimestamp = (time.time() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3 + sm_mock['gpsLocation'].unixTimestampMillis = (time.time() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3 resolver._get_from_map_data(sm_mock) assert resolver._limit_solutions[Source.map_data] == 0. assert resolver._distance_solutions[Source.map_data] == 0.