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

6 years ago · e0227c9a1d
parent cc29999960
commit e0227c9a1d
4 changed files with 112 additions and 3 deletions
--- a/selfdrive/car/nissan/carcontroller.py
+++ b/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
--- a/selfdrive/car/nissan/carstate.py
+++ b/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 = [
--- a/selfdrive/car/nissan/interface.py
+++ b/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
--- a/selfdrive/car/nissan/nissancan.py
+++ b/selfdrive/car/nissan/nissancan.py
@ -34,5 +34,27 @@ def create_acc_cancel_cmd(packer, cruise_throttle_msg, frame):
  values["RES_BUTTON"] = 0
  values["FOLLOW_DISTANCE_BUTTON"] = 0
  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)