9b1ff06bba
Maintain openpilot lateral control when the brake or gas pedals are used. Deactivation occurs only through the 'Cruise Control' button. Lots of credit goes to "pfeiferj"! Couldn't of done it without him! https: //github.com/pfeiferj/openpilot Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
155 lines
4.1 KiB
Python
155 lines
4.1 KiB
Python
import crcmod
|
|
from openpilot.selfdrive.car.nissan.values import CAR
|
|
|
|
# TODO: add this checksum to the CANPacker
|
|
nissan_checksum = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff)
|
|
|
|
|
|
def create_steering_control(packer, apply_steer, frame, steer_on, lkas_max_torque):
|
|
values = {
|
|
"COUNTER": frame % 0x10,
|
|
"DESIRED_ANGLE": apply_steer,
|
|
"SET_0x80_2": 0x80,
|
|
"SET_0x80": 0x80,
|
|
"MAX_TORQUE": lkas_max_torque if steer_on else 0,
|
|
"LKA_ACTIVE": steer_on,
|
|
}
|
|
|
|
dat = packer.make_can_msg("LKAS", 0, values)[2]
|
|
|
|
values["CHECKSUM"] = nissan_checksum(dat[:7])
|
|
return packer.make_can_msg("LKAS", 0, values)
|
|
|
|
|
|
def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg):
|
|
values = {s: cruise_throttle_msg[s] for s in [
|
|
"COUNTER",
|
|
"PROPILOT_BUTTON",
|
|
"CANCEL_BUTTON",
|
|
"GAS_PEDAL_INVERTED",
|
|
"SET_BUTTON",
|
|
"RES_BUTTON",
|
|
"FOLLOW_DISTANCE_BUTTON",
|
|
"NO_BUTTON_PRESSED",
|
|
"GAS_PEDAL",
|
|
"USER_BRAKE_PRESSED",
|
|
"NEW_SIGNAL_2",
|
|
"GAS_PRESSED_INVERTED",
|
|
"unsure1",
|
|
"unsure2",
|
|
"unsure3",
|
|
]}
|
|
can_bus = 1 if car_fingerprint == CAR.ALTIMA else 2
|
|
|
|
values["CANCEL_BUTTON"] = 1
|
|
values["NO_BUTTON_PRESSED"] = 0
|
|
values["PROPILOT_BUTTON"] = 0
|
|
values["SET_BUTTON"] = 0
|
|
values["RES_BUTTON"] = 0
|
|
values["FOLLOW_DISTANCE_BUTTON"] = 0
|
|
|
|
return packer.make_can_msg("CRUISE_THROTTLE", can_bus, values)
|
|
|
|
|
|
def create_cancel_msg(packer, cancel_msg, cruise_cancel):
|
|
values = {s: cancel_msg[s] for s in [
|
|
"CANCEL_SEATBELT",
|
|
"NEW_SIGNAL_1",
|
|
"NEW_SIGNAL_2",
|
|
"NEW_SIGNAL_3",
|
|
]}
|
|
|
|
if cruise_cancel:
|
|
values["CANCEL_SEATBELT"] = 1
|
|
|
|
return packer.make_can_msg("CANCEL_MSG", 2, values)
|
|
|
|
|
|
def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart, lat_active):
|
|
values = {s: lkas_hud_msg[s] for s in [
|
|
"LARGE_WARNING_FLASHING",
|
|
"SIDE_RADAR_ERROR_FLASHING1",
|
|
"SIDE_RADAR_ERROR_FLASHING2",
|
|
"LEAD_CAR",
|
|
"LEAD_CAR_ERROR",
|
|
"FRONT_RADAR_ERROR",
|
|
"FRONT_RADAR_ERROR_FLASHING",
|
|
"SIDE_RADAR_ERROR_FLASHING3",
|
|
"LKAS_ERROR_FLASHING",
|
|
"SAFETY_SHIELD_ACTIVE",
|
|
"RIGHT_LANE_GREEN_FLASH",
|
|
"LEFT_LANE_GREEN_FLASH",
|
|
"FOLLOW_DISTANCE",
|
|
"AUDIBLE_TONE",
|
|
"SPEED_SET_ICON",
|
|
"SMALL_STEERING_WHEEL_ICON",
|
|
"unknown59",
|
|
"unknown55",
|
|
"unknown26",
|
|
"unknown28",
|
|
"unknown31",
|
|
"SET_SPEED",
|
|
"unknown43",
|
|
"unknown08",
|
|
"unknown05",
|
|
"unknown02",
|
|
]}
|
|
|
|
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 lat_active else 0
|
|
values["RIGHT_LANE_GREEN"] = 1 if right_line and lat_active else 0
|
|
values["LEFT_LANE_GREEN"] = 1 if left_line and lat_active 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 = {s: lkas_hud_info_msg[s] for s in [
|
|
"NA_HIGH_ACCEL_TEMP",
|
|
"SIDE_RADAR_NA_HIGH_CABIN_TEMP",
|
|
"SIDE_RADAR_MALFUNCTION",
|
|
"LKAS_MALFUNCTION",
|
|
"FRONT_RADAR_MALFUNCTION",
|
|
"SIDE_RADAR_NA_CLEAN_REAR_CAMERA",
|
|
"NA_POOR_ROAD_CONDITIONS",
|
|
"CURRENTLY_UNAVAILABLE",
|
|
"SAFETY_SHIELD_OFF",
|
|
"FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION",
|
|
"PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED",
|
|
"SIDE_IMPACT_NA_RADAR_OBSTRUCTION",
|
|
"WARNING_DO_NOT_ENTER",
|
|
"SIDE_IMPACT_SYSTEM_OFF",
|
|
"SIDE_IMPACT_MALFUNCTION",
|
|
"FRONT_COLLISION_MALFUNCTION",
|
|
"SIDE_RADAR_MALFUNCTION2",
|
|
"LKAS_MALFUNCTION2",
|
|
"FRONT_RADAR_MALFUNCTION2",
|
|
"PROPILOT_NA_MSGS",
|
|
"BOTTOM_MSG",
|
|
"HANDS_ON_WHEEL_WARNING",
|
|
"WARNING_STEP_ON_BRAKE_NOW",
|
|
"PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED",
|
|
"PROPILOT_NA_HIGH_CABIN_TEMP",
|
|
"WARNING_PROPILOT_MALFUNCTION",
|
|
"ACC_UNAVAILABLE_HIGH_CABIN_TEMP",
|
|
"ACC_NA_FRONT_CAMERA_IMPARED",
|
|
"unknown07",
|
|
"unknown10",
|
|
"unknown15",
|
|
"unknown23",
|
|
"unknown19",
|
|
"unknown31",
|
|
"unknown32",
|
|
"unknown46",
|
|
"unknown61",
|
|
"unknown55",
|
|
"unknown50",
|
|
]}
|
|
|
|
if steer_hud_alert:
|
|
values["HANDS_ON_WHEEL_WARNING"] = 1
|
|
|
|
return packer.make_can_msg("PROPILOT_HUD_INFO_MSG", 0, values)
|