diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 029d16e59e..a42f754ec5 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -138,6 +138,8 @@ class Controls: def publish(self, CC, lac_log): CS = self.sm['carState'] + model_v2 = self.sm['modelV2'] + # Orientation and angle rates can be useful for carcontroller # Only calibrated (car) frame is relevant for the carcontroller @@ -158,8 +160,10 @@ class Controls: hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1 hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual - hudControl.rightLaneVisible = True - hudControl.leftLaneVisible = True + laneVisible = len(model_v2.laneLines) > 2 and len(model_v2.laneLineProbs) > 2 + hudControl.leftLaneVisible = bool(laneVisible and len(model_v2.laneLines[1].x) > 0 and model_v2.laneLineProbs[1] > 0.5) + hudControl.rightLaneVisible = bool(laneVisible and len(model_v2.laneLines[2].x) > 0 and model_v2.laneLineProbs[2] > 0.5) + if self.sm.valid['driverAssistance']: hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture