From 2892dc05c8585ba316c9e5ac49744d75c9eced78 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 20 Sep 2025 16:20:22 -0400 Subject: [PATCH] mapd: use SubMaster polling to validate GPS status (#1268) --- sunnypilot/mapd/live_map_data/base_map_data.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/sunnypilot/mapd/live_map_data/base_map_data.py b/sunnypilot/mapd/live_map_data/base_map_data.py index 536d7720b7..6c7679420a 100644 --- a/sunnypilot/mapd/live_map_data/base_map_data.py +++ b/sunnypilot/mapd/live_map_data/base_map_data.py @@ -4,12 +4,12 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ -import time from abc import abstractmethod, ABC from cereal import messaging from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot.navd.helpers import Coordinate, coordinate_from_param @@ -18,7 +18,9 @@ class BaseMapData(ABC): self.params = Params() self.gps_location_service = get_gps_location_service(self.params) - self.sm = messaging.SubMaster(['livePose', 'carControl'] + [self.gps_location_service]) + gps_packets = [self.gps_location_service] + self.sm = messaging.SubMaster(['livePose'] + gps_packets, ignore_alive=gps_packets, ignore_avg_freq=gps_packets, + ignore_valid=gps_packets, poll='livePose') self.pm = messaging.PubMaster(['liveMapDataSP']) self.last_position = coordinate_from_param("LastGPSPosition", self.params) @@ -44,20 +46,22 @@ class BaseMapData(ABC): gps = self.sm[self.gps_location_service] # ignore the message if the fix is invalid - gps_ok = self.sm.updated[self.gps_location_service] or (time.monotonic() - self.sm.logMonoTime[self.gps_location_service] / 1e9) > 2.0 + gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_MDL < 2.0 if not gps_ok and self.sm['livePose'].inputsOK: - return None + return # livePose has these data, but aren't on cereal self.last_position = Coordinate(gps.latitude, gps.longitude) self.last_altitude = gps.altitude + return + def publish(self) -> None: speed_limit = self.get_current_speed_limit() next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance() mapd_sp_send = messaging.new_message('liveMapDataSP') - mapd_sp_send.valid = self.sm.all_checks(service_list=[self.gps_location_service, 'livePose']) + mapd_sp_send.valid = self.sm.all_checks(['livePose']) live_map_data = mapd_sp_send.liveMapDataSP live_map_data.speedLimitValid = bool(speed_limit > 0)