use gps data directly

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:

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

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.