Added LKAS HUD line detection/steer status/hand on wheel warning (#1225)

selfdrive/car/nissan/carcontroller.py

@@ -1,3 +1,4 @@
+from cereal import car
 from common.numpy_fast import clip, interp
 from selfdrive.car.nissan import nissancan
 from opendbc.can.packer import CANPacker
@@ -10,6 +11,7 @@ ANGLE_DELTA_V = [5., .8, .15]     # windup limit
 ANGLE_DELTA_VU = [5., 3.5, 0.4]   # unwind limit
 LKAS_MAX_TORQUE = 100             # A value of 100 is easy to overpower
 
+VisualAlert = car.CarControl.HUDControl.VisualAlert
 
 class CarController():
   def __init__(self, dbc_name, CP, VM):
@@ -20,7 +22,8 @@ class CarController():
 
     self.packer = CANPacker(dbc_name)
 
-  def update(self, enabled, CS, frame, actuators, cruise_cancel):
+  def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
+             left_line, right_line, left_lane_depart, right_lane_depart):
     """ Controls thread """
 
     # Send CAN commands.
@@ -29,8 +32,12 @@ class CarController():
     ### STEER ###
     acc_active = bool(CS.out.cruiseState.enabled)
     cruise_throttle_msg = CS.cruise_throttle_msg
+    lkas_hud_msg = CS.lkas_hud_msg
+    lkas_hud_info_msg = CS.lkas_hud_info_msg
     apply_angle = actuators.steerAngle
 
+    steer_hud_alert = 1 if hud_alert == VisualAlert.steerRequired else 0
+
     if enabled:
       # # windup slower
       if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
@@ -70,4 +77,13 @@ class CarController():
     can_sends.append(nissancan.create_steering_control(
         self.packer, self.car_fingerprint, apply_angle, frame, acc_active, self.lkas_max_torque))
 
+    if frame % 2 == 0:
+      can_sends.append(nissancan.create_lkas_hud_msg(
+        self.packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart))
+
+    if frame % 50 == 0:
+      can_sends.append(nissancan.create_lkas_hud_info_msg(
+        self.packer, lkas_hud_info_msg, steer_hud_alert
+      ))
+
     return can_sends

selfdrive/car/nissan/carstate.py

@@ -55,6 +55,8 @@ class CarState(CarStateBase):
     ret.espDisabled = bool(cp.vl["_ESP"]["ESP_DISABLED"])
 
     self.cruise_throttle_msg = copy.copy(cp.vl["CruiseThrottle"])
+    self.lkas_hud_msg = copy.copy(cp_adas.vl["PROPILOT_HUD"])
+    self.lkas_hud_info_msg = copy.copy(cp_adas.vl["PROPILOT_HUD_INFO_MSG"])
 
     return ret
 
@@ -115,6 +117,73 @@ class CarState(CarStateBase):
       ("SET_0x80", "LKAS", 0),
       ("COUNTER", "LKAS", 0),
       ("LKA_ACTIVE", "LKAS", 0),
+      # Below are the HUD messages. We copy the stock message and modify
+      ("LARGE_WARNING_FLASHING", "PROPILOT_HUD", 0),
+      ("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD", 0),
+      ("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD", 0),
+      ("LEAD_CAR", "PROPILOT_HUD", 0),
+      ("LEAD_CAR_ERROR", "PROPILOT_HUD", 0),
+      ("FRONT_RADAR_ERROR", "PROPILOT_HUD", 0),
+      ("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD", 0),
+      ("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD", 0),
+      ("LKAS_ERROR_FLASHING", "PROPILOT_HUD", 0),
+      ("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD", 0),
+      ("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD", 0),
+      ("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD", 0),
+      ("FOLLOW_DISTANCE", "PROPILOT_HUD", 0),
+      ("AUDIBLE_TONE", "PROPILOT_HUD", 0),
+      ("SPEED_SET_ICON", "PROPILOT_HUD", 0),
+      ("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD", 0),
+      ("unknown59", "PROPILOT_HUD", 0),
+      ("unknown55", "PROPILOT_HUD", 0),
+      ("unknown26", "PROPILOT_HUD", 0),
+      ("unknown28", "PROPILOT_HUD", 0),
+      ("unknown31", "PROPILOT_HUD", 0),
+      ("unknown39", "PROPILOT_HUD", 0),
+      ("unknown43", "PROPILOT_HUD", 0),
+      ("unknown8", "PROPILOT_HUD", 0),
+      ("unknown05", "PROPILOT_HUD", 0),
+      ("unknown02", "PROPILOT_HUD", 0),
+      ("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG", 0),
+      ("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0),
+      ("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0),
+      ("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0),
+      ("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0),
+      ("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG", 0),
+      ("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG", 0),
+      ("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG", 0),
+      ("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG", 0),
+      ("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG", 0),
+      ("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG", 0),
+      ("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG", 0),
+      ("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG", 0),
+      ("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG", 0),
+      ("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0),
+      ("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0),
+      ("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0),
+      ("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0),
+      ("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0),
+      ("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG", 0),
+      ("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG", 0),
+      ("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG", 0),
+      ("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG", 0),
+      ("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG", 0),
+      ("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0),
+      ("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0),
+      ("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0),
+      ("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG", 0),
+      ("unknown07", "PROPILOT_HUD_INFO_MSG", 0),
+      ("unknown10", "PROPILOT_HUD_INFO_MSG", 0),
+      ("unknown15", "PROPILOT_HUD_INFO_MSG", 0),
+      ("unknown23", "PROPILOT_HUD_INFO_MSG", 0),
+      ("unknown19", "PROPILOT_HUD_INFO_MSG", 0),
+      ("unknown31", "PROPILOT_HUD_INFO_MSG", 0),
+      ("unknown32", "PROPILOT_HUD_INFO_MSG", 0),
+      ("unknown46", "PROPILOT_HUD_INFO_MSG", 0),
+      ("unknown61", "PROPILOT_HUD_INFO_MSG", 0),
+      ("unknown55", "PROPILOT_HUD_INFO_MSG", 0),
+      ("unknown50", "PROPILOT_HUD_INFO_MSG", 0),
+
     ]
 
     checks = [

selfdrive/car/nissan/interface.py

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

selfdrive/car/nissan/nissancan.py

@@ -36,3 +36,25 @@ def create_acc_cancel_cmd(packer, cruise_throttle_msg, frame):
   values["COUNTER"] = (frame % 4)
 
   return packer.make_can_msg("CruiseThrottle", 2, values)
+
+
+def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart):
+  values = lkas_hud_msg
+
+  values["RIGHT_LANE_YELLOW_FLASH"] = 1 if right_lane_depart else 0
+  values["LEFT_LANE_YELLOW_FLASH"] = 1 if left_lane_depart else 0
+
+  values["LARGE_STEERING_WHEEL_ICON"] = 2 if enabled else 0
+  values["RIGHT_LANE_GREEN"] = 1 if right_line and enabled else 0
+  values["LEFT_LANE_GREEN"] = 1 if left_line and enabled else 0
+
+  return packer.make_can_msg("PROPILOT_HUD", 0, values)
+
+
+def create_lkas_hud_info_msg(packer, lkas_hud_info_msg, steer_hud_alert):
+  values = lkas_hud_info_msg
+
+  if steer_hud_alert:
+    values["HANDS_ON_WHEEL_WARNING"] = 1
+
+  return packer.make_can_msg("PROPILOT_HUD_INFO_MSG", 0, values)