diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 882c5c82e5..49457c52f8 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -52,9 +52,9 @@ class Controls: self.sm = sm if self.sm is None: - self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', 'frontFrame', + self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', 'ubloxRaw', 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman', - 'ubloxRaw']) + 'frame', 'frontFrame']) self.can_sock = can_sock if can_sock is None: @@ -211,12 +211,6 @@ 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.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) if not self.sm['pathPlan'].paramsValid: self.events.add(EventName.vehicleModelInvalid) if not self.sm['liveLocationKalman'].posenetOK: @@ -231,11 +225,19 @@ class Controls: self.events.add(EventName.relayMalfunction) if self.sm['plan'].fcw: self.events.add(EventName.fcw) - if not self.sm.alive['frontFrame'] and (self.sm.frame > 5 / DT_CTRL) and not SIMULATION: - self.events.add(EventName.cameraMalfunction) - if self.sm['model'].frameDropPerc > 20 and not SIMULATION: - self.events.add(EventName.modeldLagging) + # TODO: fix simulator + if not SIMULATION: + if not NOSENSOR: + if not self.sm.alive['ubloxRaw'] and (self.sm.frame > 10. / DT_CTRL): + 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 + self.events.add(EventName.noGps) + if not self.sm.all_alive(['frame', 'frontFrame']) and (self.sm.frame > 5 / DT_CTRL): + self.events.add(EventName.cameraMalfunction) + if self.sm['model'].frameDropPerc > 20: + self.events.add(EventName.modeldLagging) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \ diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index e45fb62696..a8e6561c07 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -110,7 +110,7 @@ class Plant(): self.rate = rate if not Plant.messaging_initialized: - Plant.pm = messaging.PubMaster(['frontFrame', 'ubloxRaw']) + Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw']) Plant.logcan = messaging.pub_sock('can') Plant.sendcan = messaging.sub_sock('sendcan') Plant.model = messaging.pub_sock('model') diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 0ed457a376..e98879ff0e 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": [], "ubloxRaw": [], + "model": [], "frontFrame": [], "frame": [], "ubloxRaw": [], }, ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], init_callback=fingerprint, diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 5b714069a8..cdaee02036 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -431,8 +431,9 @@ void ui_draw(UIState *s) { s->scene.viz_rect.w -= sbr_w; } - const bool draw_vision = s->started && s->status != STATUS_OFFROAD && - s->active_app == cereal::UiLayoutState::App::NONE && s->vipc_client->connected; + const bool draw_alerts = s->started && s->status != STATUS_OFFROAD && + s->active_app == cereal::UiLayoutState::App::NONE; + const bool draw_vision = draw_alerts && s->vipc_client->connected; // GL drawing functions ui_draw_background(s); @@ -450,7 +451,7 @@ void ui_draw(UIState *s) { ui_draw_vision(s); } - if (draw_vision && s->scene.alert_size != cereal::ControlsState::AlertSize::NONE) { + if (draw_alerts && s->scene.alert_size != cereal::ControlsState::AlertSize::NONE) { ui_draw_vision_alert(s); } nvgEndFrame(s->vg); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index bd6b453dde..9e8b8437b1 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -254,7 +254,7 @@ void ui_update(UIState *s) { s->scene.alert_size = cereal::ControlsState::AlertSize::NONE; } - // Handle controls/fcamera timeout + // Handle controls timeout if (s->started && !s->scene.frontview && ((s->sm)->frame - s->started_frame) > 10*UI_FREQ) { if ((s->sm)->rcv_frame("controlsState") < s->started_frame) { // car is started, but controlsState hasn't been seen at all @@ -263,8 +263,7 @@ void ui_update(UIState *s) { s->scene.alert_size = cereal::ControlsState::AlertSize::MID; } else if (((s->sm)->frame - (s->sm)->rcv_frame("controlsState")) > 5*UI_FREQ) { // car is started, but controls is lagging or died - if (s->scene.alert_text2 != "Controls Unresponsive" && - s->scene.alert_text1 != "Camera Malfunction") { + if (s->scene.alert_text2 != "Controls Unresponsive") { s->sound->play(AudibleAlert::CHIME_WARNING_REPEAT); LOGE("Controls unresponsive"); } @@ -274,17 +273,5 @@ void ui_update(UIState *s) { s->scene.alert_size = cereal::ControlsState::AlertSize::FULL; s->status = STATUS_ALERT; } - - const uint64_t frame_pkt = (s->sm)->rcv_frame("frame"); - const uint64_t frame_delayed = (s->sm)->frame - frame_pkt; - const uint64_t since_started = (s->sm)->frame - s->started_frame; - if ((frame_pkt > s->started_frame || since_started > 15*UI_FREQ) && frame_delayed > 5*UI_FREQ) { - // controls is fine, but rear camera is lagging or died - s->scene.alert_text1 = "Camera Malfunction"; - s->scene.alert_text2 = "Contact Support"; - s->scene.alert_size = cereal::ControlsState::AlertSize::FULL; - s->status = STATUS_DISENGAGED; - s->sound->stop(); - } } }