Merge remote-tracking branch 'sunnypilot/sunnypilot/master' into feature/slc

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)