diff --git a/cereal/custom.capnp b/cereal/custom.capnp index fd20cee92..54b825e59 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -59,6 +59,8 @@ struct StarPilotCarParams @0xaedffd8f31e7b55d { isHDA2 @4 :Bool; openpilotLongitudinalControlDisabled @5 :Bool; safetyConfigs @6 :List(SafetyConfig); + pcmCruiseSpeed @7 :Bool = true; + redneckCruiseAvailable @8 :Bool; struct SafetyConfig { safetyParam @0 :UInt16; diff --git a/cereal/libcereal.a b/cereal/libcereal.a index 7a1b02d1b..fbedbfeda 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/common/libcommon.a b/common/libcommon.a index d677cf3d3..b5e28f422 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index ff52dce53..bea75ad57 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -329,6 +329,7 @@ inline static std::unordered_map keys = { {"IncreasedStoppedDistanceRain", {PERSISTENT, FLOAT, "0.0", "0.0", 2}}, {"IncreasedStoppedDistanceRainStorm", {PERSISTENT, FLOAT, "0.0", "0.0", 2}}, {"IncreasedStoppedDistanceSnow", {PERSISTENT, FLOAT, "0.0", "0.0", 2}}, + {"RedneckCruise", {PERSISTENT, BOOL, "0", "0", 1}}, {"IncreaseFollowingLowVisibility", {PERSISTENT, FLOAT, "0.0", "0.0", 2}}, {"IncreaseFollowingRain", {PERSISTENT, FLOAT, "0.0", "0.0", 2}}, {"IncreaseFollowingRainStorm", {PERSISTENT, FLOAT, "0.0", "0.0", 2}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index d62346674..969b7d2e9 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 750cfdde8..5a6ea9aa0 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -57,6 +57,10 @@ IONIQ_6_STOP_RELEASE_JERK_BP = [0.0, 0.15, 0.5] IONIQ_6_STOP_RELEASE_JERK_V = [3.6 * IONIQ_6_RESPONSE_MULTIPLIER, 4.2 * IONIQ_6_RESPONSE_MULTIPLIER, 4.8 * IONIQ_6_RESPONSE_MULTIPLIER] +REDNECK_BUTTON_COPIES = 2 +REDNECK_BUTTON_COPIES_TIME = 7 +REDNECK_BUTTON_COPIES_TIME_IMPERIAL = [REDNECK_BUTTON_COPIES_TIME + 3, 70] +REDNECK_BUTTON_COPIES_TIME_METRIC = [REDNECK_BUTTON_COPIES_TIME, 40] @dataclass @@ -229,6 +233,7 @@ class CarController(CarControllerBase): self.apply_angle_last = 0.0 self.car_fingerprint = CP.carFingerprint self.last_button_frame = 0 + self.redneck_button_frame = 0 self.ecu_disable_failed = False self._ecu_disable_checked = False self._params = Params() @@ -277,6 +282,45 @@ class CarController(CarControllerBase): return False, 0.0, 0.0 + @staticmethod + def _get_redneck_button(CS): + return { + 1: Buttons.RES_ACCEL, + 2: Buttons.SET_DECEL, + }.get(getattr(CS, "redneck_send_button", 0), Buttons.NONE) + + def _create_can_redneck_button_messages(self, CS): + send_button = self._get_redneck_button(CS) + if send_button == Buttons.NONE or (self.frame - self.last_button_frame) * DT_CTRL <= 0.1: + return [] + + copies_xp = REDNECK_BUTTON_COPIES_TIME_METRIC if CS.is_metric else REDNECK_BUTTON_COPIES_TIME_IMPERIAL + copies = int(np.interp(REDNECK_BUTTON_COPIES_TIME, copies_xp, [1, REDNECK_BUTTON_COPIES])) + can_sends = [hyundaican.create_clu11(self.packer, self.frame, CS.clu11, send_button, self.CP)] * copies + + if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15: + self.last_button_frame = self.frame + + return can_sends + + def _create_canfd_redneck_button_messages(self, CS): + send_button = self._get_redneck_button(CS) + if send_button == Buttons.NONE or self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS or \ + (self.frame - self.last_button_frame) * DT_CTRL <= 0.2: + return [] + + self.redneck_button_frame += 1 + button_counter_offset = [1, 1, 0, None][self.redneck_button_frame % 4] + if button_counter_offset is None: + return [] + + can_sends = [ + hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, (CS.buttons_counter + button_counter_offset) % 0xF, send_button) + for _ in range(20) + ] + self.last_button_frame = self.frame + return can_sends + def update(self, CC, CS, now_nanos, starpilot_toggles): actuators = CC.actuators hud_control = CC.hudControl @@ -443,6 +487,8 @@ class CarController(CarControllerBase): can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP)] * 25) if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15: self.last_button_frame = self.frame + else: + can_sends.extend(self._create_can_redneck_button_messages(CS)) if self.long_active_ecu and can_canfd_blended: can_sends.extend(hyundaican.create_radar_aux_messages(self.packer, self.CAN, self.frame)) @@ -591,5 +637,7 @@ class CarController(CarControllerBase): for _ in range(20): can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter + 1, Buttons.RES_ACCEL)) self.last_button_frame = self.frame + else: + can_sends.extend(self._create_canfd_redneck_button_messages(CS)) return can_sends diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 656780963..5d0449cfb 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -27,6 +27,14 @@ IONIQ_6_BLINDSPOT_LEFT_MASK = 0x10 CANFD_CAMERA_LEAD_MIN_DISTANCE = 0.1 +def get_non_scc_cruise_signals(CP) -> tuple[str, str, str, str, str]: + if CP.flags & HyundaiFlags.EV: + return "LABEL11", "CC_React", "CC_ACT", "E_EMS11", "Cruise_Limit_Target" + if CP.flags & HyundaiFlags.HYBRID: + return "E_CRUISE_CONTROL", "CRUISE_LAMP_M", "CRUISE_LAMP_S", "ELECT_GEAR", "SLC_SET_SPEED" + return "EMS16", "CRUISE_LAMP_M", "CRUISE_LAMP_S", "LVR12", "CF_Lvr_CruiseSet" + + def calculate_canfd_speed_limit(CP, FPCP, cp, cp_cam, speed_factor): if not (FPCP.flags & HyundaiStarPilotFlags.SPEED_LIMIT_AVAILABLE): return 0.0 @@ -71,6 +79,8 @@ class CarState(CarStateBase): self.custom_button = 0 self.cancel_button_enable_in_progress = False self.cruise_buttons_msg = {} + self.redneck_send_button = Buttons.NONE + self.redneck_v_target = 0 self.gear_msg_canfd = "ACCELERATOR" if CP.flags & HyundaiFlags.EV else \ "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \ @@ -214,18 +224,12 @@ class CarState(CarStateBase): ret.cruiseState.standstill = False ret.cruiseState.nonAdaptive = False elif no_scc: - cruise_set_speed = cp.vl["LVR12"]["CF_Lvr_CruiseSet"] - cruise_has_set_speed = 0 < cruise_set_speed < 255 - # Some regular-cruise Forte trims never assert ACC_REQ even when stock cruise is engaged. - cruise_enabled = cp.vl["TCS13"]["ACC_REQ"] == 1 or cruise_has_set_speed - - # Regular-cruise Forte trims don't publish SCC11/SCC12; use the stock cruise request and set speed. - ret.cruiseState.available = cruise_enabled or cp.vl["TCS13"]["ACCEnable"] == 0 - ret.cruiseState.enabled = cruise_enabled + cruise_msg, cruise_available_sig, cruise_enabled_sig, cruise_speed_msg, cruise_speed_sig = get_non_scc_cruise_signals(self.CP) + ret.cruiseState.available = cp.vl[cruise_msg][cruise_available_sig] != 0 + ret.cruiseState.enabled = cp.vl[cruise_msg][cruise_enabled_sig] != 0 ret.cruiseState.standstill = False ret.cruiseState.nonAdaptive = False - if cruise_has_set_speed: - ret.cruiseState.speed = cruise_set_speed * speed_conv + ret.cruiseState.speed = cp.vl[cruise_speed_msg][cruise_speed_sig] * speed_conv else: scc_msg = "SCC12" if self.CP.flags & HyundaiFlags.CAN_CANFD_BLENDED else "SCC11" ret.cruiseState.available = cp_cruise.vl[scc_msg]["MainMode_ACC"] == 1 @@ -273,14 +277,21 @@ class CarState(CarStateBase): if (not self.CP.openpilotLongitudinalControl or self.CP.flags & HyundaiFlags.CAMERA_SCC) and \ not (self.CP.flags & HyundaiFlags.CAN_CANFD_BLENDED): - aeb_src = "FCA11" if self.CP.flags & HyundaiFlags.USE_FCA.value else "SCC12" - aeb_sig = "FCA_CmdAct" if self.CP.flags & HyundaiFlags.USE_FCA.value else "AEB_CmdAct" - aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0 - # Regular-cruise Forte trims don't publish SCC12; avoid poisoning parser validity on no-SCC cars. - scc_warning = False if no_scc else cp_cruise.vl["SCC12"]["TakeOverReq"] == 1 - aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0 - ret.stockFcw = (aeb_warning or scc_warning) and not aeb_braking - ret.stockAeb = aeb_warning and aeb_braking + if no_scc: + if not (self.CP.flags & HyundaiFlags.NON_SCC_NO_FCA): + cp_fca = cp if self.CP.flags & HyundaiFlags.NON_SCC_RADAR_FCA else cp_cam + aeb_warning = cp_fca.vl["FCA11"]["CF_VSM_Warn"] != 0 + aeb_braking = cp_fca.vl["FCA11"]["CF_VSM_DecCmdAct"] != 0 or cp_fca.vl["FCA11"]["FCA_CmdAct"] != 0 + ret.stockFcw = aeb_warning and not aeb_braking + ret.stockAeb = aeb_warning and aeb_braking + else: + aeb_src = "FCA11" if self.CP.flags & HyundaiFlags.USE_FCA.value else "SCC12" + aeb_sig = "FCA_CmdAct" if self.CP.flags & HyundaiFlags.USE_FCA.value else "AEB_CmdAct" + aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0 + scc_warning = cp_cruise.vl["SCC12"]["TakeOverReq"] == 1 + aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0 + ret.stockFcw = (aeb_warning or scc_warning) and not aeb_braking + ret.stockAeb = aeb_warning and aeb_braking if self.CP.enableBsm: ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0 @@ -496,8 +507,8 @@ class CarState(CarStateBase): return self.get_can_parsers_canfd(CP) msgs = [] - if CP.flags & HyundaiFlags.NON_SCC and CP.flags & HyundaiFlags.USE_FCA.value: - msgs.append(("FCA11", 0)) # no-SCC Forte trims can stop publishing FCA11; don't let it poison canValid + if CP.flags & HyundaiFlags.NON_SCC and not (CP.flags & HyundaiFlags.NON_SCC_NO_FCA): + msgs.append(("FCA11", 0)) # Non-SCC trims can stop publishing FCA11; don't let it poison canValid return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], msgs, 0), diff --git a/opendbc_repo/opendbc/car/hyundai/fingerprints.py b/opendbc_repo/opendbc/car/hyundai/fingerprints.py index 63157f820..5d9234c69 100644 --- a/opendbc_repo/opendbc/car/hyundai/fingerprints.py +++ b/opendbc_repo/opendbc/car/hyundai/fingerprints.py @@ -1311,4 +1311,90 @@ FW_VERSIONS = { b'\xf1\x00T01G00BL T01I00A1 DOS2T16X4XI00NS0\x99L\xeeq', ], }, + CAR.KIA_CEED_PHEV_2022_NON_SCC: { + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x00CD MDPS C 1.00 1.01 56310-XX000 4CPHC101', + ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00CDH LKAS AT EUR LHD 1.00 1.01 99211-CR700 931', + ], + }, + CAR.GENESIS_G70_2021_NON_SCC: { + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00IK MDPS R 1.00 1.08 57700-G9200 4I2CL108', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00IK__ SCC --CUP 1.00 1.02 96400-G9100 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00IK MFC MT USA LHD 1.00 1.01 95740-G9000 170920', + ], + }, + CAR.HYUNDAI_KONA_NON_SCC: { + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00OS MDPS C 1.00 1.05 56310J9030\x00 4OSDC105', + b'\xf1\x00OS MDPS C 1.00 1.04 56310J9030\x00 4OSDC104', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9200 g30', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x006T6J0_C2\x00\x006T6K1051\x00\x00TOS4N20NS2\x00\x00\x00\x00', + ], + }, + CAR.KIA_FORTE_2019_NON_SCC: { + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x00BD MDPS C 1.00 1.04 56310/M6000 4BDDC104', + b'\xf1\x00BD MDPS C 1.00 1.05 56310/M6000 4BDDC105', + ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00BD LKAS AT USA LHD 1.00 1.02 95740-M6000 J31', + ], + }, + CAR.KIA_FORTE_2021_NON_SCC: { + (Ecu.eps, 0x7D4, None): [ + b'\xf1\x00BD MDPS C 1.00 1.08 56310M6000\x00 4BDDC108', + ], + (Ecu.fwdCamera, 0x7C4, None): [ + b'\xf1\x00BD LKAS AT USA LHD 1.00 1.04 95740-M6000 J33', + ], + }, + CAR.KIA_SELTOS_2023_NON_SCC: { + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00SP ESC \t 101\"\t\x01 58910-Q5510', + b'\xf1\x00SP ESC \r 100\"\x04\x01 58910-Q5510', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00SP2 MDPS C 1.00 1.04 56310Q5240 4SPSC104', + b'\xf1\x00SP2 MDPS C 1.00 1.01 56300Q5920 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00SP2 MFC AT USA LHD 1.00 1.03 99210-Q5500 230208', + b'\xf1\x00SP2 MFC AT AUS RHD 1.00 1.02 99210-Q5500 220624', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x006V2B0_C2\x00\x006V2D5051\x00\x00CSP2N20NL0\x00\x00\x00\x00', + b'\xf1\x006V2B0_C2\x00\x006V2D4051\x00\x00CSP2N20KL1\x00\x00\x00\x00', + ], + }, + CAR.HYUNDAI_ELANTRA_2022_NON_SCC: { + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00CN7 MDPS R 1.00 1.04 57700-IB000 4CNNP104', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.01 99210-AB000 210205', + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.00 99210-IB000 210531', + ], + (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00CN ESC \t 100!\x05\x01 58910-IB000', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x00T02601BL T02900A1 WCN7T20XXX900NS4\xf7\xccz\xf6', + ], + }, + CAR.HYUNDAI_BAYON_1ST_GEN_NON_SCC: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00BC3 LKA AT EUR LHD 1.00 1.01 99211-Q0100 261', + ], + }, } diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index a4d68dc44..c159e1143 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -146,12 +146,6 @@ class CarInterface(CarInterfaceBase): if hyundai_cancel_button_enables_cruise(candidate): ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANCEL_BTN_ENABLE.value - if candidate == CAR.KIA_FORTE: - has_scc_fw = any(fw.ecu == Ecu.fwdRadar for fw in car_fw) - has_scc_can = any(addr in fingerprint[bus] for bus in (0, 2) for addr in (0x420, 0x421)) - if not (has_scc_fw or has_scc_can): - ret.flags |= HyundaiFlags.NON_SCC.value - # Common lateral control setup ret.centerToFront = ret.wheelbase * 0.4 diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 99147bc24..3d65dd6db 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -51,6 +51,18 @@ NO_DATES_PLATFORMS = { } CANFD_EXPECTED_ECUS = {Ecu.fwdCamera, Ecu.fwdRadar} +HYUNDAI_NON_SCC_CARS = ( + CAR.HYUNDAI_BAYON_1ST_GEN_NON_SCC, + CAR.HYUNDAI_ELANTRA_2022_NON_SCC, + CAR.HYUNDAI_KONA_NON_SCC, + CAR.HYUNDAI_KONA_EV_NON_SCC, + CAR.KIA_CEED_PHEV_2022_NON_SCC, + CAR.KIA_FORTE_2019_NON_SCC, + CAR.KIA_FORTE_2021_NON_SCC, + CAR.KIA_SELTOS_2023_NON_SCC, + CAR.GENESIS_G70_2021_NON_SCC, +) +HYUNDAI_NON_SCC_FW_CARS = tuple(car_model for car_model in HYUNDAI_NON_SCC_CARS if car_model in FW_VERSIONS) def get_test_toggles() -> SimpleNamespace: @@ -76,17 +88,11 @@ class TestHyundaiFingerprint: CP = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, [], False, False, False, None) assert CP.radarUnavailable != radar - forte_no_scc = CarInterface.get_params(CAR.KIA_FORTE, gen_empty_fingerprint(), [], True, False, False, None) - assert bool(forte_no_scc.flags & HyundaiFlags.NON_SCC) - assert not forte_no_scc.alphaLongitudinalAvailable - assert forte_no_scc.pcmCruise - - forte_with_scc = gen_empty_fingerprint() - forte_with_scc[0][0x420] = 8 - forte_with_scc[0][0x421] = 8 - CP = CarInterface.get_params(CAR.KIA_FORTE, forte_with_scc, [], True, False, False, None) - assert not bool(CP.flags & HyundaiFlags.NON_SCC) - assert CP.alphaLongitudinalAvailable + for candidate in HYUNDAI_NON_SCC_CARS: + CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], True, False, False, None) + assert bool(CP.flags & HyundaiFlags.NON_SCC) + assert not CP.alphaLongitudinalAvailable + assert CP.pcmCruise CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, gen_empty_fingerprint(), [], False, False, False, None) assert CP.steerControlType == CarParams.SteerControlType.angle @@ -104,6 +110,40 @@ class TestHyundaiFingerprint: assert palisade_2023.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_CANFD_BLENDED assert palisade_2023.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANCEL_BTN_ENABLE + def test_non_scc_flag_quirks(self): + forte_2019 = CarInterface.get_params(CAR.KIA_FORTE_2019_NON_SCC, gen_empty_fingerprint(), [], True, False, False, None) + assert forte_2019.flags & HyundaiFlags.NON_SCC_NO_FCA + assert not (forte_2019.flags & HyundaiFlags.NON_SCC_RADAR_FCA) + + forte_2021 = CarInterface.get_params(CAR.KIA_FORTE_2021_NON_SCC, gen_empty_fingerprint(), [], True, False, False, None) + assert not (forte_2021.flags & HyundaiFlags.NON_SCC_NO_FCA) + assert not (forte_2021.flags & HyundaiFlags.NON_SCC_RADAR_FCA) + + g70_2021 = CarInterface.get_params(CAR.GENESIS_G70_2021_NON_SCC, gen_empty_fingerprint(), [], True, False, False, None) + assert g70_2021.flags & HyundaiFlags.NON_SCC_RADAR_FCA + + def test_hyundai_redneck_cruise_availability(self, monkeypatch): + class FakeParams: + def __init__(self, *args, **kwargs): + pass + + @staticmethod + def get_bool(key): + return key == "RedneckCruise" + + toggles = get_test_toggles() + monkeypatch.setattr("opendbc.car.interfaces.Params", FakeParams) + + non_scc_cp = CarInterface.get_params(CAR.KIA_FORTE_2021_NON_SCC, gen_empty_fingerprint(), [], True, False, False, toggles) + non_scc_fpcp = CarInterface.get_starpilot_params(CAR.KIA_FORTE_2021_NON_SCC, gen_empty_fingerprint(), [], non_scc_cp, toggles) + assert non_scc_fpcp.redneckCruiseAvailable + assert not non_scc_fpcp.pcmCruiseSpeed + + canfd_alt_buttons_cp = CarInterface.get_params(CAR.KIA_EV6, gen_empty_fingerprint(), [], False, False, False, toggles) + canfd_alt_buttons_fpcp = CarInterface.get_starpilot_params(CAR.KIA_EV6, gen_empty_fingerprint(), [], canfd_alt_buttons_cp, toggles) + assert canfd_alt_buttons_cp.flags & HyundaiFlags.CANFD_ALT_BUTTONS + assert not canfd_alt_buttons_fpcp.redneckCruiseAvailable + def test_palisade_2023_pause_resume_button_maps_to_enable(self): toggles = get_test_toggles() CP = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, gen_empty_fingerprint(), [], True, False, False, toggles) @@ -187,35 +227,43 @@ class TestHyundaiFingerprint: assert CP.vEgoStopping == pytest.approx(0.7) assert CP.stoppingDecelRate == pytest.approx(0.5) - def test_kia_forte_no_scc_fw_match(self): + @pytest.mark.parametrize("candidate", HYUNDAI_NON_SCC_FW_CARS) + def test_non_scc_fw_exact_matches(self, candidate): car_fw = [ CarParams.CarFw( - ecu=Ecu.eps, - fwVersion=b'\xf1\x00BD MDPS C 1.00 1.07 56310/M6300 4BDDC107', - address=0x7d4, - subAddress=0, + ecu=ecu, + fwVersion=fw_versions[0], + address=address, + subAddress=0 if sub_address is None else sub_address, brand="hyundai", - ), - CarParams.CarFw( - ecu=Ecu.fwdCamera, - fwVersion=b'\xf1\x00BD LKAS AT USA LHD 1.00 1.02 95740-M6000 J31', - address=0x7c4, - subAddress=0, - brand="hyundai", - ), + ) + for (ecu, address, sub_address), fw_versions in FW_VERSIONS[candidate].items() ] - exact, matches = match_fw_to_car(car_fw, "3KPF34AD2LE154148", allow_exact=True, allow_fuzzy=False, log=False) + exact, matches = match_fw_to_car(car_fw, "", allow_exact=True, allow_fuzzy=False, log=False) assert exact - assert matches == {CAR.KIA_FORTE} + assert matches == {candidate} - def test_kia_forte_no_scc_fca_does_not_require_scc12(self): + def test_kona_ev_non_scc_has_no_dedicated_fw_coverage(self): + assert CAR.HYUNDAI_KONA_EV_NON_SCC not in FW_VERSIONS + + def test_kia_forte_2019_non_scc_does_not_require_fca11_or_scc12(self): toggles = get_test_toggles() - fingerprint = gen_empty_fingerprint() - fingerprint[0][0x38D] = 8 + CP = CarInterface.get_params(CAR.KIA_FORTE_2019_NON_SCC, gen_empty_fingerprint(), [], True, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(CAR.KIA_FORTE_2019_NON_SCC, gen_empty_fingerprint(), [], CP, toggles) - CP = CarInterface.get_params(CAR.KIA_FORTE, fingerprint, [], True, False, False, toggles) - FPCP = CarInterface.get_starpilot_params(CAR.KIA_FORTE, fingerprint, [], CP, toggles) + car_state = CarState(CP, FPCP) + can_parsers = car_state.get_can_parsers(CP) + + car_state.update(can_parsers, toggles) + pt_states = {state.name for state in can_parsers[Bus.pt].message_states.values()} + assert "FCA11" not in pt_states + assert "SCC12" not in pt_states + + def test_kia_forte_2021_non_scc_uses_fca11_without_scc12(self): + toggles = get_test_toggles() + CP = CarInterface.get_params(CAR.KIA_FORTE_2021_NON_SCC, gen_empty_fingerprint(), [], True, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(CAR.KIA_FORTE_2021_NON_SCC, gen_empty_fingerprint(), [], CP, toggles) car_state = CarState(CP, FPCP) can_parsers = car_state.get_can_parsers(CP) @@ -237,7 +285,10 @@ class TestHyundaiFingerprint: if state.ignore_alive: continue values = {} - if state.name == "LVR12": + if state.name == "EMS16": + values["CRUISE_LAMP_M"] = 1 + values["CRUISE_LAMP_S"] = 1 + elif state.name == "LVR12": values["CF_Lvr_CruiseSet"] = 30 required_msgs.append(packer.make_can_msg(state.name, parser.bus, values)) parser.update([(t, required_msgs)]) diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index e466cb564..87ca27d91 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -6,7 +6,7 @@ from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, Pla from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL, ISO_LATERAL_JERK from opendbc.car.common.conversions import Conversions as CV from opendbc.car.structs import CarParams -from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts +from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts, SupportType from opendbc.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = CarParams.Ecu @@ -79,6 +79,7 @@ class CarControllerParams: # If the max stock LKAS request is <384, add your car to this list. elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.HYUNDAI_ELANTRA, CAR.HYUNDAI_ELANTRA_GT_I30, CAR.HYUNDAI_IONIQ, CAR.HYUNDAI_IONIQ_EV_LTD, CAR.HYUNDAI_SANTA_FE_PHEV_2022, CAR.HYUNDAI_SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV, + CAR.KIA_FORTE_2019_NON_SCC, CAR.KIA_FORTE_2021_NON_SCC, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.KIA_SORENTO): self.STEER_MAX = 255 @@ -198,12 +199,23 @@ class HyundaiFlags(IntFlag): # Palisade/Telluride 2023+ uses CAN routing with CAN-FD-style checksums. CAN_CANFD_BLENDED = 2 ** 29 + # Non-SCC platforms do not all share the same FCA source or availability. + NON_SCC_NO_FCA = 2 ** 30 + NON_SCC_RADAR_FCA = 2 ** 31 + @dataclass class HyundaiCarDocs(CarDocs): package: str = "Smart Cruise Control (SCC)" +@dataclass +class HyundaiNonSccCarDocs(CarDocs): + package: str = "No Smart Cruise Control (Non-SCC)" + support_type: SupportType = SupportType.COMMUNITY + support_link: str = "community" + + @dataclass class HyundaiPlatformConfig(PlatformConfig): dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: "hyundai_kia_generic"}) @@ -227,6 +239,17 @@ class HyundaiCanFDPlatformConfig(PlatformConfig): self.flags |= HyundaiFlags.CANFD +@dataclass +class HyundaiNonSccPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: "hyundai_kia_generic"}) + + def init(self): + self.flags |= HyundaiFlags.NON_SCC + + if self.flags & HyundaiFlags.MIN_STEER_32_MPH: + self.specs = self.specs.override(minSteerSpeed=32 * CV.MPH_TO_MS) + + class CAR(Platforms): # Hyundai HYUNDAI_AZERA_6TH_GEN = HyundaiPlatformConfig( @@ -676,6 +699,52 @@ class CAR(Platforms): flags=HyundaiFlags.RADAR_SCC, ) + # Hyundai non-SCC extensions + HYUNDAI_BAYON_1ST_GEN_NON_SCC = HyundaiNonSccPlatformConfig( + [HyundaiNonSccCarDocs("Hyundai Bayon Non-SCC 2021", car_parts=CarParts.common([CarHarness.hyundai_n]))], + CarSpecs(mass=1150, wheelbase=2.58, steerRatio=13.27 * 1.15), + flags=HyundaiFlags.CHECKSUM_CRC8, + ) + HYUNDAI_ELANTRA_2022_NON_SCC = HyundaiNonSccPlatformConfig( + [HyundaiNonSccCarDocs("Hyundai Elantra Non-SCC 2022", car_parts=CarParts.common([CarHarness.hyundai_k]))], + HYUNDAI_ELANTRA_2021.specs, + flags=HyundaiFlags.CHECKSUM_CRC8, + ) + HYUNDAI_KONA_NON_SCC = HyundaiNonSccPlatformConfig( + [HyundaiNonSccCarDocs("Hyundai Kona Non-SCC 2019", car_parts=CarParts.common([CarHarness.hyundai_b]))], + HYUNDAI_KONA.specs, + flags=HyundaiFlags.ALT_LIMITS, + ) + HYUNDAI_KONA_EV_NON_SCC = HyundaiNonSccPlatformConfig( + [HyundaiNonSccCarDocs("Hyundai Kona Electric Non-SCC 2019", car_parts=CarParts.common([CarHarness.hyundai_g]))], + HYUNDAI_KONA_EV.specs, + flags=HyundaiFlags.EV | HyundaiFlags.ALT_LIMITS, + ) + KIA_CEED_PHEV_2022_NON_SCC = HyundaiNonSccPlatformConfig( + [HyundaiNonSccCarDocs("Kia Ceed Plug-in Hybrid Non-SCC 2022", car_parts=CarParts.common([CarHarness.hyundai_i]))], + CarSpecs(mass=1650, wheelbase=2.65, steerRatio=13.75, tireStiffnessFactor=0.5), + flags=HyundaiFlags.HYBRID, + ) + KIA_FORTE_2019_NON_SCC = HyundaiNonSccPlatformConfig( + [HyundaiNonSccCarDocs("Kia Forte Non-SCC 2019", car_parts=CarParts.common([CarHarness.hyundai_g]))], + KIA_FORTE.specs, + flags=HyundaiFlags.NON_SCC_NO_FCA, + ) + KIA_FORTE_2021_NON_SCC = HyundaiNonSccPlatformConfig( + [HyundaiNonSccCarDocs("Kia Forte Non-SCC 2021", car_parts=CarParts.common([CarHarness.hyundai_g]))], + KIA_FORTE.specs, + ) + KIA_SELTOS_2023_NON_SCC = HyundaiNonSccPlatformConfig( + [HyundaiNonSccCarDocs("Kia Seltos Non-SCC 2023-24", car_parts=CarParts.common([CarHarness.hyundai_l]))], + KIA_SELTOS.specs, + flags=HyundaiFlags.CHECKSUM_CRC8, + ) + GENESIS_G70_2021_NON_SCC = HyundaiNonSccPlatformConfig( + [HyundaiNonSccCarDocs("Genesis G70 Non-SCC 2021", car_parts=CarParts.common([CarHarness.hyundai_f]))], + GENESIS_G70_2020.specs, + flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.NON_SCC_RADAR_FCA, + ) + class Buttons: NONE = 0 @@ -848,8 +917,6 @@ FW_QUERY_CONFIG = FwQueryConfig( # We lose these ECUs without the comma power on these cars. # Note that we still attempt to match with them when they are present non_essential_ecus={ - # Some Forte trims are lateral-only and omit the SCC radar entirely. - Ecu.fwdRadar: [CAR.KIA_FORTE], Ecu.abs: [CAR.HYUNDAI_PALISADE, CAR.HYUNDAI_SONATA, CAR.HYUNDAI_SANTA_FE_2022, CAR.KIA_K5_2021, CAR.HYUNDAI_ELANTRA_2021, CAR.HYUNDAI_SANTA_FE, CAR.HYUNDAI_KONA_EV_2022, CAR.HYUNDAI_KONA_EV, CAR.HYUNDAI_CUSTIN_1ST_GEN, CAR.KIA_SORENTO, CAR.KIA_CEED, CAR.KIA_SELTOS], @@ -891,6 +958,8 @@ EV_CAR = CAR.with_flags(HyundaiFlags.EV) LEGACY_SAFETY_MODE_CAR = CAR.with_flags(HyundaiFlags.LEGACY) +NON_SCC_CAR = CAR.with_flags(HyundaiFlags.NON_SCC) + # TODO: another PR with (HyundaiFlags.LEGACY | HyundaiFlags.UNSUPPORTED_LONGITUDINAL | HyundaiFlags.CAMERA_SCC | # HyundaiFlags.CANFD_RADAR_SCC | HyundaiFlags.CANFD_NO_RADAR_DISABLE | ) UNSUPPORTED_LONGITUDINAL_CAR = CAR.with_flags(HyundaiFlags.LEGACY) | CAR.with_flags(HyundaiFlags.UNSUPPORTED_LONGITUDINAL) diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index e80aff328..da4fa0ab7 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -198,6 +198,7 @@ class CarInterfaceBase(ABC): @classmethod def get_starpilot_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[structs.CarParams.CarFw], CP: structs.CarParams, starpilot_toggles: SimpleNamespace): fp_ret = custom.StarPilotCarParams.new_message() + fp_ret.pcmCruiseSpeed = True platform = PLATFORMS[candidate] @@ -227,6 +228,11 @@ class CarInterfaceBase(ABC): if 0x1FA in fingerprint[CAN.ECAN]: fp_ret.flags |= HyundaiStarPilotFlags.SPEED_LIMIT_AVAILABLE.value + fp_ret.redneckCruiseAvailable = not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) + if fp_ret.redneckCruiseAvailable and Params(return_defaults=True).get_bool("RedneckCruise") and \ + not CP.openpilotLongitudinalControl: + fp_ret.pcmCruiseSpeed = False + if CP.flags & HyundaiFlags.HAS_LDA_BUTTON: fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON.value if starpilot_toggles.always_on_lateral_lkas: diff --git a/opendbc_repo/opendbc/car/torque_data/substitute.toml b/opendbc_repo/opendbc/car/torque_data/substitute.toml index e098037c8..3893aa5e4 100644 --- a/opendbc_repo/opendbc/car/torque_data/substitute.toml +++ b/opendbc_repo/opendbc/car/torque_data/substitute.toml @@ -47,6 +47,15 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "GENESIS_G90" = "GENESIS_G70" "GENESIS_G80" = "GENESIS_G70" "GENESIS_G70_2020" = "HYUNDAI_SONATA" +"KIA_CEED_PHEV_2022_NON_SCC" = "HYUNDAI_SONATA" +"HYUNDAI_KONA_EV_NON_SCC" = "HYUNDAI_KONA_EV" +"KIA_FORTE_2019_NON_SCC" = "HYUNDAI_SONATA" +"KIA_FORTE_2021_NON_SCC" = "HYUNDAI_SONATA" +"KIA_SELTOS_2023_NON_SCC" = "HYUNDAI_SONATA" +"HYUNDAI_KONA_NON_SCC" = "HYUNDAI_KONA_EV" +"HYUNDAI_ELANTRA_2022_NON_SCC" = "HYUNDAI_ELANTRA_2021" +"GENESIS_G70_2021_NON_SCC" = "HYUNDAI_SONATA" +"HYUNDAI_BAYON_1ST_GEN_NON_SCC" = "HYUNDAI_SONATA" "HONDA_FREED" = "HONDA_ODYSSEY" "HONDA_CLARITY" = "HONDA_ACCORD" diff --git a/opendbc_repo/opendbc/safety/modes/hyundai.h b/opendbc_repo/opendbc/safety/modes/hyundai.h index 8e60954d4..94771dacf 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai.h @@ -58,8 +58,11 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = { #define HYUNDAI_LDA_BUTTON_ADDR_CHECK \ {.msg = {{0x391, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ -#define HYUNDAI_NON_SCC_CRUISE_ADDR_CHECK \ - {.msg = {{0x367, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ +#define HYUNDAI_NON_SCC_HEV_ADDR_CHECK \ + {.msg = {{0x595U, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + +#define HYUNDAI_NON_SCC_EV_ADDR_CHECK \ + {.msg = {{0x592U, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ static const CanMsg HYUNDAI_TX_MSGS[] = { HYUNDAI_COMMON_TX_MSGS(0) @@ -152,6 +155,14 @@ static void hyundai_rx_hook(const CANPacket_t *msg) { } } + if (msg->addr == 0x420U) { + if (((msg->bus == 0U) && !hyundai_camera_scc) || ((msg->bus == 2U) && hyundai_camera_scc)) { + if (!hyundai_longitudinal) { + acc_main_on = GET_BIT(msg, hyundai_can_canfd_blended ? 27U : 0U); + } + } + } + if ((msg->addr == 0x367U) && (msg->bus == 0U) && hyundai_non_scc) { uint8_t cruise_set_speed = msg->data[0]; hyundai_common_cruise_state_check((cruise_set_speed > 0U) && (cruise_set_speed < 255U)); @@ -194,10 +205,30 @@ static void hyundai_rx_hook(const CANPacket_t *msg) { brake_pressed = ((msg->data[5] >> 5U) & 0x3U) == 0x2U; } + if (msg->addr == 0x592U) { + acc_main_on = GET_BIT(msg, 34U); + bool cruise_engaged = GET_BIT(msg, 35U); + hyundai_common_cruise_state_check(cruise_engaged); + } + + if (msg->addr == 0x595U) { + acc_main_on = GET_BIT(msg, 50U); + bool cruise_engaged = GET_BIT(msg, 51U); + hyundai_common_cruise_state_check(cruise_engaged); + } + + if ((msg->addr == 0x260U) && hyundai_non_scc && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) { + acc_main_on = GET_BIT(msg, 25U); + bool cruise_engaged = GET_BIT(msg, 26U); + hyundai_common_cruise_state_check(cruise_engaged); + } + if (msg->addr == 0x391U) { hyundai_lkas_button_check(GET_BIT(msg, 4U)); } } + + hyundai_common_reset_acc_main_on_mismatches(); } static bool hyundai_tx_hook(const CANPacket_t *msg) { @@ -219,6 +250,11 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) { } } + if (msg->addr == 0x420U) { + acc_main_on_tx = GET_BIT(msg, hyundai_can_canfd_blended ? 27U : 0U); + hyundai_common_acc_main_on_sync(); + } + // ACCEL: safety check if (((msg->addr == 0x420U) && hyundai_can_canfd_blended) || ((msg->addr == 0x421U) && !hyundai_can_canfd_blended)) { int desired_accel_raw = hyundai_can_canfd_blended ? ((((msg->data[4] & 0x3FU) << 5) | (msg->data[3] >> 3)) - 1023U) : @@ -269,8 +305,9 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) { int button = msg->data[0] & 0x7U; bool allowed_resume = (button == 1) && controls_allowed; + bool allowed_set = (button == 2) && controls_allowed; bool allowed_cancel = (button == 4) && cruise_engaged_prev; - if (!(allowed_resume || allowed_cancel)) { + if (!(allowed_resume || allowed_set || allowed_cancel)) { tx = false; } } @@ -370,11 +407,13 @@ static safety_config hyundai_init(uint16_t param) { } else if (hyundai_camera_scc) { static RxCheck hyundai_cam_scc_rx_checks[] = { HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_SCC11_ADDR_CHECK(2) HYUNDAI_SCC12_ADDR_CHECK(2, false) }; static RxCheck hyundai_cam_scc_rx_checks_lda[] = { HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_SCC11_ADDR_CHECK(2) HYUNDAI_SCC12_ADDR_CHECK(2, false) HYUNDAI_LDA_BUTTON_ADDR_CHECK }; @@ -387,11 +426,13 @@ static safety_config hyundai_init(uint16_t param) { } else if (hyundai_can_canfd_blended) { static RxCheck hyundai_can_canfd_blended_rx_checks[] = { HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_SCC11_ADDR_CHECK(0) HYUNDAI_SCC12_ADDR_CHECK(0, true) }; static RxCheck hyundai_can_canfd_blended_rx_checks_lda[] = { HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_SCC11_ADDR_CHECK(0) HYUNDAI_SCC12_ADDR_CHECK(0, true) HYUNDAI_LDA_BUTTON_ADDR_CHECK }; @@ -405,34 +446,58 @@ static safety_config hyundai_init(uint16_t param) { } else { static RxCheck hyundai_rx_checks[] = { HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_SCC11_ADDR_CHECK(0) HYUNDAI_SCC12_ADDR_CHECK(0, false) }; static RxCheck hyundai_fcev_rx_checks[] = { HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_SCC11_ADDR_CHECK(0) HYUNDAI_SCC12_ADDR_CHECK(0, false) HYUNDAI_FCEV_GAS_ADDR_CHECK }; static RxCheck hyundai_rx_checks_lda[] = { HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_SCC11_ADDR_CHECK(0) HYUNDAI_SCC12_ADDR_CHECK(0, false) HYUNDAI_LDA_BUTTON_ADDR_CHECK }; static RxCheck hyundai_non_scc_rx_checks[] = { HYUNDAI_COMMON_RX_CHECKS(false) - HYUNDAI_NON_SCC_CRUISE_ADDR_CHECK }; static RxCheck hyundai_non_scc_rx_checks_lda[] = { HYUNDAI_COMMON_RX_CHECKS(false) - HYUNDAI_NON_SCC_CRUISE_ADDR_CHECK + HYUNDAI_LDA_BUTTON_ADDR_CHECK + }; + + static RxCheck hyundai_non_scc_hev_rx_checks[] = { + HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_NON_SCC_HEV_ADDR_CHECK + }; + + static RxCheck hyundai_non_scc_hev_rx_checks_lda[] = { + HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_NON_SCC_HEV_ADDR_CHECK + HYUNDAI_LDA_BUTTON_ADDR_CHECK + }; + + static RxCheck hyundai_non_scc_ev_rx_checks[] = { + HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_NON_SCC_EV_ADDR_CHECK + }; + + static RxCheck hyundai_non_scc_ev_rx_checks_lda[] = { + HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_NON_SCC_EV_ADDR_CHECK HYUNDAI_LDA_BUTTON_ADDR_CHECK }; static RxCheck hyundai_fcev_rx_checks_lda[] = { HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_SCC11_ADDR_CHECK(0) HYUNDAI_SCC12_ADDR_CHECK(0, false) HYUNDAI_FCEV_GAS_ADDR_CHECK HYUNDAI_LDA_BUTTON_ADDR_CHECK @@ -447,7 +512,19 @@ static safety_config hyundai_init(uint16_t param) { } } else { if (hyundai_non_scc) { - if (hyundai_has_lda_button) { + if (hyundai_ev_gas_signal) { + if (hyundai_has_lda_button) { + SET_RX_CHECKS(hyundai_non_scc_ev_rx_checks_lda, ret); + } else { + SET_RX_CHECKS(hyundai_non_scc_ev_rx_checks, ret); + } + } else if (hyundai_hybrid_gas_signal) { + if (hyundai_has_lda_button) { + SET_RX_CHECKS(hyundai_non_scc_hev_rx_checks_lda, ret); + } else { + SET_RX_CHECKS(hyundai_non_scc_hev_rx_checks, ret); + } + } else if (hyundai_has_lda_button) { SET_RX_CHECKS(hyundai_non_scc_rx_checks_lda, ret); } else { SET_RX_CHECKS(hyundai_non_scc_rx_checks, ret); diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h index c9152212a..611423c2b 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h @@ -152,8 +152,11 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) { int cruise_status = ((msg->data[8] >> 4) & 0x7U); bool cruise_engaged = (cruise_status == 1) || (cruise_status == 2); hyundai_common_cruise_state_check(cruise_engaged); + acc_main_on = GET_BIT(msg, 66U); } } + + hyundai_common_reset_acc_main_on_mismatches(); } static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) { @@ -234,8 +237,9 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) { int button = msg->data[2] & 0x7U; bool is_cancel = (button == HYUNDAI_BTN_CANCEL); bool is_resume = (button == HYUNDAI_BTN_RESUME); + bool is_set = (button == HYUNDAI_BTN_SET); - bool allowed = (is_cancel && cruise_engaged_prev) || (is_resume && controls_allowed); + bool allowed = (is_cancel && cruise_engaged_prev) || ((is_resume || is_set) && controls_allowed); if (!allowed) { tx = false; } @@ -273,6 +277,9 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) { if (violation) { tx = false; } + + acc_main_on_tx = GET_BIT(msg, 66U); + hyundai_common_acc_main_on_sync(); } return tx; diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_common.h b/opendbc_repo/opendbc/safety/modes/hyundai_common.h index 0b2d91e3d..dba7431d7 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_common.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_common.h @@ -58,6 +58,9 @@ extern bool hyundai_cancel_button_enable; bool hyundai_cancel_button_enable = false; static uint8_t hyundai_last_button_interaction; // button messages since the user pressed an enable button +static bool acc_main_on_prev; +static bool acc_main_on_tx; +static uint32_t acc_main_on_mismatches; void hyundai_common_init(uint16_t param) { const uint16_t HYUNDAI_PARAM_EV_GAS = 1; @@ -89,6 +92,9 @@ void hyundai_common_init(uint16_t param) { hyundai_cancel_button_enable = GET_FLAG(param, HYUNDAI_PARAM_CANCEL_BTN_ENABLE); hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; + acc_main_on_prev = false; + acc_main_on_tx = false; + acc_main_on_mismatches = 0U; #ifdef ALLOW_DEBUG const uint16_t HYUNDAI_PARAM_LONGITUDINAL = 4; @@ -179,6 +185,31 @@ uint32_t hyundai_common_canfd_compute_checksum(const CANPacket_t *msg) { return crc; } +void hyundai_common_reset_acc_main_on_mismatches(void) { + if (acc_main_on && !acc_main_on_prev) { + acc_main_on_mismatches = 0U; + } + + acc_main_on_prev = acc_main_on; +} + +void hyundai_common_acc_main_on_sync(void) { + if (acc_main_on && !acc_main_on_tx) { + acc_main_on_mismatches += 1U; + + if (acc_main_on_mismatches >= 3U) { + acc_main_on = false; + lkas_on = false; + } + } else { + acc_main_on_mismatches = 0U; + } +} + +uint32_t get_acc_main_on_mismatches(void) { + return acc_main_on_mismatches; +} + void hyundai_lkas_button_check(const bool lkas_button) { if (lkas_button && !lkas_button_prev) { lkas_on = !lkas_on; diff --git a/opendbc_repo/opendbc/safety/tests/hyundai_common.py b/opendbc_repo/opendbc/safety/tests/hyundai_common.py index 627cd785b..f33c5bd39 100644 --- a/opendbc_repo/opendbc/safety/tests/hyundai_common.py +++ b/opendbc_repo/opendbc/safety/tests/hyundai_common.py @@ -23,8 +23,8 @@ class HyundaiButtonBase: def test_button_sends(self): """ - Only RES and CANCEL buttons are allowed - - RES allowed while controls allowed + Only RES/SET and CANCEL buttons are allowed + - RES/SET allowed while controls allowed - CANCEL allowed while cruise is enabled """ self.safety.set_controls_allowed(0) @@ -33,7 +33,7 @@ class HyundaiButtonBase: self.safety.set_controls_allowed(1) self.assertTrue(self._tx(self._button_msg(Buttons.RESUME, bus=self.BUTTONS_TX_BUS))) - self.assertFalse(self._tx(self._button_msg(Buttons.SET, bus=self.BUTTONS_TX_BUS))) + self.assertTrue(self._tx(self._button_msg(Buttons.SET, bus=self.BUTTONS_TX_BUS))) for enabled in (True, False): self._rx(self._pcm_status_msg(enabled)) @@ -95,6 +95,7 @@ class HyundaiButtonBase: class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest): MAX_ACCEL = 3.5 + CANCEL_BUTTON_ENABLE = False DISABLED_ECU_UDS_MSG: tuple[int, int] DISABLED_ECU_ACTUATION_MSG: tuple[int, int] @@ -127,6 +128,9 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest): def _accel_msg(self, accel, aeb_req=False, aeb_decel=0): raise NotImplementedError + def _tx_acc_state_msg(self, main_on): + raise NotImplementedError + def test_set_resume_buttons(self): """ SET and RESUME enter controls allowed on their falling edge. @@ -141,8 +145,9 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest): # should enter controls allowed on falling edge and not transitioning to cancel should_enable = btn_cur != btn_prev and \ - btn_cur != Buttons.CANCEL and \ - btn_prev in (Buttons.RESUME, Buttons.SET) + (btn_prev in (Buttons.RESUME, Buttons.SET) or (self.CANCEL_BUTTON_ENABLE and btn_prev == Buttons.CANCEL)) + if (btn_cur == Buttons.CANCEL) and not self.CANCEL_BUTTON_ENABLE: + should_enable = False self._rx(self._button_msg(btn_cur)) self.assertEqual(should_enable, self.safety.get_controls_allowed()) @@ -150,7 +155,34 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest): def test_cancel_button(self): self.safety.set_controls_allowed(1) self._rx(self._button_msg(Buttons.CANCEL)) - self.assertFalse(self.safety.get_controls_allowed()) + self.assertEqual(self.CANCEL_BUTTON_ENABLE, self.safety.get_controls_allowed()) + + def test_acc_main_sync_mismatch_counter(self): + try: + tx_acc_state_msg = self._tx_acc_state_msg(False) + except NotImplementedError as err: + raise unittest.SkipTest("ACC main TX state message not implemented") from err + + self._rx(self._button_msg(Buttons.NONE, main_button=0)) + self._rx(self._button_msg(Buttons.NONE, main_button=1)) + self._rx(self._button_msg(Buttons.NONE, main_button=0)) + self.assertTrue(self.safety.get_acc_main_on()) + self.assertEqual(0, self.safety.get_acc_main_on_mismatches()) + + self._tx(tx_acc_state_msg) + self.assertTrue(self.safety.get_acc_main_on()) + self.assertEqual(1, self.safety.get_acc_main_on_mismatches()) + + self._tx(tx_acc_state_msg) + self.assertTrue(self.safety.get_acc_main_on()) + self.assertEqual(2, self.safety.get_acc_main_on_mismatches()) + + self._tx(tx_acc_state_msg) + self.assertFalse(self.safety.get_acc_main_on()) + self.assertEqual(3, self.safety.get_acc_main_on_mismatches()) + + self._tx(tx_acc_state_msg) + self.assertEqual(0, self.safety.get_acc_main_on_mismatches()) def test_tester_present_allowed(self, ecu_disable: bool = True): """ diff --git a/opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py b/opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py index ee314b6e1..df66a7fcb 100644 --- a/opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py +++ b/opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py @@ -43,6 +43,7 @@ bool get_brake_pressed_prev(void); bool get_regen_braking_prev(void); bool get_steering_disengage_prev(void); bool get_acc_main_on(void); +uint32_t get_acc_main_on_mismatches(void); float get_vehicle_speed_min(void); float get_vehicle_speed_max(void); int get_current_safety_mode(void); diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai.py b/opendbc_repo/opendbc/safety/tests/test_hyundai.py index db09e39c3..b4ade671f 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai.py @@ -103,6 +103,11 @@ class TestHyundaiSafety(HyundaiButtonBase, common.CarSafetyTest, common.DriverTo self.__class__.cnt_cruise += 1 return self.packer.make_can_msg_safety("SCC12", self.SCC_BUS, values, fix_checksum=checksum) + def _acc_state_msg(self, main_on): + dat = bytearray(8) + dat[0] = int(main_on) + return libsafety_py.make_CANPacket(0x420, self.SCC_BUS, bytes(dat)) + def _torque_driver_msg(self, torque): values = {"CR_Mdps_StrColTq": torque} return self.packer.make_can_msg_safety("MDPS12", 0, values) @@ -111,6 +116,16 @@ class TestHyundaiSafety(HyundaiButtonBase, common.CarSafetyTest, common.DriverTo values = {"CR_Lkas_StrToqReq": torque, "CF_Lkas_ActToi": steer_req} return self.packer.make_can_msg_safety("LKAS11", 0, values) + def test_pcm_main_cruise_state_availability(self): + if self.safety.get_current_safety_param() & HyundaiSafetyFlags.LONG: + raise unittest.SkipTest("Longitudinal mode does not learn ACC main state from SCC11 RX") + if self.safety.get_current_safety_mode() == CarParams.SafetyModel.hyundaiLegacy: + raise unittest.SkipTest("Legacy Hyundai safety does not track ACC main state from SCC11 RX") + + for should_turn_acc_main_on in (True, False): + self._rx(self._acc_state_msg(should_turn_acc_main_on)) + self.assertEqual(should_turn_acc_main_on, self.safety.get_acc_main_on()) + class TestHyundaiSafetyAltLimits(TestHyundaiSafety): MAX_RATE_UP = 2 @@ -154,11 +169,35 @@ class TestHyundaiSafetyNonScc(TestHyundaiSafety): self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.NON_SCC) self.safety.init_tests() - def _pcm_status_msg(self, enable): - values = {"CF_Lvr_CruiseSet": 30 if enable else 0} - return self.packer.make_can_msg_safety("LVR12", 0, values) + def _user_gas_msg(self, gas): + dat = bytearray(8) + cruise_active = self.safety.get_controls_allowed() or self.safety.get_cruise_engaged_prev() + acc_main_on = self.safety.get_acc_main_on() or cruise_active + dat[3] |= int(acc_main_on) << 1 + dat[3] |= int(cruise_active) << 2 + dat[7] = ((self.cnt_gas % 4) << 4) | (0x40 if gas else 0x00) + self.__class__.cnt_gas += 1 + _, dat, bus = checksum((0x260, dat, 0)) + return libsafety_py.make_CANPacket(0x260, bus, bytes(dat)) - def test_non_scc_uses_lvr12_rx_checks(self): + def _acc_state_msg(self, main_on): + dat = bytearray(8) + dat[3] |= int(main_on) << 1 + dat[7] |= (self.cnt_cruise % 4) << 4 + self.__class__.cnt_cruise += 1 + _, dat, bus = checksum((0x260, dat, 0)) + return libsafety_py.make_CANPacket(0x260, bus, bytes(dat)) + + def _pcm_status_msg(self, enable): + dat = bytearray(8) + dat[3] |= int(enable) << 1 + dat[3] |= int(enable) << 2 + dat[7] |= (self.cnt_cruise % 4) << 4 + self.__class__.cnt_cruise += 1 + _, dat, bus = checksum((0x260, dat, 0)) + return libsafety_py.make_CANPacket(0x260, bus, bytes(dat)) + + def test_non_scc_uses_live_acc_state_rx_checks(self): self._rx(self._user_gas_msg(False)) self._rx(self._torque_driver_msg(0)) self._rx(self._speed_msg(0)) @@ -187,6 +226,11 @@ class TestHyundaiCanCanfdBlendedSafety(TestHyundaiSafety): self.__class__.cnt_cruise += 1 return self.packer.make_can_msg_panda("SCC12", 0, values) + def _acc_state_msg(self, main_on): + dat = bytearray(8) + dat[3] = int(main_on) << 3 + return libsafety_py.make_CANPacket(0x420, 0, bytes(dat)) + class TestHyundaiSafetyFCEV(TestHyundaiSafety): def setUp(self): @@ -257,6 +301,11 @@ class TestHyundaiLongitudinalSafety(HyundaiLongitudinalBase, TestHyundaiSafety): } return self.packer.make_can_msg_safety("SCC12", self.SCC_BUS, values) + def _tx_acc_state_msg(self, main_on): + dat = bytearray(8) + dat[0] = int(main_on) + return libsafety_py.make_CANPacket(0x420, 0, bytes(dat)) + def _fca11_msg(self, idx=0, vsm_aeb_req=False, fca_aeb_req=False, aeb_decel=0): values = { "CR_FCA_Alive": idx % 0xF, @@ -287,6 +336,7 @@ class TestHyundaiCanCanfdBlendedLongitudinalSafety(HyundaiLongitudinalBase, Test DISABLED_ECU_UDS_MSG = (0x7D0, 0) DISABLED_ECU_ACTUATION_MSG = (0x420, 0) + CANCEL_BUTTON_ENABLE = True def setUp(self): self.packer = CANPackerSafety("hyundai_palisade_2023_generated") @@ -302,6 +352,11 @@ class TestHyundaiCanCanfdBlendedLongitudinalSafety(HyundaiLongitudinalBase, Test } return self.packer.make_can_msg_panda("SCC11", 0, values) + def _tx_acc_state_msg(self, main_on): + dat = bytearray(8) + dat[3] = int(main_on) << 3 + return libsafety_py.make_CANPacket(0x420, 0, bytes(dat)) + def test_no_aeb_fca11(self): pass @@ -336,6 +391,11 @@ class TestHyundaiLongitudinalSafetyCameraSCC(HyundaiLongitudinalBase, TestHyunda } return self.packer.make_can_msg_safety("SCC12", self.SCC_BUS, values) + def _tx_acc_state_msg(self, main_on): + dat = bytearray(8) + dat[0] = int(main_on) + return libsafety_py.make_CANPacket(0x420, self.SCC_BUS, bytes(dat)) + def test_no_aeb_scc12(self): self.assertTrue(self._tx(self._accel_msg(0))) self.assertFalse(self._tx(self._accel_msg(0, aeb_req=True))) diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index 6516b7cf4..ee8054b82 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -93,6 +93,10 @@ class TestHyundaiCanfdBase(HyundaiButtonBase, common.CarSafetyTest, common.Drive values = {"ACCMode": 1 if enable else 0} return self.packer.make_can_msg_safety("SCC_CONTROL", self.SCC_BUS, values) + def _acc_state_msg(self, main_on): + values = {"MainMode_ACC": int(main_on), "ACCMode": 0} + return self.packer.make_can_msg_safety("SCC_CONTROL", self.SCC_BUS, values) + def _button_msg(self, buttons, main_button=0, bus=None): if bus is None: bus = self.PT_BUS @@ -123,6 +127,14 @@ class TestHyundaiCanfdBase(HyundaiButtonBase, common.CarSafetyTest, common.Drive self._aol_state = toggle_on return None # avoid duplicate message in harness + def test_pcm_main_cruise_state_availability(self): + if self.safety.get_current_safety_param() & HyundaiSafetyFlags.LONG: + raise unittest.SkipTest("Longitudinal mode does not learn ACC main state from SCC_CONTROL RX") + + for should_turn_acc_main_on in (True, False): + self._rx(self._acc_state_msg(should_turn_acc_main_on)) + self.assertEqual(should_turn_acc_main_on, self.safety.get_acc_main_on()) + class TestHyundaiCanfdLFASteeringBase(TestHyundaiCanfdBase): @@ -495,6 +507,10 @@ class TestHyundaiCanfdLKASteeringLongEV(HyundaiLongitudinalBase, TestHyundaiCanf } return self.packer.make_can_msg_safety("SCC_CONTROL", 1, values) + def _tx_acc_state_msg(self, main_on): + values = {"MainMode_ACC": int(main_on), "ACCMode": 0} + return self.packer.make_can_msg_safety("SCC_CONTROL", 1, values) + # Tests longitudinal for ICE, hybrid, EV cars with LFA steering class TestHyundaiCanfdLFASteeringLongBase(HyundaiLongitudinalBase, TestHyundaiCanfdLFASteeringBase): @@ -526,6 +542,10 @@ class TestHyundaiCanfdLFASteeringLongBase(HyundaiLongitudinalBase, TestHyundaiCa } return self.packer.make_can_msg_safety("SCC_CONTROL", 0, values) + def _tx_acc_state_msg(self, main_on): + values = {"MainMode_ACC": int(main_on), "ACCMode": 0} + return self.packer.make_can_msg_safety("SCC_CONTROL", 0, values) + def test_tester_present_allowed(self, ecu_disable: bool = True): super().test_tester_present_allowed(ecu_disable=not self.SAFETY_PARAM & HyundaiSafetyFlags.CAMERA_SCC) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index b3cba235e..a1b32dbdb 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -21,6 +21,7 @@ from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.common.constants import CV from openpilot.selfdrive.car.cruise import VCruiseHelper, IMPERIAL_INCREMENT, V_CRUISE_MAX, V_CRUISE_MIN +from openpilot.selfdrive.car.redneck_cruise import RedneckCruise from openpilot.selfdrive.car.car_specific import MockCarState from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles, update_starpilot_toggles @@ -164,6 +165,7 @@ class Car: self.mock_carstate = MockCarState() self.v_cruise_helper = VCruiseHelper(self.CP) + self.redneck_cruise = RedneckCruise(self.CP, self.FPCP) if self.CP.brand == "hyundai" else None self.is_metric = self.params.get_bool("IsMetric") self.safe_mode = self.params.get_bool("SafeMode") @@ -339,6 +341,7 @@ class Car: if self.sm.all_alive(['carControl']): # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) + self._update_redneck_cruise(CS, CC) self._update_openpilot_lead_state(CC) self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos, self.starpilot_toggles) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) @@ -365,6 +368,14 @@ class Car: self.CI.CS.openpilot_lead_distance = lead_distance self.CI.CS.openpilot_lead_rel_speed = lead_rel_speed + def _update_redneck_cruise(self, CS: car.CarState, CC: car.CarControl) -> None: + if self.redneck_cruise is None: + return + + send_button, v_target = self.redneck_cruise.run(CS, CC, self.sm['starpilotPlan'].vCruise, self.is_metric) + self.CI.CS.redneck_send_button = send_button + self.CI.CS.redneck_v_target = v_target + def step(self): CS, RD, FPCS = self.state_update() diff --git a/selfdrive/car/redneck_cruise.py b/selfdrive/car/redneck_cruise.py new file mode 100644 index 000000000..fea8563f8 --- /dev/null +++ b/selfdrive/car/redneck_cruise.py @@ -0,0 +1,121 @@ +from cereal import car +from opendbc.car import apply_hysteresis +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_CTRL + +ButtonType = car.CarState.ButtonEvent.Type + +SEND_BUTTON_NONE = 0 +SEND_BUTTON_INCREASE = 1 +SEND_BUTTON_DECREASE = 2 + +HYST_GAP = 0.0 +INACTIVE_TIMER = 0.4 + +CRUISE_BUTTON_TIMERS = { + ButtonType.decelCruise: 0, + ButtonType.accelCruise: 0, + ButtonType.setCruise: 0, + ButtonType.resumeCruise: 0, + ButtonType.cancel: 0, + ButtonType.mainCruise: 0, +} + + +def get_minimum_set_speed(is_metric: bool) -> int: + return 30 if is_metric else 20 + + +def update_manual_button_timers(CS: car.CarState, button_timers: dict[ButtonType, int]) -> None: + for button_type in button_timers: + if button_timers[button_type] > 0: + button_timers[button_type] += 1 + + for event in CS.buttonEvents: + if event.type in button_timers: + button_timers[event.type] = 1 if event.pressed else 0 + + +class RedneckCruise: + def __init__(self, CP, FPCP): + self.CP = CP + self.FPCP = FPCP + + self.v_target = 0 + self.v_target_ms_last = 0.0 + self.v_cruise_cluster = 0 + self.v_cruise_min = 0 + self.state = "inactive" + self.pre_active_timer = 0 + self.is_ready = False + self.is_ready_prev = False + self.cruise_button_timers = dict(CRUISE_BUTTON_TIMERS) + + @staticmethod + def _send_button_for_state(state: str) -> int: + if state == "increasing": + return SEND_BUTTON_INCREASE + if state == "decreasing": + return SEND_BUTTON_DECREASE + return SEND_BUTTON_NONE + + def _reset(self) -> None: + self.state = "inactive" + self.pre_active_timer = 0 + self.is_ready = False + self.is_ready_prev = False + + def _update_calculations(self, CS: car.CarState, v_target_ms: float, is_metric: bool) -> None: + speed_conv = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH + ms_conv = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS + + self.v_target_ms_last = apply_hysteresis(v_target_ms, self.v_target_ms_last, HYST_GAP * ms_conv) + self.v_target = round(self.v_target_ms_last * speed_conv) + self.v_cruise_min = get_minimum_set_speed(is_metric) + self.v_cruise_cluster = round(CS.cruiseState.speedCluster * speed_conv) + + def _update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None: + update_manual_button_timers(CS, self.cruise_button_timers) + button_pressed = any(timer > 0 for timer in self.cruise_button_timers.values()) + self.is_ready = CC.enabled and not CC.cruiseControl.override and not CC.cruiseControl.cancel and not CC.cruiseControl.resume and not button_pressed + + def _update_state_machine(self) -> int: + self.pre_active_timer = max(0, self.pre_active_timer - 1) + + if self.state != "inactive": + if not self.is_ready: + self.state = "inactive" + elif self.state == "preActive": + if self.pre_active_timer <= 0: + if self.v_target == self.v_cruise_cluster: + self.state = "holding" + elif self.v_target > self.v_cruise_cluster: + self.state = "increasing" + elif self.v_target < self.v_cruise_cluster and self.v_cruise_cluster > self.v_cruise_min: + self.state = "decreasing" + elif self.state == "holding": + if self.v_target != self.v_cruise_cluster: + self.state = "preActive" + elif self.state == "increasing": + if self.v_target <= self.v_cruise_cluster: + self.state = "holding" + elif self.state == "decreasing": + if self.v_target >= self.v_cruise_cluster or self.v_cruise_cluster <= self.v_cruise_min: + self.state = "holding" + elif self.is_ready and not self.is_ready_prev: + self.pre_active_timer = int(INACTIVE_TIMER / DT_CTRL) + self.state = "preActive" + + return self._send_button_for_state(self.state) + + def run(self, CS: car.CarState, CC: car.CarControl, v_target_ms: float, is_metric: bool) -> tuple[int, int]: + if self.FPCP.pcmCruiseSpeed or not self.FPCP.redneckCruiseAvailable: + self._reset() + return SEND_BUTTON_NONE, 0 + + self._update_calculations(CS, v_target_ms, is_metric) + self._update_readiness(CS, CC) + send_button = self._update_state_machine() + + self.is_ready_prev = self.is_ready + return send_button, self.v_target diff --git a/selfdrive/car/tests/test_redneck_cruise.py b/selfdrive/car/tests/test_redneck_cruise.py new file mode 100644 index 000000000..525ee2b90 --- /dev/null +++ b/selfdrive/car/tests/test_redneck_cruise.py @@ -0,0 +1,85 @@ +import unittest +from types import SimpleNamespace + +from cereal import car +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.car.redneck_cruise import ( + INACTIVE_TIMER, + RedneckCruise, + SEND_BUTTON_DECREASE, + SEND_BUTTON_INCREASE, + SEND_BUTTON_NONE, +) + + +ButtonType = car.CarState.ButtonEvent.Type + + +class TestRedneckCruise(unittest.TestCase): + def setUp(self): + self.CP = SimpleNamespace() + self.FPCP = SimpleNamespace(pcmCruiseSpeed=False, redneckCruiseAvailable=True) + self.redneck = RedneckCruise(self.CP, self.FPCP) + + def _new_state(self, speed_cluster_mph=20.0, button_events=None): + return SimpleNamespace( + cruiseState=SimpleNamespace(speedCluster=speed_cluster_mph * CV.MPH_TO_MS), + buttonEvents=button_events or [], + ) + + @staticmethod + def _new_control(override=False, cancel=False, resume=False): + return SimpleNamespace( + enabled=True, + cruiseControl=SimpleNamespace(override=override, cancel=cancel, resume=resume), + ) + + @staticmethod + def _button_event(button_type, pressed): + return SimpleNamespace(type=button_type, pressed=pressed) + + def _run_until_active(self, target_mph, speed_cluster_mph=20.0, button_events=None, override=False, cancel=False, resume=False): + frames = int(INACTIVE_TIMER / DT_CTRL) + 2 + send_button = SEND_BUTTON_NONE + v_target = 0 + for _ in range(frames): + send_button, v_target = self.redneck.run( + self._new_state(speed_cluster_mph=speed_cluster_mph, button_events=button_events), + self._new_control(override=override, cancel=cancel, resume=resume), + target_mph * CV.MPH_TO_MS, + is_metric=False, + ) + button_events = None + return send_button, v_target + + def test_increases_cluster_speed_toward_target(self): + send_button, v_target = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0) + self.assertEqual(SEND_BUTTON_INCREASE, send_button) + self.assertEqual(25, v_target) + + def test_decreases_cluster_speed_toward_target(self): + send_button, v_target = self._run_until_active(target_mph=20.0, speed_cluster_mph=25.0) + self.assertEqual(SEND_BUTTON_DECREASE, send_button) + self.assertEqual(20, v_target) + + def test_suppresses_output_during_manual_cruise_button_use(self): + button_event = self._button_event(ButtonType.accelCruise, True) + send_button, _ = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0, button_events=[button_event]) + self.assertEqual(SEND_BUTTON_NONE, send_button) + + def test_suppresses_output_for_override_cancel_and_resume(self): + for kwargs in ({"override": True}, {"cancel": True}, {"resume": True}): + with self.subTest(**kwargs): + send_button, _ = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0, **kwargs) + self.assertEqual(SEND_BUTTON_NONE, send_button) + + def test_resets_when_pcm_cruise_speed_is_enabled(self): + self.FPCP.pcmCruiseSpeed = True + send_button, v_target = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0) + self.assertEqual(SEND_BUTTON_NONE, send_button) + self.assertEqual(0, v_target) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py index 9ce17cf40..47cf3b2ae 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -582,6 +582,12 @@ class StarPilotLongitudinalLayout(_SettingsPage): get_value=lambda: f"{self._params.get_int('ForceStopDistanceOffset'):+d} ft", on_click=lambda: self._show_slider("ForceStopDistanceOffset", -20, 20, unit=" ft"), visible=lambda: self._params.get_bool("QOLLongitudinal") and self._params.get_bool("ForceStops")), + SettingRow("RedneckCruise", "toggle", tr_noop("Redneck Cruise"), + subtitle=tr_noop("On supported Hyundai stock-long cars, use RES/SET button spam to match the cluster set speed to StarPilot's target."), + get_state=lambda: self._params.get_bool("RedneckCruise"), + set_state=lambda s: self._params.put_bool("RedneckCruise", s), + visible=lambda: starpilot_state.car_state.redneckCruiseAvailable and + (not starpilot_state.car_state.hasOpenpilotLongitudinal or self._params.get_bool("DisableOpenpilotLongitudinal"))), ], tab_key="daily", column_pair="daily"), SettingSection(tr_noop("Standstill & Gears"), [ SettingRow("ForceStandstill", "toggle", tr_noop("Force Standstill"), diff --git a/selfdrive/ui/lib/starpilot_state.py b/selfdrive/ui/lib/starpilot_state.py index 8a58806df..a0f850a68 100644 --- a/selfdrive/ui/lib/starpilot_state.py +++ b/selfdrive/ui/lib/starpilot_state.py @@ -36,6 +36,7 @@ class StarPilotCarState: hasZSS: bool = False canUsePedal: bool = False canUseSDSU: bool = False + redneckCruiseAvailable: bool = False # ========== Device/Car State ========== isFrogsGoMoo: bool = False @@ -190,6 +191,7 @@ class StarPilotState: FPCP = messaging.log_from_bytes(fpcp_bytes, custom.StarPilotCarParams) self.car_state.canUsePedal = FPCP.canUsePedal self.car_state.canUseSDSU = FPCP.canUseSDSU + self.car_state.redneckCruiseAvailable = FPCP.redneckCruiseAvailable self.car_state.openpilotLongitudinalControlDisabled = FPCP.openpilotLongitudinalControlDisabled except Exception: pass diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 4b6e07973..03ff9f975 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -531,6 +531,7 @@ class StarPilotVariables: toggle.has_sdsu = toggle.car_make == "toyota" and bool(FPCP.flags & ToyotaStarPilotFlags.SMART_DSU.value) has_sng = CP.autoResumeSng toggle.has_zss = toggle.car_make == "toyota" and bool(FPCP.flags & ToyotaStarPilotFlags.ZSS.value) + toggle.redneck_cruise_available = bool(FPCP.redneckCruiseAvailable) is_angle_car = CP.steerControlType == car.CarParams.SteerControlType.angle latAccelFactor = CP.lateralTuning.torque.latAccelFactor if not math.isfinite(latAccelFactor): @@ -541,6 +542,12 @@ class StarPilotVariables: ) longitudinalActuatorDelay = CP.longitudinalActuatorDelay toggle.openpilot_longitudinal = CP.openpilotLongitudinalControl and not toggle.disable_openpilot_long + if not toggle.redneck_cruise_available or toggle.openpilot_longitudinal: + self.params.put_bool("RedneckCruise", False) + toggle.redneck_cruise = self.get_value( + "RedneckCruise", + condition=toggle.redneck_cruise_available and not toggle.openpilot_longitudinal, + ) pcm_cruise = CP.pcmCruise prohibited_main_aol = not toggle.openpilot_longitudinal and toggle.car_make == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD or CP.flags & HyundaiFlags.HAS_LDA_BUTTON) startAccel = CP.startAccel