diff --git a/common/params_keys.h b/common/params_keys.h index d4346f4b0..d050499fb 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -627,6 +627,7 @@ inline static std::unordered_map keys = { {"VeryLongStarButtonControl", {PERSISTENT, INT, "0", "0", 2}}, {"VoltSNG", {PERSISTENT, BOOL, "0", "0", 2}}, {"GMAutoHold", {PERSISTENT, BOOL, "0", "0", 2}}, + {"ToyotaAutoHold", {PERSISTENT, BOOL, "0", "0", 2}}, {"WarningImmediateVolume", {PERSISTENT, INT, "101", "101", 2}}, {"WarningSoftVolume", {PERSISTENT, INT, "101", "101", 2}}, {"WeatherPresets", {PERSISTENT, BOOL, "0", "0", 2}}, diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 773e94ac2..5af38ccb4 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -709,13 +709,13 @@ class TestHyundaiFingerprint: def update(lkas_button: int, frame: int): msg = packer.make_can_msg("CLU13", 1, { - "CF_Clu_SWL_Stat": lkas_button, + "CF_Clu_LdwsLkasSW": lkas_button, }) can_parsers[Bus.alt].update([(frame, [msg])]) return car_state.update(can_parsers, toggles)[0] update(0, 1) - ret = update(4, 2) + ret = update(1, 2) assert any(be.type == ButtonType.lkas and be.pressed for be in ret.buttonEvents) assert any(be.type == ButtonType.lkas and not be.pressed for be in ret.buttonEvents) @@ -733,18 +733,42 @@ class TestHyundaiFingerprint: def update(lkas_button: int, frame: int): msg = packer.make_can_msg("CLU13", 1, { - "CF_Clu_SWL_Stat": lkas_button, + "CF_Clu_LdwsLkasSW": lkas_button, }) can_parsers[Bus.alt].update([(frame, [msg])]) return car_state.update(can_parsers, toggles)[0] - ret = update(4, 1) + ret = update(1, 1) assert not any(be.type == ButtonType.lkas for be in ret.buttonEvents) ret = update(0, 2) assert not any(be.type == ButtonType.lkas for be in ret.buttonEvents) - ret = update(4, 3) + ret = update(1, 3) + assert any(be.type == ButtonType.lkas and be.pressed for be in ret.buttonEvents) + assert any(be.type == ButtonType.lkas and not be.pressed for be in ret.buttonEvents) + + def test_sonata_alt_bus_clu13_swl_stat_lkas_button_event(self): + toggles = get_test_toggles() + fingerprint = gen_empty_fingerprint() + fingerprint[0][0x391] = 8 + fingerprint[1][0x50C] = 8 + CP = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, [], False, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(CAR.HYUNDAI_SONATA, fingerprint, [], CP, toggles) + + car_state = CarState(CP, FPCP) + can_parsers = car_state.get_can_parsers(CP) + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + + def update(lkas_button: int, frame: int): + msg = packer.make_can_msg("CLU13", 1, { + "CF_Clu_SWL_Stat": lkas_button, + }) + can_parsers[Bus.alt].update([(frame, [msg])]) + return car_state.update(can_parsers, toggles)[0] + + update(0, 1) + ret = update(4, 2) assert any(be.type == ButtonType.lkas and be.pressed for be in ret.buttonEvents) assert any(be.type == ButtonType.lkas and not be.pressed for be in ret.buttonEvents) diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 8446b8827..45da0f9cf 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -919,7 +919,6 @@ ALT_BUS_LDA_BUTTON_CARS = frozenset({ # steering-wheel-status field instead of the dedicated LKAS bit. ALT_BUS_LDA_BUTTON_SWL_STAT_CARS = frozenset({ CAR.HYUNDAI_SONATA, - CAR.HYUNDAI_SONATA_HYBRID, }) diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 5265b1dd5..14f04e7be 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -43,6 +43,7 @@ MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut MAX_USER_TORQUE = 500 PARK = structs.CarState.GearShifter.park +REVERSE = structs.CarState.GearShifter.reverse # Lock / unlock door commands - Credit goes to AlexandreSato! LOCK_CMD = b"\x40\x05\x30\x11\x00\x80\x00\x00" @@ -192,6 +193,11 @@ class CarController(CarControllerBase): self.secoc_prev_reset_counter = 0 self.doors_locked = False + self.auto_brake_hold = bool(self.CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value) + self.brake_hold_active = False + self._brake_hold_counter = 0 + self._brake_hold_reset = False + self._prev_brake_pressed = False def _compute_interceptor_gas_cmd(self, CC, CS): if not (self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl and CC.longActive): @@ -237,6 +243,27 @@ class CarController(CarControllerBase): self.last_standstill = CS.out.standstill + def create_auto_brake_hold_messages(self, CS: structs.CarState, brake_hold_allowed_timer: int = 100): + can_sends = [] + brake_hold_allowed = (CS.out.standstill and CS.out.cruiseState.available and + not CS.out.gasPressed and not CS.out.cruiseState.enabled and + CS.out.gearShifter not in (PARK, REVERSE)) + + if brake_hold_allowed: + self._brake_hold_counter += 1 + self.brake_hold_active = self._brake_hold_counter > brake_hold_allowed_timer and not self._brake_hold_reset + self._brake_hold_reset = not self._prev_brake_pressed and CS.out.brakePressed and not self._brake_hold_reset + else: + self._brake_hold_counter = 0 + self.brake_hold_active = False + self._brake_hold_reset = False + self._prev_brake_pressed = CS.out.brakePressed + + if self.frame % 2 == 0: + can_sends.append(toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active)) + + return can_sends + def update(self, CC, CS, now_nanos, starpilot_toggles): actuators = CC.actuators stopping = actuators.longControlState == LongCtrlState.stopping @@ -330,6 +357,9 @@ class CarController(CarControllerBase): # *** gas and brake *** self._update_standstill_request(CC, CS, actuators, starpilot_toggles) + if self.auto_brake_hold: + can_sends.extend(self.create_auto_brake_hold_messages(CS)) + interceptor_gas_cmd = self._compute_interceptor_gas_cmd(CC, CS) # handle UI messages diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index 1d91de1f6..d73b8663d 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/opendbc_repo/opendbc/car/toyota/carstate.py @@ -80,6 +80,8 @@ class CarState(CarStateBase): self.has_can_filter = self.FPCP.flags & ToyotaStarPilotFlags.RADAR_CAN_FILTER.value self.has_SDSU = self.FPCP.flags & ToyotaStarPilotFlags.SMART_DSU.value self.has_ZSS = self.FPCP.flags & ToyotaStarPilotFlags.ZSS.value + self.auto_brake_hold = bool(self.CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value) + self.pre_collision_2 = {} def update(self, can_parsers, starpilot_toggles) -> structs.CarState: cp = can_parsers[Bus.pt] @@ -211,6 +213,9 @@ class CarState(CarStateBase): if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V: self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"]) + if self.auto_brake_hold: + self.pre_collision_2 = copy.copy(cp_cam.vl["PRE_COLLISION_2"]) + if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR: self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"] diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index d7da4c159..fc5b7190a 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -2,11 +2,13 @@ from opendbc.car import Bus, structs, get_safety_config, uds from opendbc.car.toyota.carstate import CarState from opendbc.car.toyota.carcontroller import CarController from opendbc.car.toyota.radar_interface import RadarInterface -from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ +from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, SECOC_CAR, NO_DSU_CAR, \ MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \ ToyotaSafetyFlags from opendbc.car.disable_ecu import disable_ecu from opendbc.car.interfaces import CarInterfaceBase +from opendbc.safety import ALTERNATIVE_EXPERIENCE +from openpilot.common.params import Params SteerControlType = structs.CarParams.SteerControlType @@ -138,6 +140,11 @@ class CarInterface(CarInterfaceBase): ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR ret.enableGasInterceptorDEPRECATED = 0x201 in fingerprint[0] and ret.openpilotLongitudinalControl + toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold") + if toyota_auto_hold and candidate in (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR): + ret.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB + ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value + if not ret.openpilotLongitudinalControl: ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index 310b5c03d..27524a6d5 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -2,7 +2,7 @@ from types import SimpleNamespace from hypothesis import given, settings, strategies as st -from opendbc.car import Bus +from opendbc.car import Bus, structs from opendbc.can import CANPacker, CANParser from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict @@ -14,7 +14,9 @@ from opendbc.car.toyota.fingerprints import FW_VERSIONS from opendbc.car.toyota.interface import CarInterface from opendbc.car.toyota.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \ FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \ - get_platform_codes + ToyotaFlags, get_platform_codes +from opendbc.safety import ALTERNATIVE_EXPERIENCE +from openpilot.common.params import Params Ecu = CarParams.Ecu @@ -40,6 +42,25 @@ class TestToyotaInterfaces: if car_model in TSS2_CAR and car_model not in SECOC_CAR: assert dbc[Bus.pt] == "toyota_nodsu_pt_generated" + def test_auto_hold_sets_flag_on_supported_tss2(self): + params = Params() + try: + params.put_bool("ToyotaAutoHold", True) + car_params = CarInterface.get_params( + CAR.TOYOTA_CAMRY_TSS2, + {bus: {} for bus in range(8)}, + [], + alpha_long=False, + is_release=False, + docs=False, + starpilot_toggles=SimpleNamespace(), + ) + finally: + params.remove("ToyotaAutoHold") + + assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value + assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB + def test_essential_ecus(self, subtests): # Asserts standard ECUs exist for each platform common_ecus = {Ecu.fwdRadar, Ecu.fwdCamera} @@ -297,6 +318,33 @@ class TestToyotaCarController: assert parser.vl["LKAS_HUD"]["LEFT_LINE"] == 0 assert parser.vl["LKAS_HUD"]["RIGHT_LINE"] == 0 + def test_auto_brake_hold_sends_modified_pre_collision_after_timer(self): + controller = self._make_controller() + controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt]) + controller.frame = 0 + controller.brake_hold_active = False + controller._brake_hold_counter = 0 + controller._brake_hold_reset = False + controller._prev_brake_pressed = False + cs = SimpleNamespace( + out=SimpleNamespace( + standstill=True, + cruiseState=SimpleNamespace(available=True, enabled=False), + gasPressed=False, + brakePressed=False, + gearShifter=structs.CarState.GearShifter.drive, + ), + pre_collision_2={}, + ) + + can_sends = controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0) + + parser = CANParser(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt], [("PRE_COLLISION_2", 0)], 0) + parser.update([(1, can_sends)]) + assert controller.brake_hold_active + assert parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0 + assert parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 1 + def test_interceptor_stop_and_go_holds_small_launch_at_standstill(self): controller = self._make_controller() controller.CP.enableGasInterceptorDEPRECATED = True diff --git a/opendbc_repo/opendbc/car/toyota/toyotacan.py b/opendbc_repo/opendbc/car/toyota/toyotacan.py index da2771a7b..394b59851 100644 --- a/opendbc_repo/opendbc/car/toyota/toyotacan.py +++ b/opendbc_repo/opendbc/car/toyota/toyotacan.py @@ -86,6 +86,38 @@ def create_pcs_commands(packer, accel, active, mass): return [msg1, msg2] +def create_brake_hold_command(packer, frame, pre_collision_2, brake_hold_active): + values = {s: pre_collision_2[s] for s in [ + "DSS1GDRV", + "DS1STAT2", + "DS1STBK2", + "PCSWAR", + "PCSALM", + "PCSOPR", + "PCSABK", + "PBATRGR", + "PPTRGR", + "IBTRGR", + "CLEXTRGR", + "IRLT_REQ", + "BRKHLD", + "AVSTRGR", + "VGRSTRGR", + "PREFILL", + "PBRTRGR", + "PCSDIS", + "PBPREPMP", + ] if s in pre_collision_2} + + if brake_hold_active: + values = { + "DSS1GDRV": 0x3FF, + "PBRTRGR": frame % 730 < 727, + } + + return packer.make_can_msg("PRE_COLLISION_2", 0, values) + + def create_acc_cancel_command(packer): values = { "GAS_RELEASED": 0, diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index 0ae977a21..1f9587381 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -75,6 +75,7 @@ class ToyotaFlags(IntFlag): # these cars can utilize 2.0 m/s^2 RAISED_ACCEL_LIMIT = 1024 SECOC = 2048 + AUTO_BRAKE_HOLD = 4096 # deprecated flags # these cars are speculated to allow stop and go when the DSU is unplugged diff --git a/opendbc_repo/opendbc/dbc/generator/toyota/_toyota_adas_standard.dbc b/opendbc_repo/opendbc/dbc/generator/toyota/_toyota_adas_standard.dbc index 20680225a..bd22a9548 100644 --- a/opendbc_repo/opendbc/dbc/generator/toyota/_toyota_adas_standard.dbc +++ b/opendbc_repo/opendbc/dbc/generator/toyota/_toyota_adas_standard.dbc @@ -37,11 +37,24 @@ BO_ 740 STEERING_LKA: 5 XXX BO_ 836 PRE_COLLISION_2: 8 DSU SG_ DSS1GDRV : 7|10@0- (0.1,0) [0|0] "m/s^2" Vector__XXX + SG_ DS1STAT2 : 13|3@0+ (1,0) [0|0] "" Vector__XXX + SG_ DS1STBK2 : 10|3@0+ (1,0) [0|0] "" Vector__XXX + SG_ PCSWAR : 18|1@0+ (1,0) [0|0] "" FCM SG_ PCSALM : 17|1@0+ (1,0) [0|0] "" FCM + SG_ PCSOPR : 16|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ PCSABK : 31|1@0+ (1,0) [0|0] "" Vector__XXX SG_ IBTRGR : 27|1@0+ (1,0) [0|0] "" FCM SG_ PBATRGR : 30|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ PPTRGR : 28|1@0+ (1,0) [0|0] "" FCM + SG_ CLEXTRGR : 26|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ IRLT_REQ : 25|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ BRKHLD : 37|1@0+ (1,0) [0|0] "" Vector__XXX SG_ PREFILL : 33|1@0+ (1,0) [0|0] "" Vector__XXX SG_ AVSTRGR : 36|1@0+ (1,0) [0|0] "" SCS + SG_ VGRSTRGR : 35|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ PBRTRGR : 32|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ PCSDIS : 43|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ PBPREPMP : 40|1@0+ (1,0) [0|0] "" Vector__XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input"; diff --git a/opendbc_repo/opendbc/dbc/toyota_new_mc_pt_generated.dbc b/opendbc_repo/opendbc/dbc/toyota_new_mc_pt_generated.dbc index 1efcc52df..0823d607e 100644 --- a/opendbc_repo/opendbc/dbc/toyota_new_mc_pt_generated.dbc +++ b/opendbc_repo/opendbc/dbc/toyota_new_mc_pt_generated.dbc @@ -650,11 +650,24 @@ BO_ 740 STEERING_LKA: 5 XXX BO_ 836 PRE_COLLISION_2: 8 DSU SG_ DSS1GDRV : 7|10@0- (0.1,0) [0|0] "m/s^2" Vector__XXX + SG_ DS1STAT2 : 13|3@0+ (1,0) [0|0] "" Vector__XXX + SG_ DS1STBK2 : 10|3@0+ (1,0) [0|0] "" Vector__XXX + SG_ PCSWAR : 18|1@0+ (1,0) [0|0] "" FCM SG_ PCSALM : 17|1@0+ (1,0) [0|0] "" FCM + SG_ PCSOPR : 16|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ PCSABK : 31|1@0+ (1,0) [0|0] "" Vector__XXX SG_ IBTRGR : 27|1@0+ (1,0) [0|0] "" FCM SG_ PBATRGR : 30|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ PPTRGR : 28|1@0+ (1,0) [0|0] "" FCM + SG_ CLEXTRGR : 26|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ IRLT_REQ : 25|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ BRKHLD : 37|1@0+ (1,0) [0|0] "" Vector__XXX SG_ PREFILL : 33|1@0+ (1,0) [0|0] "" Vector__XXX SG_ AVSTRGR : 36|1@0+ (1,0) [0|0] "" SCS + SG_ VGRSTRGR : 35|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ PBRTRGR : 32|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ PCSDIS : 43|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ PBPREPMP : 40|1@0+ (1,0) [0|0] "" Vector__XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input"; diff --git a/opendbc_repo/opendbc/dbc/toyota_nodsu_pt_generated.dbc b/opendbc_repo/opendbc/dbc/toyota_nodsu_pt_generated.dbc index 8506729ef..76023e3cf 100644 --- a/opendbc_repo/opendbc/dbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc_repo/opendbc/dbc/toyota_nodsu_pt_generated.dbc @@ -650,11 +650,24 @@ BO_ 740 STEERING_LKA: 5 XXX BO_ 836 PRE_COLLISION_2: 8 DSU SG_ DSS1GDRV : 7|10@0- (0.1,0) [0|0] "m/s^2" Vector__XXX + SG_ DS1STAT2 : 13|3@0+ (1,0) [0|0] "" Vector__XXX + SG_ DS1STBK2 : 10|3@0+ (1,0) [0|0] "" Vector__XXX + SG_ PCSWAR : 18|1@0+ (1,0) [0|0] "" FCM SG_ PCSALM : 17|1@0+ (1,0) [0|0] "" FCM + SG_ PCSOPR : 16|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ PCSABK : 31|1@0+ (1,0) [0|0] "" Vector__XXX SG_ IBTRGR : 27|1@0+ (1,0) [0|0] "" FCM SG_ PBATRGR : 30|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ PPTRGR : 28|1@0+ (1,0) [0|0] "" FCM + SG_ CLEXTRGR : 26|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ IRLT_REQ : 25|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ BRKHLD : 37|1@0+ (1,0) [0|0] "" Vector__XXX SG_ PREFILL : 33|1@0+ (1,0) [0|0] "" Vector__XXX SG_ AVSTRGR : 36|1@0+ (1,0) [0|0] "" SCS + SG_ VGRSTRGR : 35|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ PBRTRGR : 32|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ PCSDIS : 43|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ PBPREPMP : 40|1@0+ (1,0) [0|0] "" Vector__XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input"; diff --git a/opendbc_repo/opendbc/dbc/toyota_tnga_k_pt_generated.dbc b/opendbc_repo/opendbc/dbc/toyota_tnga_k_pt_generated.dbc index 75357cc2b..b396a6aba 100644 --- a/opendbc_repo/opendbc/dbc/toyota_tnga_k_pt_generated.dbc +++ b/opendbc_repo/opendbc/dbc/toyota_tnga_k_pt_generated.dbc @@ -650,11 +650,24 @@ BO_ 740 STEERING_LKA: 5 XXX BO_ 836 PRE_COLLISION_2: 8 DSU SG_ DSS1GDRV : 7|10@0- (0.1,0) [0|0] "m/s^2" Vector__XXX + SG_ DS1STAT2 : 13|3@0+ (1,0) [0|0] "" Vector__XXX + SG_ DS1STBK2 : 10|3@0+ (1,0) [0|0] "" Vector__XXX + SG_ PCSWAR : 18|1@0+ (1,0) [0|0] "" FCM SG_ PCSALM : 17|1@0+ (1,0) [0|0] "" FCM + SG_ PCSOPR : 16|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ PCSABK : 31|1@0+ (1,0) [0|0] "" Vector__XXX SG_ IBTRGR : 27|1@0+ (1,0) [0|0] "" FCM SG_ PBATRGR : 30|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ PPTRGR : 28|1@0+ (1,0) [0|0] "" FCM + SG_ CLEXTRGR : 26|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ IRLT_REQ : 25|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ BRKHLD : 37|1@0+ (1,0) [0|0] "" Vector__XXX SG_ PREFILL : 33|1@0+ (1,0) [0|0] "" Vector__XXX SG_ AVSTRGR : 36|1@0+ (1,0) [0|0] "" SCS + SG_ VGRSTRGR : 35|2@0+ (1,0) [0|0] "" Vector__XXX + SG_ PBRTRGR : 32|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ PCSDIS : 43|1@0+ (1,0) [0|0] "" Vector__XXX + SG_ PBPREPMP : 40|1@0+ (1,0) [0|0] "" Vector__XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input"; diff --git a/opendbc_repo/opendbc/safety/modes/toyota.h b/opendbc_repo/opendbc/safety/modes/toyota.h index 0b370759d..d297c289f 100644 --- a/opendbc_repo/opendbc/safety/modes/toyota.h +++ b/opendbc_repo/opendbc/safety/modes/toyota.h @@ -351,6 +351,13 @@ static bool toyota_tx_hook(const CANPacket_t *msg) { } } } + + // Auto brake hold replaces the camera AEB message only while stopped. + if ((msg->addr == 0x344U) && ((alternative_experience & ALT_EXP_ALLOW_AEB) != 0)) { + if (vehicle_moving || gas_pressed || !acc_main_on) { + tx = false; + } + } } // UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address @@ -458,10 +465,20 @@ static safety_config toyota_init(uint16_t param) { return ret; } +static bool toyota_fwd_hook(int bus_num, int addr) { + bool block_msg = false; + if (bus_num == 2) { + block_msg = (addr == 0x344) && ((alternative_experience & ALT_EXP_ALLOW_AEB) != 0) && + !vehicle_moving && !gas_pressed && acc_main_on; + } + return block_msg; +} + const safety_hooks toyota_hooks = { .init = toyota_init, .rx = toyota_rx_hook, .tx = toyota_tx_hook, + .fwd = toyota_fwd_hook, .get_checksum = toyota_get_checksum, .compute_checksum = toyota_compute_checksum, .get_quality_flag_valid = toyota_get_quality_flag_valid, diff --git a/opendbc_repo/opendbc/safety/tests/test_toyota.py b/opendbc_repo/opendbc/safety/tests/test_toyota.py index e7fe2bb8e..c7c6712ec 100644 --- a/opendbc_repo/opendbc/safety/tests/test_toyota.py +++ b/opendbc_repo/opendbc/safety/tests/test_toyota.py @@ -9,6 +9,7 @@ from opendbc.car.structs import CarParams from opendbc.safety.tests.libsafety import libsafety_py import opendbc.safety.tests.common as common from opendbc.safety.tests.common import CANPackerSafety +from opendbc.safety import ALTERNATIVE_EXPERIENCE TOYOTA_COMMON_TX_MSGS = [[0x2E4, 0], [0x191, 0], [0x412, 0], [0x343, 0], [0x1D2, 0], [0x1D3, 0]] # LKAS + LTA + ACC & PCM cancel cmds TOYOTA_SECOC_TX_MSGS = [[0x131, 0], [0x183, 0]] + TOYOTA_COMMON_TX_MSGS @@ -94,6 +95,30 @@ class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyT msg = libsafety_py.make_CANPacket(0x283, 0, bytes(dat)) self.assertEqual(not bad and not stock_longitudinal, self._tx(msg)) + def test_auto_brake_hold_aeb_replacement_only_at_standstill(self): + self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALLOW_AEB) + hold_msg = libsafety_py.make_CANPacket(0x344, 0, b"\xfd\x80\x00\x00\x00\x00\x00\xcc") + + self._rx(self._speed_msg(0)) + self._rx(self._toggle_aol(True)) + self._rx(self._user_gas_msg(False)) + self.assertTrue(self._tx(hold_msg)) + self.assertEqual(-1, self.safety.safety_fwd_hook(2, 0x344)) + + self._rx(self._speed_msg(1.0)) + self.assertFalse(self._tx(hold_msg)) + self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344)) + + self._rx(self._speed_msg(0)) + self._rx(self._user_gas_msg(True)) + self.assertFalse(self._tx(hold_msg)) + self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344)) + + self._rx(self._user_gas_msg(False)) + self._rx(self._toggle_aol(False)) + self.assertFalse(self._tx(hold_msg)) + self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344)) + # Only allow LTA msgs with no actuation def test_lta_steer_cmd(self): for engaged, req, req2, torque_wind_down, angle in itertools.product([True, False], diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 737a3352a..8d77cccf8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -2307,7 +2307,7 @@ class LongitudinalPlanner: if close_final_guard_caps: close_final_guard_cap = min(close_final_guard_caps) - if lead_one_active: + if allow_complex_follow_logic and lead_one_active: lead_catchup_accel_cap = self.get_lead_catchup_accel_cap(self.lead_one, scene_v_ego, effective_t_follow) if lead_catchup_accel_cap is not None: self.a_desired = min(self.a_desired, lead_catchup_accel_cap) @@ -2328,7 +2328,7 @@ class LongitudinalPlanner: effective_t_follow, allow_optional_far_lead_logic=allow_complex_follow_logic, ) - if follow_control_lead is not None and not panic_bypass: + if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass: if not output_should_stop and not vision_low_speed_stop_active: tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor( follow_control_lead, @@ -2366,7 +2366,7 @@ class LongitudinalPlanner: self.a_desired = max(self.a_desired, far_lead_brake_cap) output_a_target = max(output_a_target, far_lead_brake_cap) - if follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: + if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: tracked_vision_model_brake_cap = self.get_tracked_vision_model_brake_cap( follow_control_lead, scene_v_ego, @@ -2411,7 +2411,7 @@ class LongitudinalPlanner: self.a_desired = max(self.a_desired, near_duplicate_transition_target) output_a_target = near_duplicate_transition_target - if follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: + if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active: cruise_tracking_lead_accel_cap = self.get_cruise_tracking_lead_accel_cap( follow_control_lead, scene_v_ego, diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 18d1d6cd3..0eb3bca71 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -111,7 +111,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta } -def make_toggles(model_version: str = "v11", radar_takeoffs: bool = False): +def make_toggles(model_version: str = "v11", radar_takeoffs: bool = False, prioritize_smooth_following: bool = False): return SimpleNamespace( taco_tune=False, classic_model=False, @@ -120,6 +120,7 @@ def make_toggles(model_version: str = "v11", radar_takeoffs: bool = False): stop_distance=6.0, vEgoStopping=0.5, radar_takeoffs=radar_takeoffs, + prioritize_smooth_following=prioritize_smooth_following, ) @@ -2190,6 +2191,59 @@ def test_cruise_tracking_lead_accel_transition_target_skips_clear_pullaway(): assert smoothed is None +def test_prioritize_smooth_following_skips_post_097_follow_nudges(): + v_ego = 16.2 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + lead = make_lead(status=True, d_rel=33.4, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12) + + def run_once(prioritize_smooth_following: bool) -> list[str]: + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=0.8, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=lead, + ) + sm["starpilotPlan"].vCruise = v_ego + 8.0 + + calls = [] + + def record(name, return_value=None): + def _inner(self, *args, **kwargs): + calls.append(name) + return return_value + return types.MethodType(_inner, planner) + + planner.get_follow_control_lead = record("get_follow_control_lead", lead) + planner.get_lead_catchup_accel_cap = record("get_lead_catchup_accel_cap", None) + planner.get_tracked_vision_model_brake_floor = record("get_tracked_vision_model_brake_floor", None) + planner.get_low_speed_follow_transition_brake_cap = record("get_low_speed_follow_transition_brake_cap", None) + planner.get_tracked_vision_model_brake_cap = record("get_tracked_vision_model_brake_cap", None) + planner.get_cruise_tracking_lead_accel_cap = record("get_cruise_tracking_lead_accel_cap", None) + planner.get_cruise_tracking_lead_accel_transition_target = record("get_cruise_tracking_lead_accel_transition_target", None) + + planner.update(sm, make_toggles(prioritize_smooth_following=prioritize_smooth_following)) + return calls + + calls_default = run_once(False) + calls_smooth = run_once(True) + + expected_calls = { + "get_lead_catchup_accel_cap", + "get_follow_control_lead", + "get_tracked_vision_model_brake_floor", + "get_low_speed_follow_transition_brake_cap", + "get_tracked_vision_model_brake_cap", + "get_cruise_tracking_lead_accel_cap", + "get_cruise_tracking_lead_accel_transition_target", + } + + assert expected_calls.issubset(set(calls_default)) + assert not expected_calls.intersection(set(calls_smooth)) + + def test_near_duplicate_lead_source_hysteresis_prefers_previous_source(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 2d1e8e5b2..342a3d337 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -25,6 +25,7 @@ class Maneuver: self.e2e = kwargs.get("e2e", False) self.personality = kwargs.get("personality", 0) self.force_decel = kwargs.get("force_decel", False) + self.prioritize_smooth_following = kwargs.get("prioritize_smooth_following", False) self.duration = duration self.title = title @@ -41,6 +42,7 @@ class Maneuver: e2e=self.e2e, personality=self.personality, force_decel=self.force_decel, + prioritize_smooth_following=self.prioritize_smooth_following, ) valid = True diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 57ebd9539..beae16893 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -56,7 +56,8 @@ class Plant: def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, enabled=True, only_lead2=False, only_radar=False, track_lead_with_gate=False, - e2e=False, personality=0, force_decel=False): + e2e=False, personality=0, force_decel=False, + prioritize_smooth_following=False): self.rate = 1. / DT_MDL current_prefix = os.environ.get("OPENPILOT_PREFIX") @@ -91,6 +92,7 @@ class Plant: self.e2e = e2e self.personality = personality self.force_decel = force_decel + self.prioritize_smooth_following = prioritize_smooth_following self.tracking_lead_filter = FirstOrderFilter(0.0, 0.5, DT_MDL) self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) @@ -111,6 +113,7 @@ class Plant: stop_distance=6.0, longitudinalActuatorDelay=0.2, vEgoStopping=0.5, + prioritize_smooth_following=self.prioritize_smooth_following, ) @property diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 1340fa005..afbfe3a00 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -180,6 +180,7 @@ SAFE_MODE_MANAGED_KEYS = ( "RelaxedJerkSpeedDecrease", "FrogsGoMoosTweak", "SNGHack", + "ToyotaAutoHold", "SubaruSNG", "SubaruSNGManualParkingBrake", "VoltSNG", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 14b06b347..c6c1890c3 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -1190,6 +1190,7 @@ class StarPilotVariables: toggle.standby_mode = self.get_value("StandbyMode", condition=screen_management) toggle.sng_hack = self.get_value("SNGHack", condition=toggle.openpilot_longitudinal and toggle.car_make == "toyota" and not toggle.has_pedal and not has_sng) + toggle.toyota_auto_hold = self.get_value("ToyotaAutoHold", condition=toggle.car_make == "toyota") toggle.speed_limit_controller = toggle.openpilot_longitudinal and self.get_value("SpeedLimitController") speed_limit_display = toggle.show_speed_limits or toggle.speed_limit_controller diff --git a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json index d7fc075e8..36155209c 100644 --- a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json @@ -2502,6 +2502,13 @@ "data_type": "bool", "ui_type": "toggle" }, + { + "key": "ToyotaAutoHold", + "label": "Toyota Auto Hold", + "description": "Hold the brakes at a stop on supported Toyota/Lexus TSS2 vehicles when cruise main is available and cruise is not active.", + "data_type": "bool", + "ui_type": "toggle" + }, { "key": "RemapCancelToDistance", "label": "Remap Cancel Button",