sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
old-commit-hash: e0227c9a1d
commatwo_master
Andre Volmensky 5 years ago committed by GitHub
parent de40f1448d
commit ff7588f6d2
4 changed files with 112 additions and 3 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      selfdrive/car/nissan/carcontroller.py
  2. 69
      selfdrive/car/nissan/carstate.py
  3. 4
      selfdrive/car/nissan/interface.py
  4. 24
      selfdrive/car/nissan/nissancan.py

18
selfdrive/car/nissan/carcontroller.py
Unescape View File

@ -1,3 +1,4 @@
from cereal import car
from common.numpy_fast import clip, interp from common.numpy_fast import clip, interp
from selfdrive.car.nissan import nissancan from selfdrive.car.nissan import nissancan
from opendbc.can.packer import CANPacker 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 ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
LKAS_MAX_TORQUE = 100 # A value of 100 is easy to overpower LKAS_MAX_TORQUE = 100 # A value of 100 is easy to overpower
VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarController(): class CarController():
def __init__(self, dbc_name, CP, VM): def __init__(self, dbc_name, CP, VM):
@ -20,7 +22,8 @@ class CarController():
self.packer = CANPacker(dbc_name) 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 """ """ Controls thread """
# Send CAN commands. # Send CAN commands.
@ -29,8 +32,12 @@ class CarController():
### STEER ### ### STEER ###
acc_active = bool(CS.out.cruiseState.enabled) acc_active = bool(CS.out.cruiseState.enabled)
cruise_throttle_msg = CS.cruise_throttle_msg 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 apply_angle = actuators.steerAngle
steer_hud_alert = 1 if hud_alert == VisualAlert.steerRequired else 0
if enabled: if enabled:
# # windup slower # # windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): 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( can_sends.append(nissancan.create_steering_control(
self.packer, self.car_fingerprint, apply_angle, frame, acc_active, self.lkas_max_torque)) 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 return can_sends

69
selfdrive/car/nissan/carstate.py
Unescape View File

@ -55,6 +55,8 @@ class CarState(CarStateBase):
ret.espDisabled = bool(cp.vl["_ESP"]["ESP_DISABLED"]) ret.espDisabled = bool(cp.vl["_ESP"]["ESP_DISABLED"])
self.cruise_throttle_msg = copy.copy(cp.vl["CruiseThrottle"]) 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 return ret
@ -115,6 +117,73 @@ class CarState(CarStateBase):
("SET_0x80", "LKAS", 0), ("SET_0x80", "LKAS", 0),
("COUNTER", "LKAS", 0), ("COUNTER", "LKAS", 0),
("LKA_ACTIVE", "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 = [ checks = [

4
selfdrive/car/nissan/interface.py
Unescape View File

@ -87,6 +87,8 @@ class CarInterface(CarInterfaceBase):
def apply(self, c): def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, 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 self.frame += 1
return can_sends return can_sends

24
selfdrive/car/nissan/nissancan.py
Unescape View File

@ -34,5 +34,27 @@ def create_acc_cancel_cmd(packer, cruise_throttle_msg, frame):
values["RES_BUTTON"] = 0 values["RES_BUTTON"] = 0
values["FOLLOW_DISTANCE_BUTTON"] = 0 values["FOLLOW_DISTANCE_BUTTON"] = 0
values["COUNTER"] = (frame % 4) values["COUNTER"] = (frame % 4)
return packer.make_can_msg("CruiseThrottle", 2, values) 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)

Write Preview
Loading…
Cancel
Save