diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index ec443dfd4d..5553fda0d8 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -80,29 +80,30 @@ class CarController: else: if self.frame % 10 == 0: - can_sends.append(subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg, CC.enabled, self.CP.openpilotLongitudinalControl, + can_sends.append(subarucan.create_es_dashstatus(self.packer, self.frame // 10, CS.es_dashstatus_msg, CC.enabled, self.CP.openpilotLongitudinalControl, CC.longActive, hud_control.leadVisible)) - can_sends.append(subarucan.create_es_lkas_state(self.packer, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert, + can_sends.append(subarucan.create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: - can_sends.append(subarucan.create_es_infotainment(self.packer, CS.es_infotainment_msg, hud_control.visualAlert)) + can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert)) if self.CP.openpilotLongitudinalControl: if self.frame % 5 == 0: - can_sends.append(subarucan.create_es_status(self.packer, CS.es_status_msg, self.CP.openpilotLongitudinalControl, CC.longActive, cruise_rpm)) + can_sends.append(subarucan.create_es_status(self.packer, self.frame // 5, CS.es_status_msg, + self.CP.openpilotLongitudinalControl, CC.longActive, cruise_rpm)) - can_sends.append(subarucan.create_es_brake(self.packer, CS.es_brake_msg, CC.enabled, cruise_brake)) + can_sends.append(subarucan.create_es_brake(self.packer, self.frame // 5, CS.es_brake_msg, CC.enabled, cruise_brake)) - can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, 0, pcm_cancel_cmd, + can_sends.append(subarucan.create_es_distance(self.packer, self.frame // 5, CS.es_distance_msg, 0, pcm_cancel_cmd, self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle)) else: if pcm_cancel_cmd: if self.CP.carFingerprint not in HYBRID_CARS: bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main - can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd)) + can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd)) new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 0f297d9635..57c1ae7741 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -16,10 +16,9 @@ def create_steering_control(packer, apply_steer, steer_req): def create_steering_status(packer): return packer.make_can_msg("ES_LKAS_State", 0, {}) -def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd, long_enabled = False, brake_cmd = False, cruise_throttle = 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", - "COUNTER", "Signal1", "Cruise_Fault", "Cruise_Throttle", @@ -39,7 +38,8 @@ def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd, long_enable "Cruise_Resume", "Signal6", ]} - values["COUNTER"] = (values["COUNTER"] + 1) % 0x10 + + values["COUNTER"] = frame % 0x10 if long_enabled: values["Cruise_Throttle"] = cruise_throttle @@ -57,10 +57,9 @@ def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd, long_enable return packer.make_can_msg("ES_Distance", bus, values) -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, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): values = {s: es_lkas_state_msg[s] for s in [ "CHECKSUM", - "COUNTER", "LKAS_Alert_Msg", "Signal1", "LKAS_ACTIVE", @@ -77,6 +76,8 @@ def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_ "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 @@ -119,10 +120,9 @@ def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_ return packer.make_can_msg("ES_LKAS_State", CanBus.main, values) -def create_es_dashstatus(packer, dashstatus_msg, enabled, long_enabled, long_active, lead_visible): +def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, long_active, lead_visible): values = {s: dashstatus_msg[s] for s in [ "CHECKSUM", - "COUNTER", "PCB_Off", "LDW_Off", "Signal1", @@ -150,6 +150,8 @@ def create_es_dashstatus(packer, dashstatus_msg, enabled, long_enabled, long_act "Cruise_State", ]} + values["COUNTER"] = frame % 0x10 + if enabled and long_active: values["Cruise_State"] = 0 values["Cruise_Activated"] = 1 @@ -165,10 +167,9 @@ def create_es_dashstatus(packer, dashstatus_msg, enabled, long_enabled, long_act return packer.make_can_msg("ES_DashStatus", CanBus.main, values) -def create_es_brake(packer, es_brake_msg, enabled, brake_value): +def create_es_brake(packer, frame, es_brake_msg, enabled, brake_value): values = {s: es_brake_msg[s] for s in [ "CHECKSUM", - "COUNTER", "Signal1", "Brake_Pressure", "AEB_Status", @@ -179,6 +180,8 @@ def create_es_brake(packer, es_brake_msg, enabled, brake_value): "Signal3", ]} + values["COUNTER"] = frame % 0x10 + if enabled: values["Cruise_Activated"] = 1 @@ -190,10 +193,9 @@ def create_es_brake(packer, es_brake_msg, enabled, brake_value): return packer.make_can_msg("ES_Brake", CanBus.main, values) -def create_es_status(packer, es_status_msg, long_enabled, long_active, cruise_rpm): +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", - "COUNTER", "Signal1", "Cruise_Fault", "Cruise_RPM", @@ -204,6 +206,8 @@ def create_es_status(packer, es_status_msg, long_enabled, long_active, cruise_rp "Signal3", ]} + values["COUNTER"] = frame % 0x10 + if long_enabled: values["Cruise_RPM"] = cruise_rpm @@ -213,16 +217,18 @@ def create_es_status(packer, es_status_msg, long_enabled, long_active, cruise_rp return packer.make_can_msg("ES_Status", CanBus.main, values) -def create_es_infotainment(packer, es_infotainment_msg, visual_alert): +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", - "COUNTER", "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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index cee528c62d..f5e098910f 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -20022e4a1b5f5df343465180a3865b0c78890544 \ No newline at end of file +649c512f95d9ad80e7c8c4d852cfa5f468a28fd5 \ No newline at end of file