diff --git a/cereal b/cereal index 344c5c6fdb..a85bf58bb5 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 344c5c6fdb101526b2f78cb3982a6b4179be0f71 +Subproject commit a85bf58bb595ad2507186e81d4b2cf2b975a5140 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a09ba171df..882c5c82e5 100755 --- a/selfdrive/controls/controlsd.py +++ b/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) diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 4b54e41867..2d6c0be68f 100644 --- a/selfdrive/controls/lib/events.py +++ b/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: { diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index f401226dbe..e45fb62696 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/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)) diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 60f886e307..a41d0db3af 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/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" diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 15a6c4b2ae..0ed457a376 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/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,