|
|
@ -1,4 +1,3 @@ |
|
|
|
import copy |
|
|
|
|
|
|
|
from cereal import car |
|
|
|
from cereal import car |
|
|
|
|
|
|
|
|
|
|
|
VisualAlert = car.CarControl.HUDControl.VisualAlert |
|
|
|
VisualAlert = car.CarControl.HUDControl.VisualAlert |
|
|
@ -18,7 +17,28 @@ def create_steering_status(packer): |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd): |
|
|
|
def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd): |
|
|
|
values = copy.copy(es_distance_msg) |
|
|
|
values = {s: es_distance_msg[s] for s in [ |
|
|
|
|
|
|
|
"CHECKSUM", |
|
|
|
|
|
|
|
"COUNTER", |
|
|
|
|
|
|
|
"Signal1", |
|
|
|
|
|
|
|
"Cruise_Fault", |
|
|
|
|
|
|
|
"Cruise_Throttle", |
|
|
|
|
|
|
|
"Signal2", |
|
|
|
|
|
|
|
"Car_Follow", |
|
|
|
|
|
|
|
"Signal3", |
|
|
|
|
|
|
|
"Cruise_Soft_Disable", |
|
|
|
|
|
|
|
"Signal7", |
|
|
|
|
|
|
|
"Cruise_Brake_Active", |
|
|
|
|
|
|
|
"Distance_Swap", |
|
|
|
|
|
|
|
"Cruise_EPB", |
|
|
|
|
|
|
|
"Signal4", |
|
|
|
|
|
|
|
"Close_Distance", |
|
|
|
|
|
|
|
"Signal5", |
|
|
|
|
|
|
|
"Cruise_Cancel", |
|
|
|
|
|
|
|
"Cruise_Set", |
|
|
|
|
|
|
|
"Cruise_Resume", |
|
|
|
|
|
|
|
"Signal6", |
|
|
|
|
|
|
|
]} |
|
|
|
values["COUNTER"] = (values["COUNTER"] + 1) % 0x10 |
|
|
|
values["COUNTER"] = (values["COUNTER"] + 1) % 0x10 |
|
|
|
if pcm_cancel_cmd: |
|
|
|
if pcm_cancel_cmd: |
|
|
|
values["Cruise_Cancel"] = 1 |
|
|
|
values["Cruise_Cancel"] = 1 |
|
|
@ -26,7 +46,24 @@ def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd): |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): |
|
|
|
def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): |
|
|
|
values = copy.copy(es_lkas_state_msg) |
|
|
|
values = {s: es_lkas_state_msg[s] for s in [ |
|
|
|
|
|
|
|
"CHECKSUM", |
|
|
|
|
|
|
|
"COUNTER", |
|
|
|
|
|
|
|
"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", |
|
|
|
|
|
|
|
]} |
|
|
|
|
|
|
|
|
|
|
|
# Filter the stock LKAS "Keep hands on wheel" alert |
|
|
|
# Filter the stock LKAS "Keep hands on wheel" alert |
|
|
|
if values["LKAS_Alert_Msg"] == 1: |
|
|
|
if values["LKAS_Alert_Msg"] == 1: |
|
|
@ -69,7 +106,35 @@ def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def create_es_dashstatus(packer, dashstatus_msg): |
|
|
|
def create_es_dashstatus(packer, dashstatus_msg): |
|
|
|
values = copy.copy(dashstatus_msg) |
|
|
|
values = {s: dashstatus_msg[s] for s in [ |
|
|
|
|
|
|
|
"CHECKSUM", |
|
|
|
|
|
|
|
"COUNTER", |
|
|
|
|
|
|
|
"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", |
|
|
|
|
|
|
|
]} |
|
|
|
|
|
|
|
|
|
|
|
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts |
|
|
|
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts |
|
|
|
if values["LKAS_State_Msg"] in (2, 3): |
|
|
|
if values["LKAS_State_Msg"] in (2, 3): |
|
|
@ -80,18 +145,26 @@ def create_es_dashstatus(packer, dashstatus_msg): |
|
|
|
|
|
|
|
|
|
|
|
def create_infotainmentstatus(packer, infotainmentstatus_msg, visual_alert): |
|
|
|
def create_infotainmentstatus(packer, infotainmentstatus_msg, visual_alert): |
|
|
|
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts |
|
|
|
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts |
|
|
|
if infotainmentstatus_msg["LKAS_State_Infotainment"] in (3, 4): |
|
|
|
values = {s: infotainmentstatus_msg[s] for s in [ |
|
|
|
infotainmentstatus_msg["LKAS_State_Infotainment"] = 0 |
|
|
|
"CHECKSUM", |
|
|
|
|
|
|
|
"COUNTER", |
|
|
|
|
|
|
|
"LKAS_State_Infotainment", |
|
|
|
|
|
|
|
"LKAS_Blue_Lines", |
|
|
|
|
|
|
|
"Signal1", |
|
|
|
|
|
|
|
"Signal2", |
|
|
|
|
|
|
|
]} |
|
|
|
|
|
|
|
if values["LKAS_State_Infotainment"] in (3, 4): |
|
|
|
|
|
|
|
values["LKAS_State_Infotainment"] = 0 |
|
|
|
|
|
|
|
|
|
|
|
# Show Keep hands on wheel alert for openpilot steerRequired alert |
|
|
|
# Show Keep hands on wheel alert for openpilot steerRequired alert |
|
|
|
if visual_alert == VisualAlert.steerRequired: |
|
|
|
if visual_alert == VisualAlert.steerRequired: |
|
|
|
infotainmentstatus_msg["LKAS_State_Infotainment"] = 3 |
|
|
|
values["LKAS_State_Infotainment"] = 3 |
|
|
|
|
|
|
|
|
|
|
|
# Show Obstacle Detected for fcw |
|
|
|
# Show Obstacle Detected for fcw |
|
|
|
if visual_alert == VisualAlert.fcw: |
|
|
|
if visual_alert == VisualAlert.fcw: |
|
|
|
infotainmentstatus_msg["LKAS_State_Infotainment"] = 2 |
|
|
|
values["LKAS_State_Infotainment"] = 2 |
|
|
|
|
|
|
|
|
|
|
|
return packer.make_can_msg("INFOTAINMENT_STATUS", 0, infotainmentstatus_msg) |
|
|
|
return packer.make_can_msg("INFOTAINMENT_STATUS", 0, values) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# *** Subaru Pre-global *** |
|
|
|
# *** Subaru Pre-global *** |
|
|
@ -112,9 +185,27 @@ def create_preglobal_steering_control(packer, apply_steer): |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def create_preglobal_es_distance(packer, cruise_button, es_distance_msg): |
|
|
|
def create_preglobal_es_distance(packer, cruise_button, es_distance_msg): |
|
|
|
values = copy.copy(es_distance_msg) |
|
|
|
values = {s: es_distance_msg[s] for s in [ |
|
|
|
values["Cruise_Button"] = cruise_button |
|
|
|
"Cruise_Throttle", |
|
|
|
|
|
|
|
"Signal1", |
|
|
|
|
|
|
|
"Car_Follow", |
|
|
|
|
|
|
|
"Signal2", |
|
|
|
|
|
|
|
"Brake_On", |
|
|
|
|
|
|
|
"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") |
|
|
|
values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_Distance") |
|
|
|
|
|
|
|
|
|
|
|
return packer.make_can_msg("ES_Distance", 0, values) |
|
|
|
return packer.make_can_msg("ES_Distance", 0, values) |
|
|
|