Improve VW HUD with laneless and LDW

selfdrive/car/volkswagen/carcontroller.py

@@ -1,4 +1,5 @@
 from cereal import car
+from common.params import Params
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.volkswagen import volkswagencan
 from selfdrive.car.volkswagen.values import DBC, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams
@@ -18,9 +19,11 @@ class CarController():
     self.graMsgStartFramePrev = 0
     self.graMsgBusCounterPrev = 0
 
+    self.use_lanelines = Params().get('EndToEndToggle') != b'1'
+
     self.steer_rate_limited = False
 
-  def update(self, enabled, CS, frame, actuators, visual_alert, left_lane_visible, right_lane_visible):
+  def update(self, enabled, CS, frame, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart):
     """ Controls thread """
 
     P = CarControllerParams
@@ -110,18 +113,20 @@ class CarController():
     # filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz.
 
     if frame % P.LDW_STEP == 0:
-      hcaEnabled = True if enabled and not CS.out.standstill else False
-
       if visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired:
         hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"]
       else:
         hud_alert = MQB_LDW_MESSAGES["none"]
 
-      can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, hcaEnabled,
+      left_lane_visible = (left_lane_visible and not CS.out.standstill) if self.use_lanelines else True
+      right_lane_visible = (right_lane_visible and not CS.out.standstill) if self.use_lanelines else True
+
+      can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, enabled,
                                                             CS.out.steeringPressed, hud_alert, left_lane_visible,
                                                             right_lane_visible, CS.ldw_lane_warning_left,
                                                             CS.ldw_lane_warning_right, CS.ldw_side_dlc_tlc,
-                                                            CS.ldw_dlc, CS.ldw_tlc))
+                                                            CS.ldw_dlc, CS.ldw_tlc,
+                                                            left_lane_depart, right_lane_depart))
 
     #--------------------------------------------------------------------------
     #                                                                         #

selfdrive/car/volkswagen/interface.py

@@ -168,6 +168,8 @@ class CarInterface(CarInterfaceBase):
     can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
                    c.hudControl.visualAlert,
                    c.hudControl.leftLaneVisible,
-                   c.hudControl.rightLaneVisible)
+                   c.hudControl.rightLaneVisible,
+                   c.hudControl.leftLaneDepart,
+                   c.hudControl.rightLaneDepart)
     self.frame += 1
     return can_sends

selfdrive/car/volkswagen/volkswagencan.py

@@ -15,18 +15,22 @@ def create_mqb_steering_control(packer, bus, apply_steer, idx, lkas_enabled):
   }
   return packer.make_can_msg("HCA_01", bus, values, idx)
 
-def create_mqb_hud_control(packer, bus, hca_enabled, steering_pressed, hud_alert, left_lane_visible, right_lane_visible,
-                           ldw_lane_warning_left, ldw_lane_warning_right, ldw_side_dlc_tlc, ldw_dlc, ldw_tlc):
-  if hca_enabled:
-    left_lane_hud = 3 if left_lane_visible else 1
-    right_lane_hud = 3 if right_lane_visible else 1
-  else:
+def create_mqb_hud_control(packer, bus, enabled, steering_pressed, hud_alert, left_lane_visible, right_lane_visible,
+                           ldw_lane_warning_left, ldw_lane_warning_right, ldw_side_dlc_tlc, ldw_dlc, ldw_tlc,
+                           left_lane_depart, right_lane_depart):
+  if enabled:
     left_lane_hud = 2 if left_lane_visible else 1
     right_lane_hud = 2 if right_lane_visible else 1
+  else:
+    left_lane_hud = 0
+    right_lane_hud = 0
+
+  left_lane_hud = 3 if left_lane_depart else left_lane_hud
+  right_lane_hud = 3 if right_lane_depart else right_lane_hud
 
   values = {
-    "LDW_Status_LED_gelb": 1 if hca_enabled and steering_pressed else 0,
-    "LDW_Status_LED_gruen": 1 if hca_enabled and not steering_pressed else 0,
+    "LDW_Status_LED_gelb": 1 if enabled and steering_pressed else 0,
+    "LDW_Status_LED_gruen": 1 if enabled and not steering_pressed else 0,
     "LDW_Lernmodus_links": left_lane_hud,
     "LDW_Lernmodus_rechts": right_lane_hud,
     "LDW_Texte": hud_alert,