diff --git a/release/files_common b/release/files_common index bac9b382d629d8..a229954221a40e 100644 --- a/release/files_common +++ b/release/files_common @@ -569,6 +569,7 @@ opendbc/nissan_x_trail_2017_generated.dbc opendbc/nissan_leaf_2018_generated.dbc opendbc/subaru_global_2017_generated.dbc +opendbc/subaru_global_2020_hybrid_generated.dbc opendbc/subaru_outback_2015_generated.dbc opendbc/subaru_outback_2019_generated.dbc opendbc/subaru_forester_2017_generated.dbc diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 00915808d0272c..05524d4524aa4f 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -3,6 +3,7 @@ from openpilot.selfdrive.car import apply_driver_steer_torque_limits from openpilot.selfdrive.car.subaru import subarucan from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CanBus, CarControllerParams, SubaruFlags +from selfdrive.car.subaru.values import HYBRID_CARS class CarController: @@ -100,8 +101,9 @@ def update(self, CC, CS, now_nanos): self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle)) else: if pcm_cancel_cmd: - 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)) + 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)) new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 66c4c6386e7071..80eab10eba9779 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -6,6 +6,7 @@ from opendbc.can.parser import CANParser from openpilot.selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS, CanBus, SubaruFlags from openpilot.selfdrive.car import CanSignalRateCalculator +from selfdrive.car.subaru.values import HYBRID_CARS class CarState(CarStateBase): @@ -19,7 +20,9 @@ def __init__(self, CP): def update(self, cp, cp_cam, cp_body): ret = car.CarState.new_message() - ret.gas = cp.vl["Throttle"]["Throttle_Pedal"] / 255. + throttle_msg = cp.vl["Throttle"] if self.car_fingerprint not in HYBRID_CARS else cp_body.vl["Throttle_Hybrid"] + ret.gas = throttle_msg["Throttle_Pedal"] / 255. + ret.gasPressed = ret.gas > 1e-5 if self.car_fingerprint in PREGLOBAL_CARS: ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 2 @@ -46,7 +49,8 @@ def update(self, cp, cp_cam, cp_body): ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1) ret.rightBlindspot = (cp.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["R_APPROACHING"] == 1) - can_gear = int(cp.vl["Transmission"]["Gear"]) + cp_transmission = cp_body if self.car_fingerprint in HYBRID_CARS else cp + can_gear = int(cp_transmission.vl["Transmission"]["Gear"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"] @@ -62,8 +66,12 @@ def update(self, cp, cp_cam, cp_body): ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp - ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0 - ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0 + if self.car_fingerprint in HYBRID_CARS: + ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0 + ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0 + else: + ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0 + ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0 ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \ @@ -77,7 +85,7 @@ def update(self, cp, cp_cam, cp_body): cp.vl["BodyInfo"]["DOOR_OPEN_FL"]]) ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1 - cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam + cp_es_distance = cp_body if self.car_fingerprint in (GLOBAL_GEN2 | HYBRID_CARS) else cp_cam if self.car_fingerprint in PREGLOBAL_CARS: self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"] self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"] @@ -87,18 +95,23 @@ def update(self, cp, cp_cam, cp_body): ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3 ret.stockFcw = (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 1) or \ (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2) - # 8 is known AEB, there are a few other values related to AEB we ignore - ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \ - (cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0) + self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) cp_es_brake = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam self.es_brake_msg = copy.copy(cp_es_brake.vl["ES_Brake"]) cp_es_status = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam - self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"]) - self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"]) - self.brake_status_msg = copy.copy(cp_brakes.vl["Brake_Status"]) - self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) + if self.car_fingerprint not in HYBRID_CARS: # Hybrid cars don't have es_distance, need a replacement + # 8 is known AEB, there are a few other values related to AEB we ignore + ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \ + (cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0) + self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) + + self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"]) + self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"]) + + if self.car_fingerprint not in HYBRID_CARS: + self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"]) if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"]) @@ -106,44 +119,53 @@ def update(self, cp, cp_cam, cp_body): return ret @staticmethod - def get_common_global_body_messages(): + def get_common_global_body_messages(CP): messages = [ - ("CruiseControl", 20), ("Wheel_Speeds", 50), ("Brake_Status", 50), ] + if CP.carFingerprint not in HYBRID_CARS: + messages.append(("CruiseControl", 20)) + return messages @staticmethod - def get_common_global_es_messages(): + def get_common_global_es_messages(CP): messages = [ ("ES_Brake", 20), - ("ES_Distance", 20), - ("ES_Status", 20), - ("ES_Brake", 20), ] + if CP.carFingerprint not in HYBRID_CARS: + messages += [ + ("ES_Distance", 20), + ("ES_Status", 20) + ] + return messages @staticmethod def get_can_parser(CP): messages = [ # sig_address, frequency - ("Throttle", 100), ("Dashlights", 10), - ("Brake_Pedal", 50), - ("Transmission", 100), ("Steering_Torque", 50), ("BodyInfo", 1), + ("Brake_Pedal", 50), ] + if CP.carFingerprint not in HYBRID_CARS: + messages += [ + ("Throttle", 100), + ("Transmission", 100) + ] + if CP.enableBsm: messages.append(("BSD_RCTA", 17)) if CP.carFingerprint not in PREGLOBAL_CARS: if CP.carFingerprint not in GLOBAL_GEN2: - messages += CarState.get_common_global_body_messages() + messages += CarState.get_common_global_body_messages(CP) messages += [ ("Dashlights", 10), @@ -184,7 +206,7 @@ def get_cam_can_parser(CP): ] if CP.carFingerprint not in GLOBAL_GEN2: - messages += CarState.get_common_global_es_messages() + messages += CarState.get_common_global_es_messages(CP) if CP.flags & SubaruFlags.SEND_INFOTAINMENT: messages.append(("ES_Infotainment", 10)) @@ -193,9 +215,17 @@ def get_cam_can_parser(CP): @staticmethod def get_body_can_parser(CP): + messages = [] + if CP.carFingerprint in GLOBAL_GEN2: - messages = CarState.get_common_global_body_messages() - messages += CarState.get_common_global_es_messages() - return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.alt) + messages += CarState.get_common_global_body_messages(CP) + messages += CarState.get_common_global_es_messages(CP) + + if CP.carFingerprint in HYBRID_CARS: + messages += [ + ("Throttle_Hybrid", 40), + ("Transmission", 100) + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.alt) - return None diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index b206a816ac0540..e6e69316f82737 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -3,7 +3,7 @@ from panda import Panda from openpilot.selfdrive.car import get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase -from openpilot.selfdrive.car.subaru.values import CAR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags +from openpilot.selfdrive.car.subaru.values import CAR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, SubaruFlags class CarInterface(CarInterfaceBase): @@ -12,7 +12,11 @@ class CarInterface(CarInterfaceBase): def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "subaru" ret.radarUnavailable = True - ret.dashcamOnly = candidate in (PREGLOBAL_CARS | LKAS_ANGLE) + # for HYBRID CARS to be upstreamed, we need: + # - replacement for ES_Distance so we can cancel the cruise control + # - to find the Cruise_Activated bit from the car + # - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc) + ret.dashcamOnly = candidate in (PREGLOBAL_CARS | LKAS_ANGLE | HYBRID_CARS) ret.autoResumeSng = False # Detect infotainment message sent from the camera @@ -68,6 +72,13 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]] + elif candidate == CAR.CROSSTREK_HYBRID: + ret.mass = 1668. + ret.wheelbase = 2.67 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 17 + ret.steerActuatorDelay = 0.1 + elif candidate in (CAR.FORESTER, CAR.FORESTER_2022): ret.mass = 1568. ret.wheelbase = 2.67 @@ -107,7 +118,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): else: raise ValueError(f"unknown car: {candidate}") - #ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE) + #ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE | HYBRID_CARS) ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable if ret.openpilotLongitudinalControl: diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 8cb3a904b2a2a0..4cc1df97541960 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -70,6 +70,7 @@ class CAR: IMPREZA_2020 = "SUBARU IMPREZA SPORT 2020" FORESTER = "SUBARU FORESTER 2019" OUTBACK = "SUBARU OUTBACK 6TH GEN" + CROSSTREK_HYBRID = "SUBARU CROSSTREK HYBRID 2020" LEGACY = "SUBARU LEGACY 7TH GEN" FORESTER_2022 = "SUBARU FORESTER 2022" OUTBACK_2023 = "SUBARU OUTBACK 7TH GEN" @@ -110,6 +111,8 @@ def init_make(self, CP: car.CarParams): SubaruCarInfo("Subaru Crosstrek 2020-23"), SubaruCarInfo("Subaru XV 2020-21"), ], + # TODO: is there an XV and Impreza too? + CAR.CROSSTREK_HYBRID: SubaruCarInfo("Subaru Crosstrek Hybrid 2020"), CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"), CAR.FORESTER_PREGLOBAL: SubaruCarInfo("Subaru Forester 2017-18"), CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"), @@ -325,6 +328,24 @@ def init_make(self, CP: car.CarParams): b'\xe9\xf6B0\x00', ], }, + CAR.CROSSTREK_HYBRID: { + (Ecu.abs, 0x7b0, None): [ + b'\xa2 \x19e\x01', + b'\xa2 !e\x01', + ], + (Ecu.eps, 0x746, None): [ + b'\x9a\xc2\x01\x00', + b'\n\xc2\x01\x00', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x00\x00el\x1f@ #', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xd7!`@\x07', + b'\xd7!`p\a', + b'\xf4!`0\x07', + ], + }, CAR.FORESTER: { (Ecu.abs, 0x7b0, None): [ b'\xa3 \x18\x14\x00', @@ -628,6 +649,7 @@ def init_make(self, CP: car.CarParams): CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None), CAR.FORESTER_2022: dbc_dict('subaru_global_2017_generated', None), CAR.OUTBACK: dbc_dict('subaru_global_2017_generated', None), + CAR.CROSSTREK_HYBRID: dbc_dict('subaru_global_2020_hybrid_generated', None), CAR.OUTBACK_2023: dbc_dict('subaru_global_2017_generated', None), CAR.LEGACY: dbc_dict('subaru_global_2017_generated', None), CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None), @@ -639,3 +661,4 @@ def init_make(self, CP: car.CarParams): LKAS_ANGLE = {CAR.FORESTER_2022, CAR.OUTBACK_2023} GLOBAL_GEN2 = {CAR.OUTBACK, CAR.LEGACY, CAR.OUTBACK_2023} PREGLOBAL_CARS = {CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018} +HYBRID_CARS = {CAR.CROSSTREK_HYBRID, } \ No newline at end of file diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 60b99a1ad6a5d1..4ecf6e192a744d 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -240,6 +240,7 @@ class CarTestRoute(NamedTuple): CarTestRoute("8bf7e79a3ce64055|2021-05-24--09-36-27", SUBARU.IMPREZA_2020), 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), CarTestRoute("7fd1e4f3a33c1673|2022-12-04--15-09-53", SUBARU.FORESTER_2022, segment=4), CarTestRoute("f3b34c0d2632aa83|2023-07-23--20-43-25", SUBARU.OUTBACK_2023, segment=7), # Pre-global, dashcam diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index d79dbe8573d17e..0f742e6f3998b5 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -76,6 +76,7 @@ VOLKSWAGEN POLO 6TH GEN: VOLKSWAGEN GOLF 7TH GEN SEAT LEON 3RD GEN: VOLKSWAGEN GOLF 7TH GEN SEAT ATECA 1ST GEN: VOLKSWAGEN GOLF 7TH GEN +SUBARU CROSSTREK HYBRID 2020: SUBARU IMPREZA SPORT 2020 SUBARU LEGACY 7TH GEN: SUBARU OUTBACK 6TH GEN # Old subarus don't have much data guessing it's like low torque impreza