carrot/selfdrive/car/subaru/subarucan.py
FrogAi 9b1ff06bba Controls - Always On Lateral
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>
2024-06-01 02:34:36 -07:00

324 lines
8.6 KiB
Python

from cereal import car
from openpilot.selfdrive.car.subaru.values import CanBus
VisualAlert = car.CarControl.HUDControl.VisualAlert
def create_steering_control(packer, apply_steer, steer_req):
values = {
"LKAS_Output": apply_steer,
"LKAS_Request": steer_req,
"SET_1": 1
}
return packer.make_can_msg("ES_LKAS", 0, values)
def create_steering_control_angle(packer, apply_steer, steer_req):
values = {
"LKAS_Output": apply_steer,
"LKAS_Request": steer_req,
"SET_3": 3
}
return packer.make_can_msg("ES_LKAS_ANGLE", 0, values)
def create_steering_status(packer):
return packer.make_can_msg("ES_LKAS_State", 0, {})
def create_es_distance(packer, frame, es_distance_msg, bus, pcm_cancel_cmd, long_enabled = False, brake_cmd = False, cruise_throttle = 0):
values = {s: es_distance_msg[s] for s in [
"CHECKSUM",
"Signal1",
"Cruise_Fault",
"Cruise_Throttle",
"Signal2",
"Car_Follow",
"Low_Speed_Follow",
"Cruise_Soft_Disable",
"Signal7",
"Cruise_Brake_Active",
"Distance_Swap",
"Cruise_EPB",
"Signal4",
"Close_Distance",
"Signal5",
"Cruise_Cancel",
"Cruise_Set",
"Cruise_Resume",
"Signal6",
]}
values["COUNTER"] = frame % 0x10
if long_enabled:
values["Cruise_Throttle"] = cruise_throttle
# Do not disable openpilot on Eyesight Soft Disable, if openpilot is controlling long
values["Cruise_Soft_Disable"] = 0
values["Cruise_Fault"] = 0
values["Cruise_Brake_Active"] = brake_cmd
if pcm_cancel_cmd:
values["Cruise_Cancel"] = 1
values["Cruise_Throttle"] = 1818 # inactive throttle
return packer.make_can_msg("ES_Distance", bus, values)
def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart, lat_active):
values = {s: es_lkas_state_msg[s] for s in [
"CHECKSUM",
"LKAS_Alert_Msg",
"Signal1",
"LKAS_ACTIVE",
"LKAS_Dash_State",
"Signal2",
"Backward_Speed_Limit_Menu",
"LKAS_Left_Line_Enable",
"LKAS_Left_Line_Light_Blink",
"LKAS_Right_Line_Enable",
"LKAS_Right_Line_Light_Blink",
"LKAS_Left_Line_Visible",
"LKAS_Right_Line_Visible",
"LKAS_Alert",
"Signal3",
]}
values["COUNTER"] = frame % 0x10
# Filter the stock LKAS "Keep hands on wheel" alert
if values["LKAS_Alert_Msg"] == 1:
values["LKAS_Alert_Msg"] = 0
# Filter the stock LKAS sending an audible alert when it turns off LKAS
if values["LKAS_Alert"] == 27:
values["LKAS_Alert"] = 0
# Filter the stock LKAS sending an audible alert when "Keep hands on wheel" alert is active (2020+ models)
if values["LKAS_Alert"] == 28 and values["LKAS_Alert_Msg"] == 7:
values["LKAS_Alert"] = 0
# Filter the stock LKAS sending an audible alert when "Keep hands on wheel OFF" alert is active (2020+ models)
if values["LKAS_Alert"] == 30:
values["LKAS_Alert"] = 0
# Filter the stock LKAS sending "Keep hands on wheel OFF" alert (2020+ models)
if values["LKAS_Alert_Msg"] == 7:
values["LKAS_Alert_Msg"] = 0
# Show Keep hands on wheel alert for openpilot steerRequired alert
if visual_alert == VisualAlert.steerRequired:
values["LKAS_Alert_Msg"] = 1
# Ensure we don't overwrite potentially more important alerts from stock (e.g. FCW)
if visual_alert == VisualAlert.ldw and values["LKAS_Alert"] == 0:
if left_lane_depart:
values["LKAS_Alert"] = 12 # Left lane departure dash alert
elif right_lane_depart:
values["LKAS_Alert"] = 11 # Right lane departure dash alert
if lat_active:
values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines
values["LKAS_Dash_State"] = 2 # Green enabled indicator
values["LKAS_Left_Line_Enable"] = 1
values["LKAS_Right_Line_Enable"] = 1
else:
values["LKAS_Dash_State"] = 0 # LKAS Not enabled
values["LKAS_Left_Line_Visible"] = int(left_line)
values["LKAS_Right_Line_Visible"] = int(right_line)
return packer.make_can_msg("ES_LKAS_State", CanBus.main, values)
def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, long_active, lead_visible):
values = {s: dashstatus_msg[s] for s in [
"CHECKSUM",
"PCB_Off",
"LDW_Off",
"Signal1",
"Cruise_State_Msg",
"LKAS_State_Msg",
"Signal2",
"Cruise_Soft_Disable",
"Cruise_Status_Msg",
"Signal3",
"Cruise_Distance",
"Signal4",
"Conventional_Cruise",
"Signal5",
"Cruise_Disengaged",
"Cruise_Activated",
"Signal6",
"Cruise_Set_Speed",
"Cruise_Fault",
"Cruise_On",
"Display_Own_Car",
"Brake_Lights",
"Car_Follow",
"Signal7",
"Far_Distance",
"Cruise_State",
]}
values["COUNTER"] = frame % 0x10
if long_enabled:
values["Cruise_State"] = 0
values["Cruise_Activated"] = enabled
values["Cruise_Disengaged"] = 0
values["Car_Follow"] = int(lead_visible)
values["PCB_Off"] = 1 # AEB is not presevered, so show the PCB_Off on dash
values["LDW_Off"] = 0
values["Cruise_Fault"] = 0
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
if values["LKAS_State_Msg"] in (2, 3):
values["LKAS_State_Msg"] = 0
return packer.make_can_msg("ES_DashStatus", CanBus.main, values)
def create_es_brake(packer, frame, es_brake_msg, long_enabled, long_active, brake_value):
values = {s: es_brake_msg[s] for s in [
"CHECKSUM",
"Signal1",
"Brake_Pressure",
"AEB_Status",
"Cruise_Brake_Lights",
"Cruise_Brake_Fault",
"Cruise_Brake_Active",
"Cruise_Activated",
"Signal3",
]}
values["COUNTER"] = frame % 0x10
if long_enabled:
values["Cruise_Brake_Fault"] = 0
values["Cruise_Activated"] = long_active
values["Brake_Pressure"] = brake_value
values["Cruise_Brake_Active"] = brake_value > 0
values["Cruise_Brake_Lights"] = brake_value >= 70
return packer.make_can_msg("ES_Brake", CanBus.main, values)
def create_es_status(packer, frame, es_status_msg, long_enabled, long_active, cruise_rpm):
values = {s: es_status_msg[s] for s in [
"CHECKSUM",
"Signal1",
"Cruise_Fault",
"Cruise_RPM",
"Cruise_Activated",
"Brake_Lights",
"Cruise_Hold",
"Signal3",
]}
values["COUNTER"] = frame % 0x10
if long_enabled:
values["Cruise_RPM"] = cruise_rpm
values["Cruise_Fault"] = 0
values["Cruise_Activated"] = long_active
return packer.make_can_msg("ES_Status", CanBus.main, values)
def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert):
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
values = {s: es_infotainment_msg[s] for s in [
"CHECKSUM",
"LKAS_State_Infotainment",
"LKAS_Blue_Lines",
"Signal1",
"Signal2",
]}
values["COUNTER"] = frame % 0x10
if values["LKAS_State_Infotainment"] in (3, 4):
values["LKAS_State_Infotainment"] = 0
# Show Keep hands on wheel alert for openpilot steerRequired alert
if visual_alert == VisualAlert.steerRequired:
values["LKAS_State_Infotainment"] = 3
# Show Obstacle Detected for fcw
if visual_alert == VisualAlert.fcw:
values["LKAS_State_Infotainment"] = 2
return packer.make_can_msg("ES_Infotainment", CanBus.main, values)
def create_es_highbeamassist(packer):
values = {
"HBA_Available": False,
}
return packer.make_can_msg("ES_HighBeamAssist", CanBus.main, values)
def create_es_static_1(packer):
values = {
"SET_3": 3,
}
return packer.make_can_msg("ES_STATIC_1", CanBus.main, values)
def create_es_static_2(packer):
values = {
"SET_3": 3,
}
return packer.make_can_msg("ES_STATIC_2", CanBus.main, values)
# *** Subaru Pre-global ***
def subaru_preglobal_checksum(packer, values, addr, checksum_byte=7):
dat = packer.make_can_msg(addr, 0, values)[2]
return (sum(dat[:checksum_byte]) + sum(dat[checksum_byte+1:])) % 256
def create_preglobal_steering_control(packer, frame, apply_steer, steer_req):
values = {
"COUNTER": frame % 0x08,
"LKAS_Command": apply_steer,
"LKAS_Active": steer_req,
}
values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_LKAS")
return packer.make_can_msg("ES_LKAS", CanBus.main, values)
def create_preglobal_es_distance(packer, cruise_button, es_distance_msg):
values = {s: es_distance_msg[s] for s in [
"Cruise_Throttle",
"Signal1",
"Car_Follow",
"Signal2",
"Cruise_Brake_Active",
"Distance_Swap",
"Standstill",
"Signal3",
"Close_Distance",
"Signal4",
"Standstill_2",
"Cruise_Fault",
"Signal5",
"COUNTER",
"Signal6",
"Cruise_Button",
"Signal7",
]}
values["Cruise_Button"] = cruise_button
values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_Distance")
return packer.make_can_msg("ES_Distance", CanBus.main, values)