GPS malfunction alert (#19756)

* GPS malfunction alert

* fix process replay

* fix long tests

* fix long tests again

cereal

@@ -1 +1 @@
-Subproject commit 344c5c6fdb101526b2f78cb3982a6b4179be0f71
+Subproject commit a85bf58bb595ad2507186e81d4b2cf2b975a5140

selfdrive/controls/controlsd.py

@@ -53,7 +53,8 @@ class Controls:
     self.sm = sm
     if self.sm is None:
       self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', 'frontFrame',
-                                     'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])
+                                     'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman',
+                                     'ubloxRaw'])
 
     self.can_sock = can_sock
     if can_sock is None:
@@ -210,7 +211,9 @@ class Controls:
     if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
       if self.sm.frame > 5 / DT_CTRL:  # Give locationd some time to receive all the inputs
         self.events.add(EventName.sensorDataInvalid)
-    if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000):
+    if not self.sm.alive['ubloxRaw'] and (self.sm.frame > 10. / DT_CTRL) and not SIMULATION:
+      self.events.add(EventName.gpsMalfunction)
+    elif not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000):
       # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
       if not (SIMULATION or NOSENSOR):  # TODO: send GPS in carla
         self.events.add(EventName.noGps)

selfdrive/controls/lib/events.py

@@ -468,6 +468,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
     ET.PERMANENT: NormalPermanentAlert("Camera Malfunction", "Contact Support"),
   },
 
+  EventName.gpsMalfunction: {
+    ET.PERMANENT: NormalPermanentAlert("GPS Malfunction", "Contact Support"),
+  },
+
   # ********** events that affect controls state transitions **********
 
   EventName.pcmEnable: {

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -110,10 +110,10 @@ class Plant():
     self.rate = rate
 
     if not Plant.messaging_initialized:
+      Plant.pm = messaging.PubMaster(['frontFrame', 'ubloxRaw'])
       Plant.logcan = messaging.pub_sock('can')
       Plant.sendcan = messaging.sub_sock('sendcan')
       Plant.model = messaging.pub_sock('model')
-      Plant.front_frame = messaging.pub_sock('frontFrame')
       Plant.live_params = messaging.pub_sock('liveParameters')
       Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman')
       Plant.health = messaging.pub_sock('health')
@@ -163,7 +163,6 @@ class Plant():
   def close(self):
     Plant.logcan.close()
     Plant.model.close()
-    Plant.front_frame.close()
     Plant.live_params.close()
     Plant.live_location_kalman.close()
 
@@ -394,7 +393,6 @@ class Plant():
     if publish_model and self.frame % 5 == 0:
       md = messaging.new_message('model')
       cal = messaging.new_message('liveCalibration')
-      fp = messaging.new_message('frontFrame')
       md.model.frameId = 0
       for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
         x.points = [0.0]*50
@@ -426,7 +424,11 @@ class Plant():
       # fake values?
       Plant.model.send(md.to_bytes())
       Plant.cal.send(cal.to_bytes())
-      Plant.front_frame.send(fp.to_bytes())
+      for s in Plant.pm.sock.keys():
+        try:
+          Plant.pm.send(s, messaging.new_message(s))
+        except Exception:
+          Plant.pm.send(s, messaging.new_message(s, 1))
 
     Plant.logcan.send(can_list_to_can_capnp(can_msgs))
 

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -318,6 +318,7 @@ def setup_output():
 class LongitudinalControl(unittest.TestCase):
   @classmethod
   def setUpClass(cls):
+    os.environ['SIMULATION'] = "1"
     os.environ['SKIP_FW_QUERY'] = "1"
     os.environ['NO_CAN_TIMEOUT'] = "1"
 

selfdrive/test/process_replay/process_replay.py

@@ -222,7 +222,7 @@ CONFIGS = [
     pub_sub={
       "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
       "thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], "liveLocationKalman": [],
-      "model": [], "frontFrame": [],
+      "model": [], "frontFrame": [], "ubloxRaw": [],
     },
     ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"],
     init_callback=fingerprint,