Added LKAS HUD line detection/steer status/hand on wheel warning (#1225)
Andre Volmensky
GitHub
parent
cc29999960
commit
e0227c9a1d
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 = [
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue