diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 19304efdb1..c277f012ab 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -2,7 +2,8 @@ from openpilot.common.numpy_fast import clip, interp from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance from openpilot.selfdrive.car.subaru import subarucan -from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, STEER_RATE_LIMITED, CanBus, CarControllerParams, SubaruFlags +from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_ES_ADDR, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, STEER_RATE_LIMITED, \ + CanBus, CarControllerParams, SubaruFlags # FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and # involves the total steering angle change rather than rate, but these limits work well for now @@ -119,6 +120,21 @@ class CarController: 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["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd)) + if self.CP.flags & SubaruFlags.DISABLE_EYESIGHT: + # Tester present (keeps eyesight disabled) + if self.frame % 100 == 0: + can_sends.append([GLOBAL_ES_ADDR, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", CanBus.camera]) + + # Create all of the other eyesight messages to keep the rest of the car happy when eyesight is disabled + if self.frame % 5 == 0: + can_sends.append(subarucan.create_es_highbeamassist(self.packer)) + + if self.frame % 10 == 0: + can_sends.append(subarucan.create_es_static_1(self.packer)) + + if self.frame % 2 == 0: + can_sends.append(subarucan.create_es_static_2(self.packer)) + new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 75bbc8ae91..10ed728812 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -1,8 +1,9 @@ from cereal import car from panda import Panda from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase -from openpilot.selfdrive.car.subaru.values import CAR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, SubaruFlags +from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, SubaruFlags class CarInterface(CarInterfaceBase): @@ -120,6 +121,9 @@ class CarInterface(CarInterfaceBase): #ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE | HYBRID_CARS) ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable + if candidate in GLOBAL_GEN2 and ret.openpilotLongitudinalControl: + ret.flags |= SubaruFlags.DISABLE_EYESIGHT + if ret.openpilotLongitudinalControl: ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.8, 1.0, 1.5] @@ -140,5 +144,10 @@ class CarInterface(CarInterfaceBase): return ret + @staticmethod + def init(CP, logcan, sendcan): + if CP.flags & SubaruFlags.DISABLE_EYESIGHT: + disable_ecu(logcan, sendcan, bus=2, addr=GLOBAL_ES_ADDR, com_cont_req=b'\x28\x03\x01') + def apply(self, c, now_nanos): return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 57c1ae7741..7d1939a497 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -243,6 +243,30 @@ def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert): 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): diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 53104d7d02..c9dc6ba5db 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -55,6 +55,11 @@ class CarControllerParams: class SubaruFlags(IntFlag): SEND_INFOTAINMENT = 1 + DISABLE_EYESIGHT = 2 + + +GLOBAL_ES_ADDR = 0x787 +GEN2_ES_BUTTONS_DID = b'\x11\x30' class CanBus: diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index da5d760dfe..8cf5f3debd 100755 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -251,6 +251,7 @@ routes = [ CarTestRoute("791340bc01ed993d|2019-03-10--16-28-08", SUBARU.IMPREZA), CarTestRoute("8bf7e79a3ce64055|2021-05-24--09-36-27", SUBARU.IMPREZA_2020), # CarTestRoute("8de015561e1ea4a0|2023-08-29--17-08-31", SUBARU.IMPREZA), # openpilot longitudinal + # CarTestRoute("c3d1ccb52f5f9d65|2023-07-22--01-23-20", SUBARU.OUTBACK, segment=9), # gen2 longitudinal, eyesight disabled CarTestRoute("1bbe6bf2d62f58a8|2022-07-14--17-11-43", SUBARU.OUTBACK, segment=10), CarTestRoute("c56e69bbc74b8fad|2022-08-18--09-43-51", SUBARU.LEGACY, segment=3), CarTestRoute("f4e3a0c511a076f4|2022-08-04--16-16-48", SUBARU.CROSSTREK_HYBRID, segment=2),