diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index f79617f56..6b1395228 100644 --- 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 diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 5ce06955a..1f9dc8d6f 100644 --- 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 = [ diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 47d18297a..437e01ef5 100644 --- 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 diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py index 9e42e0fe4..b48c9890b 100644 --- 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)