diff --git a/common/gps.py b/common/gps.py new file mode 100644 index 000000000..6f96d72e9 --- /dev/null +++ b/common/gps.py @@ -0,0 +1,8 @@ +from openpilot.common.params import Params + + +def get_gps_location_service(params: Params) -> str: + if params.get_bool("UbloxAvailable"): + return "gpsLocationExternal" + else: + return "gpsLocation" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c30b458ef..fac978b97 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -17,6 +17,7 @@ from openpilot.common.numpy_fast import clip from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL from openpilot.common.swaglog import cloudlog +from openpilot.common.gps import get_gps_location_service from opendbc.car.car_helpers import get_car_interface from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert @@ -79,10 +80,7 @@ class Controls: # Setup sockets self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents']) - if self.params.get_bool("UbloxAvailable"): - self.gps_location_service = "gpsLocationExternal" - else: - self.gps_location_service = "gpsLocation" + self.gps_location_service = get_gps_location_service(self.params) self.gps_packets = [self.gps_location_service] self.sensor_packets = ["accelerometer", "gyroscope"] self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] diff --git a/system/timed.py b/system/timed.py index 6c85bf2a2..31d0d0658 100755 --- a/system/timed.py +++ b/system/timed.py @@ -7,6 +7,8 @@ from typing import NoReturn import cereal.messaging as messaging from openpilot.common.time import system_time_valid from openpilot.common.swaglog import cloudlog +from openpilot.common.params import Params +from openpilot.common.gps import get_gps_location_service def set_time(new_time): @@ -31,8 +33,11 @@ def main() -> NoReturn: AGNOS will also use NTP to update the time. """ + params = Params() + gps_location_service = get_gps_location_service(params) + pm = messaging.PubMaster(['clocks']) - sm = messaging.SubMaster(['liveLocationKalman']) + sm = messaging.SubMaster([gps_location_service]) while True: sm.update(1000) @@ -41,13 +46,13 @@ def main() -> NoReturn: msg.clocks.wallTimeNanos = time.time_ns() pm.send('clocks', msg) - llk = sm['liveLocationKalman'] - if not llk.gpsOK or (time.monotonic() - sm.logMonoTime['liveLocationKalman']/1e9) > 0.2: + gps = sm[gps_location_service] + if not sm.updated[gps_location_service] or (time.monotonic() - sm.logMonoTime[gps_location_service] / 1e9) > 2.0: continue # set time # TODO: account for unixTimesatmpMillis being a (usually short) time in the past - gps_time = datetime.datetime.fromtimestamp(llk.unixTimestampMillis / 1000.) + gps_time = datetime.datetime.fromtimestamp(gps.unixTimestampMillis / 1000.) set_time(gps_time) time.sleep(10)