diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index fb672cf4f1..4ab4f5434f 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -117,6 +117,7 @@ class CarState(CarStateBase): "CRUISE_BUTTONS" self.is_metric = False self.buttons_counter = 0 + self.main_cruise_on = False self.cruise_info = {} self.msg_161 = {} @@ -416,6 +417,9 @@ class CarState(CarStateBase): *create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}), *lkas_button_events] + if getattr(self.FPCP, "flags", 0) & HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING: + ret.cruiseState.available = self.update_main_cruise(ret) + ret.blockPcmEnable = not self.recent_button_interaction() # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index 75add3ee2d..e7244462bb 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -214,6 +214,11 @@ class CarInterface(CarInterfaceBase): if hyundai_cancel_button_enables_cruise(candidate): ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANCEL_BTN_ENABLE.value + if 0x2AA in fingerprint[0]: + ret.minSteerSpeed = 0.0 + ret.flags &= ~HyundaiFlags.MIN_STEER_32_MPH.value + ret.steerAtStandstill = True + # 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 1c4f3bfdea..7c3b228ef2 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -26,7 +26,7 @@ from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \ UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \ LEGACY_LONGITUDINAL_CAR, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags, \ - HyundaiStarPilotSafetyFlags, Buttons, CarControllerParams, kia_ev6_gt_line_longitudinal_tuning + HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, Buttons, CarControllerParams, kia_ev6_gt_line_longitudinal_tuning LongCtrlState = CarControl.Actuators.LongControlState from opendbc.car.hyundai.fingerprints import FW_VERSIONS @@ -312,6 +312,21 @@ class TestHyundaiFingerprint: CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, None) assert CP.flags & HyundaiFlags.SEND_LFA + def test_smart_mdps_allows_low_speed_steering(self): + candidate = CAR.HYUNDAI_IONIQ_EV_LTD + + regular_fingerprint = gen_empty_fingerprint() + regular_cp = CarInterface.get_params(candidate, regular_fingerprint, [], False, False, False, None) + assert regular_cp.flags & HyundaiFlags.MIN_STEER_32_MPH + assert regular_cp.minSteerSpeed > 0.0 + + smart_mdps_fingerprint = gen_empty_fingerprint() + smart_mdps_fingerprint[0][0x2AA] = 8 + smart_mdps_cp = CarInterface.get_params(candidate, smart_mdps_fingerprint, [], False, False, False, None) + assert not (smart_mdps_cp.flags & HyundaiFlags.MIN_STEER_32_MPH) + assert smart_mdps_cp.minSteerSpeed == 0.0 + assert smart_mdps_cp.steerAtStandstill + @pytest.mark.parametrize("candidate", CCNC_NON_HDA2_CARS) def test_ccnc_non_hda2_platforms_set_ccnc_safety(self, candidate): CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], False, False, False, None) @@ -509,6 +524,27 @@ class TestHyundaiFingerprint: ) assert not (minimal_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC) + def test_classic_hyundai_long_tracks_main_cruise_state(self): + toggles = get_test_toggles() + classic_cp = CarInterface.get_params(CAR.HYUNDAI_ELANTRA_2021, gen_empty_fingerprint(), [], True, False, False, toggles) + classic_fpcp = CarInterface.get_starpilot_params( + CAR.HYUNDAI_ELANTRA_2021, gen_empty_fingerprint(), [], classic_cp, toggles, + ) + assert classic_fpcp.flags & HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING + + car_state = CarState(classic_cp, classic_fpcp) + ret = SimpleNamespace( + cruiseState=SimpleNamespace(available=True), + buttonEvents=[structs.CarState.ButtonEvent(pressed=True, type=ButtonType.mainCruise)], + ) + assert car_state.update_main_cruise(ret) + + ioniq_cp = CarInterface.get_params(CAR.HYUNDAI_IONIQ_6, gen_empty_fingerprint(), [], True, False, False, toggles) + ioniq_fpcp = CarInterface.get_starpilot_params( + CAR.HYUNDAI_IONIQ_6, gen_empty_fingerprint(), [], ioniq_cp, toggles, + ) + assert not (ioniq_fpcp.flags & HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING) + def test_non_scc_flag_quirks(self): elantra_hev = CarInterface.get_params(CAR.HYUNDAI_ELANTRA_HEV_2022_NON_SCC, gen_empty_fingerprint(), [], True, False, False, None) assert elantra_hev.flags & HyundaiFlags.HYBRID diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 76102af0d1..57456969ec 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -118,6 +118,7 @@ class HyundaiStarPilotSafetyFlags(IntFlag): class HyundaiStarPilotFlags(IntFlag): SPEED_LIMIT_AVAILABLE = 1 + MAIN_CRUISE_STATE_TRACKING = 2 ** 2 class HyundaiFlags(IntFlag): diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index fdf5453239..e208d5ff4b 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -226,6 +226,9 @@ class CarInterfaceBase(ABC): fp_ret.flags |= int(HondaStarPilotFlags.HAS_CAMERA_MESSAGES) elif platform in HYUNDAI: + if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD): + fp_ret.flags |= HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING.value + if candidate in CANFD_CAR: hda2 = Ecu.adas in [fw.ecu for fw in car_fw] CAN = CanBus(None, fingerprint, bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING)) diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 63bf1a8505..c5f5db09ae 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -33,6 +33,9 @@ class CarController(CarControllerBase): self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + self.main_bus = CanBus.main_for_cp(CP) + self.angle_bus = CanBus.angle_for_cp(CP) + self.status_bus = CanBus.camera if CP.flags & SubaruFlags.D_PLATFORM else CanBus.main if CP.flags & SubaruFlags.LKAS_ANGLE: self.VM = VehicleModel(get_safety_CP()) @@ -63,7 +66,7 @@ class CarController(CarControllerBase): apply_steer = CS.out.steeringAngleDeg self.apply_steer_last = apply_steer - return subarucan.create_steering_control_angle(self.packer, apply_steer, lat_active) + return subarucan.create_steering_control_angle(self.packer, apply_steer, lat_active, self.angle_bus) def lateral_torque(self, CC, CS): apply_torque = int(round(CC.actuators.torque * self.p.STEER_MAX)) @@ -154,14 +157,16 @@ class CarController(CarControllerBase): else: if self.frame % 10 == 0: can_sends.append(subarucan.create_es_dashstatus(self.packer, self.frame // 10, CS.es_dashstatus_msg, CC.enabled, - self.CP.openpilotLongitudinalControl, CC.longActive, hud_control.leadVisible)) + self.CP.openpilotLongitudinalControl, CC.longActive, hud_control.leadVisible, + self.status_bus)) can_sends.append(subarucan.create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, - hud_control.leftLaneDepart, hud_control.rightLaneDepart)) + hud_control.leftLaneDepart, hud_control.rightLaneDepart, self.status_bus)) if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: - can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert)) + can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, + hud_control.visualAlert, self.status_bus)) if starpilot_toggles.subaru_sng: can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg["COUNTER"] + 1, CS.throttle_msg, @@ -183,7 +188,7 @@ class CarController(CarControllerBase): else: if pcm_cancel_cmd: if not (self.CP.flags & SubaruFlags.HYBRID): - bus = CanBus.alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else CanBus.main + bus = CanBus.alt_for_cp(self.CP) if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else self.main_bus can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd)) if self.CP.flags & SubaruFlags.DISABLE_EYESIGHT: diff --git a/opendbc_repo/opendbc/car/subaru/carstate.py b/opendbc_repo/opendbc/car/subaru/carstate.py index ae679cb323..8a15a4161c 100644 --- a/opendbc_repo/opendbc/car/subaru/carstate.py +++ b/opendbc_repo/opendbc/car/subaru/carstate.py @@ -20,6 +20,7 @@ class CarState(CarStateBase): cp = can_parsers[Bus.pt] cp_cam = can_parsers[Bus.cam] cp_alt = can_parsers[Bus.alt] + cp_angle = cp_cam if self.CP.flags & SubaruFlags.D_PLATFORM else cp ret = structs.CarState() throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_alt.vl["Throttle_Hybrid"] @@ -54,16 +55,17 @@ class CarState(CarStateBase): cp.vl["Dashlights"]["RIGHT_BLINKER"]) if self.CP.enableBsm: - ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1) - ret.rightBlindspot = (cp.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["R_APPROACHING"] == 1) + cp_bsm = cp_cam if self.CP.flags & SubaruFlags.D_PLATFORM else cp + ret.leftBlindspot = (cp_bsm.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp_bsm.vl["BSD_RCTA"]["L_APPROACHING"] == 1) + ret.rightBlindspot = (cp_bsm.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp_bsm.vl["BSD_RCTA"]["R_APPROACHING"] == 1) cp_transmission = cp_alt if self.CP.flags & SubaruFlags.HYBRID else cp can_gear = int(cp_transmission.vl["Transmission"]["Gear"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) if self.CP.flags & SubaruFlags.LKAS_ANGLE: - ret.steeringAngleDeg = cp.vl["Steering_2"]["Steering_Angle"] - steering_updated = len(cp.vl_all["Steering_2"]["Steering_Angle"]) > 0 + ret.steeringAngleDeg = cp_angle.vl["Steering_2"]["Steering_Angle"] + steering_updated = len(cp_angle.vl_all["Steering_2"]["Steering_Angle"]) > 0 else: ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"] steering_updated = len(cp.vl_all["Steering_Torque"]["Steering_Angle"]) > 0 @@ -72,8 +74,8 @@ class CarState(CarStateBase): # ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, steering_updated) - ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"] - ret.steeringTorqueEps = cp.vl["Steering_Torque"]["Steer_Torque_Output"] + ret.steeringTorque = cp_angle.vl["Steering_Torque"]["Steer_Torque_Sensor"] + ret.steeringTorqueEps = cp_angle.vl["Steering_Torque"]["Steer_Torque_Output"] steer_threshold = 75 if self.CP.flags & SubaruFlags.PREGLOBAL else 80 ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold @@ -100,13 +102,13 @@ class CarState(CarStateBase): cp.vl["BodyInfo"]["DOOR_OPEN_RL"], cp.vl["BodyInfo"]["DOOR_OPEN_FR"], cp.vl["BodyInfo"]["DOOR_OPEN_FL"]]) - ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1 + ret.steerFaultPermanent = cp_angle.vl["Steering_Torque"]["Steer_Error_1"] == 1 if self.CP.flags & SubaruFlags.PREGLOBAL: self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"] self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"] else: - ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1 + ret.steerFaultTemporary = cp_angle.vl["Steering_Torque"]["Steer_Warning"] == 1 ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1 ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3 ret.stockFcw = (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 1) or \ @@ -145,7 +147,7 @@ class CarState(CarStateBase): @staticmethod def get_can_parsers(CP): return { - Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main), + Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main_for_cp(CP)), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.camera), - Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.alt) + Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.alt_for_cp(CP)) } diff --git a/opendbc_repo/opendbc/car/subaru/interface.py b/opendbc_repo/opendbc/car/subaru/interface.py index a0181ed09a..5518c85f79 100644 --- a/opendbc_repo/opendbc/car/subaru/interface.py +++ b/opendbc_repo/opendbc/car/subaru/interface.py @@ -3,7 +3,7 @@ from opendbc.car.disable_ecu import disable_ecu from opendbc.car.interfaces import CarInterfaceBase from opendbc.car.subaru.carcontroller import CarController from opendbc.car.subaru.carstate import CarState -from opendbc.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFlags, SubaruSafetyFlags +from opendbc.car.subaru.values import CAR, CanBus, GLOBAL_ES_ADDR, SubaruFlags, SubaruSafetyFlags class CarInterface(CarInterfaceBase): @@ -29,12 +29,15 @@ class CarInterface(CarInterfaceBase): ret.enableBsm = 0x25c in fingerprint[0] ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.subaruPreglobal)] else: - ret.enableBsm = 0x228 in fingerprint[0] + bsm_bus = CanBus.camera if ret.flags & SubaruFlags.D_PLATFORM else CanBus.main + ret.enableBsm = 0x228 in fingerprint[bsm_bus] ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.subaru)] if ret.flags & SubaruFlags.GLOBAL_GEN2: ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.GEN2.value if ret.flags & SubaruFlags.LKAS_ANGLE: ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LKAS_ANGLE.value + if ret.flags & SubaruFlags.D_PLATFORM: + ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM.value ret.steerLimitTimer = 0.4 ret.steerActuatorDelay = 0.1 diff --git a/opendbc_repo/opendbc/car/subaru/subarucan.py b/opendbc_repo/opendbc/car/subaru/subarucan.py index 87bf827940..93a354ab2d 100644 --- a/opendbc_repo/opendbc/car/subaru/subarucan.py +++ b/opendbc_repo/opendbc/car/subaru/subarucan.py @@ -13,13 +13,13 @@ def create_steering_control(packer, apply_torque, steer_req): return packer.make_can_msg("ES_LKAS", 0, values) -def create_steering_control_angle(packer, apply_angle, steer_req): +def create_steering_control_angle(packer, apply_angle, steer_req, bus=CanBus.main): values = { "LKAS_Output": apply_angle, "LKAS_Request": steer_req, "SET_3": 3 } - return packer.make_can_msg("ES_LKAS_ANGLE", 0, values) + return packer.make_can_msg("ES_LKAS_ANGLE", bus, values) def create_steering_status(packer): @@ -67,7 +67,8 @@ def create_es_distance(packer, frame, es_distance_msg, bus, pcm_cancel_cmd, long return packer.make_can_msg("ES_Distance", bus, values) -def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): +def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart, + bus=CanBus.main): values = {s: es_lkas_state_msg[s] for s in [ "CHECKSUM", "LKAS_Alert_Msg", @@ -128,10 +129,10 @@ def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert values["LKAS_Left_Line_Visible"] = int(left_line) values["LKAS_Right_Line_Visible"] = int(right_line) - return packer.make_can_msg("ES_LKAS_State", CanBus.main, values) + return packer.make_can_msg("ES_LKAS_State", bus, values) -def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, long_active, lead_visible): +def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, long_active, lead_visible, bus=CanBus.main): values = {s: dashstatus_msg[s] for s in [ "CHECKSUM", "PCB_Off", @@ -177,10 +178,10 @@ def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, l if values["LKAS_State_Msg"] in (2, 3): values["LKAS_State_Msg"] = 0 - return packer.make_can_msg("ES_DashStatus", CanBus.main, values) + return packer.make_can_msg("ES_DashStatus", bus, values) -def create_es_brake(packer, frame, es_brake_msg, long_enabled, long_active, brake_value): +def create_es_brake(packer, frame, es_brake_msg, long_enabled, long_active, brake_value, bus=CanBus.main): values = {s: es_brake_msg[s] for s in [ "CHECKSUM", "Signal1", @@ -204,10 +205,10 @@ def create_es_brake(packer, frame, es_brake_msg, long_enabled, long_active, brak values["Cruise_Brake_Active"] = brake_value > 0 values["Cruise_Brake_Lights"] = brake_value >= 70 - return packer.make_can_msg("ES_Brake", CanBus.main, values) + return packer.make_can_msg("ES_Brake", bus, values) -def create_es_status(packer, frame, es_status_msg, long_enabled, long_active, cruise_rpm): +def create_es_status(packer, frame, es_status_msg, long_enabled, long_active, cruise_rpm, bus=CanBus.main): values = {s: es_status_msg[s] for s in [ "CHECKSUM", "Signal1", @@ -227,10 +228,10 @@ def create_es_status(packer, frame, es_status_msg, long_enabled, long_active, cr values["Cruise_Activated"] = long_active - return packer.make_can_msg("ES_Status", CanBus.main, values) + return packer.make_can_msg("ES_Status", bus, values) -def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert): +def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert, bus=CanBus.main): # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts values = {s: es_infotainment_msg[s] for s in [ "CHECKSUM", @@ -253,7 +254,7 @@ def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert): if visual_alert == VisualAlert.fcw: values["LKAS_State_Infotainment"] = 2 - return packer.make_can_msg("ES_Infotainment", CanBus.main, values) + return packer.make_can_msg("ES_Infotainment", bus, values) def create_es_highbeamassist(packer): diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index f2e963689a..4bb99d44fc 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -2,11 +2,13 @@ from types import SimpleNamespace import pytest +from opendbc.car import Bus from opendbc.car.subaru.carcontroller import CarController +from opendbc.car.subaru.carstate import CarState from opendbc.car.subaru.fingerprints import FW_VERSIONS from opendbc.car.fw_versions import match_fw_to_car from opendbc.car.subaru.interface import CarInterface -from opendbc.car.subaru.values import CAR, SubaruFlags, SubaruSafetyFlags +from opendbc.car.subaru.values import CAR, CanBus, SubaruFlags, SubaruSafetyFlags from opendbc.car.structs import CarParams @@ -112,6 +114,30 @@ def test_torque_platform_does_not_enable_angle_safety(): assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LKAS_ANGLE) +def test_outback_2023_uses_d_platform_bus_layout(): + CP = CarInterface.get_non_essential_params(CAR.SUBARU_OUTBACK_2023) + parsers = CarState.get_can_parsers(CP) + + assert CP.flags & SubaruFlags.D_PLATFORM + assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM + assert CanBus.main_for_cp(CP) == CanBus.alt + assert CanBus.angle_for_cp(CP) == CanBus.camera + assert parsers[Bus.pt].bus == CanBus.alt + assert parsers[Bus.cam].bus == CanBus.camera + assert parsers[Bus.alt].bus == CanBus.alt + + +def test_other_angle_platforms_keep_existing_bus_layout(): + CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025) + parsers = CarState.get_can_parsers(CP) + + assert not (CP.flags & SubaruFlags.D_PLATFORM) + assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM) + assert parsers[Bus.pt].bus == CanBus.main + assert parsers[Bus.cam].bus == CanBus.camera + assert parsers[Bus.alt].bus == CanBus.alt + + def test_angle_controller_tracks_driver_override(): CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025) controller = CarController({}, CP) diff --git a/opendbc_repo/opendbc/car/subaru/values.py b/opendbc_repo/opendbc/car/subaru/values.py index 468f241fec..ee1cdfabf9 100644 --- a/opendbc_repo/opendbc/car/subaru/values.py +++ b/opendbc_repo/opendbc/car/subaru/values.py @@ -74,6 +74,7 @@ class SubaruSafetyFlags(IntFlag): PREGLOBAL_REVERSED_DRIVER_TORQUE = 4 STOP_AND_GO = 8 LKAS_ANGLE = 16 + D_PLATFORM = 32 class SubaruFlags(IntFlag): @@ -90,6 +91,7 @@ class SubaruFlags(IntFlag): PREGLOBAL = 16 HYBRID = 32 LKAS_ANGLE = 64 + D_PLATFORM = 128 GLOBAL_ES_ADDR = 0x787 @@ -101,6 +103,18 @@ class CanBus: alt = 1 camera = 2 + @staticmethod + def main_for_cp(CP): + return CanBus.alt if CP.flags & SubaruFlags.D_PLATFORM else CanBus.main + + @staticmethod + def alt_for_cp(CP): + return CanBus.alt + + @staticmethod + def angle_for_cp(CP): + return CanBus.camera if CP.flags & SubaruFlags.D_PLATFORM else CanBus.main + class Footnote(Enum): GLOBAL = CarFootnote( @@ -221,7 +235,7 @@ class CAR(Platforms): SUBARU_OUTBACK_2023 = SubaruGen2PlatformConfig( [SubaruCarDocs("Subaru Outback 2023-24", "All", car_parts=CarParts.common([CarHarness.subaru_d]))], SUBARU_OUTBACK.specs, - flags=SubaruFlags.LKAS_ANGLE, + flags=SubaruFlags.LKAS_ANGLE | SubaruFlags.D_PLATFORM, ) SUBARU_ASCENT_2023 = SubaruGen2PlatformConfig( [SubaruCarDocs("Subaru Ascent 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d]))], diff --git a/opendbc_repo/opendbc/safety/modes/subaru.h b/opendbc_repo/opendbc/safety/modes/subaru.h index 73d4814010..94f69cedfe 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru.h +++ b/opendbc_repo/opendbc/safety/modes/subaru.h @@ -55,6 +55,12 @@ #define SUBARU_COMMON_TX_MSGS(alt_bus) \ {MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = false}, \ +#define SUBARU_D_PLATFORM_ANGLE_TX_MSGS() \ + {MSG_SUBARU_ES_LKAS_ANGLE, SUBARU_CAM_BUS, 8, .check_relay = true}, \ + {MSG_SUBARU_ES_DashStatus, SUBARU_CAM_BUS, 8, .check_relay = true}, \ + {MSG_SUBARU_ES_LKAS_State, SUBARU_CAM_BUS, 8, .check_relay = true}, \ + {MSG_SUBARU_ES_Infotainment, SUBARU_CAM_BUS, 8, .check_relay = true}, \ + #define SUBARU_COMMON_LONG_TX_MSGS(alt_bus) \ {MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = true}, \ {MSG_SUBARU_ES_Brake, alt_bus, 8, .check_relay = true}, \ @@ -85,10 +91,21 @@ {.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ {.msg = {{MSG_SUBARU_ES_Status, status_bus, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ +#define SUBARU_D_PLATFORM_ANGLE_RX_CHECKS() \ + {.msg = {{MSG_SUBARU_Throttle, SUBARU_ALT_BUS, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_CAM_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Steering_2, SUBARU_CAM_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Wheel_Speeds, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_Brake_Status, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_ES_Brake, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_ES_Status, SUBARU_ALT_BUS, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{MSG_SUBARU_ES_DashStatus, SUBARU_CAM_BUS, 8, 10U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + static bool subaru_gen2 = false; static bool subaru_longitudinal = false; static bool subaru_stop_and_go = false; static bool subaru_lkas_angle = false; +static bool subaru_d_platform = false; static uint32_t subaru_get_checksum(const CANPacket_t *msg) { return (uint8_t)msg->data[0]; @@ -110,15 +127,17 @@ static uint32_t subaru_compute_checksum(const CANPacket_t *msg) { static void subaru_rx_hook(const CANPacket_t *msg) { const unsigned int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS; const unsigned int status_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_CAM_BUS; + const unsigned int steering_bus = subaru_d_platform ? SUBARU_CAM_BUS : SUBARU_MAIN_BUS; + const unsigned int main_bus = subaru_d_platform ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS; - if ((msg->addr == MSG_SUBARU_Steering_Torque) && (msg->bus == SUBARU_MAIN_BUS)) { + if ((msg->addr == MSG_SUBARU_Steering_Torque) && (msg->bus == steering_bus)) { int torque_driver_new; torque_driver_new = ((GET_BYTES(msg, 0, 4) >> 16) & 0x7FFU); torque_driver_new = -1 * to_signed(torque_driver_new, 11); update_sample(&torque_driver, torque_driver_new); } - if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_Steering_2) && (msg->bus == SUBARU_MAIN_BUS)) { + if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_Steering_2) && (msg->bus == steering_bus)) { int angle_meas_new = GET_BYTES(msg, 3, 3) & 0x1FFFFU; angle_meas_new = -1 * to_signed(angle_meas_new, 17); update_sample(&angle_meas, angle_meas_new); @@ -152,7 +171,7 @@ static void subaru_rx_hook(const CANPacket_t *msg) { brake_pressed = (msg->data[7] >> 6) & 1U; } - if ((msg->addr == MSG_SUBARU_Throttle) && (msg->bus == SUBARU_MAIN_BUS)) { + if ((msg->addr == MSG_SUBARU_Throttle) && (msg->bus == main_bus)) { gas_pressed = msg->data[4] != 0U; } } @@ -286,6 +305,11 @@ static safety_config subaru_init(uint16_t param) { SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS) }; + static const CanMsg SUBARU_D_PLATFORM_ANGLE_TX_MSGS[] = { + SUBARU_D_PLATFORM_ANGLE_TX_MSGS() + SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS) + }; + static RxCheck subaru_rx_checks[] = { SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS) }; @@ -302,6 +326,10 @@ static safety_config subaru_init(uint16_t param) { SUBARU_LKAS_ANGLE_RX_CHECKS(SUBARU_ALT_BUS, SUBARU_ALT_BUS) }; + static RxCheck subaru_d_platform_angle_rx_checks[] = { + SUBARU_D_PLATFORM_ANGLE_RX_CHECKS() + }; + const uint16_t SUBARU_PARAM_GEN2 = 1; subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2); @@ -312,6 +340,9 @@ static safety_config subaru_init(uint16_t param) { const uint16_t SUBARU_PARAM_LKAS_ANGLE = 16; subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE); + const uint16_t SUBARU_PARAM_D_PLATFORM = 32; + subaru_d_platform = GET_FLAG(param, SUBARU_PARAM_D_PLATFORM); + #ifdef ALLOW_DEBUG const uint16_t SUBARU_PARAM_LONGITUDINAL = 2; subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL); @@ -319,7 +350,8 @@ static safety_config subaru_init(uint16_t param) { safety_config ret; if (subaru_lkas_angle) { - ret = subaru_gen2 ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_TX_MSGS) : \ + ret = subaru_d_platform ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_TX_MSGS) : \ + subaru_gen2 ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_TX_MSGS) : \ BUILD_SAFETY_CFG(subaru_lkas_angle_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS); } else if (subaru_gen2) { ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_LONG_TX_MSGS) : \ diff --git a/opendbc_repo/opendbc/safety/tests/common.py b/opendbc_repo/opendbc/safety/tests/common.py index 4047a6e673..ed80f95f4e 100644 --- a/opendbc_repo/opendbc/safety/tests/common.py +++ b/opendbc_repo/opendbc/safety/tests/common.py @@ -1026,6 +1026,9 @@ class SafetyTest(SafetyTestBase): if attr.startswith('TestHyundaiCanfdCCNC') and current_test.startswith('TestSubaruPreglobal'): tx = list(filter(lambda m: m[0] not in [0x161], tx)) + if current_test == 'TestSubaruDPlatformAngleSafety' and attr.startswith('TestRivian'): + tx = list(filter(lambda m: not (m[1] == 2 and m[0] in [0x321, 0x322, 0x323]), tx)) + if attr.startswith('TestHyundaiLongitudinal') or attr in ('TestHyundaiSafetyFCEVLong', 'TestHyundaiLongitudinalAolLkasOnEngageSafety', 'TestHyundaiCanCanfdBlendedLongitudinalSafety', diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru.py b/opendbc_repo/opendbc/safety/tests/test_subaru.py index be4836fe64..0b9099883e 100755 --- a/opendbc_repo/opendbc/safety/tests/test_subaru.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru.py @@ -340,6 +340,39 @@ class TestSubaruGen2AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafe TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS, SubaruMsg.ES_LKAS_ANGLE) +class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase): + FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM + ALT_MAIN_BUS = SUBARU_ALT_BUS + TX_MSGS = [[SubaruMsg.ES_LKAS_ANGLE, SUBARU_CAM_BUS], + [SubaruMsg.ES_DashStatus, SUBARU_CAM_BUS], + [SubaruMsg.ES_LKAS_State, SUBARU_CAM_BUS], + [SubaruMsg.ES_Infotainment, SUBARU_CAM_BUS], + [SubaruMsg.ES_Distance, SUBARU_ALT_BUS]] + RELAY_MALFUNCTION_ADDRS = {SUBARU_CAM_BUS: (SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_DashStatus, + SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment)} + D_PLATFORM_STATUS_ADDRS = (SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_DashStatus, + SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment) + FWD_BLACKLISTED_ADDRS = { + SUBARU_MAIN_BUS: D_PLATFORM_STATUS_ADDRS, + } + + def _torque_driver_msg(self, torque): + return self.packer.make_can_msg_safety("Steering_Torque", SUBARU_CAM_BUS, {"Steer_Torque_Sensor": torque}) + + def _user_gas_msg(self, gas): + return self.packer.make_can_msg_safety("Throttle", SUBARU_ALT_BUS, {"Throttle_Pedal": gas}) + + def _angle_cmd_msg(self, angle, enabled, increment_timer=True): + if increment_timer: + self.safety.set_timer(self.angle_cmd_cnt * int(1e6 / self.LATERAL_FREQUENCY)) + self.angle_cmd_cnt += 1 + values = {"LKAS_Output": angle, "LKAS_Request": enabled, "SET_3": 3} + return self.packer.make_can_msg_safety("ES_LKAS_ANGLE", SUBARU_CAM_BUS, values) + + def _angle_meas_msg(self, angle): + return self.packer.make_can_msg_safety("Steering_2", SUBARU_CAM_BUS, {"Steering_Angle": angle}) + + class TestSubaruGen2LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruGen2TorqueSafetyBase): FLAGS = SubaruSafetyFlags.LONG | SubaruSafetyFlags.GEN2 TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS) + long_tx_msgs(SUBARU_ALT_BUS) + gen2_long_additional_tx_msgs() diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index f90171fe8f..3b1f245afd 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -799,6 +799,11 @@ PRIUS_CENTER_TAPER_LAT = 0.16 PRIUS_CENTER_TAPER_LAT_WIDTH = 0.035 PRIUS_CENTER_TAPER_SPEED = 18.0 PRIUS_CENTER_TAPER_SPEED_WIDTH = 2.2 +PRIUS_CENTER_FRICTION_THRESHOLD_GAIN = 0.08 +PRIUS_CENTER_FRICTION_THRESHOLD_LAT = 0.30 +PRIUS_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.07 +PRIUS_CENTER_FRICTION_THRESHOLD_SPEED = 18.0 +PRIUS_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.2 RAV4_PRIME_PHASE_SCALE = 0.12 RAV4_PRIME_TURN_IN_FF_BOOST_LEFT = 0.055 @@ -853,8 +858,10 @@ SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED = 27.0 SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED_WIDTH = 3.0 LEXUS_IS_PHASE_SCALE = 0.10 -LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.06 -LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT = 0.12 +LEXUS_IS_TURN_IN_FF_BOOST_LEFT = 0.04 +LEXUS_IS_TURN_IN_FF_BOOST_RIGHT = 0.04 +LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.10 +LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT = 0.16 LEXUS_IS_UNWIND_LAT_ONSET = 0.18 LEXUS_IS_UNWIND_LAT_WIDTH = 0.07 LEXUS_IS_UNWIND_SPEED_ONSET = 9.0 @@ -876,14 +883,15 @@ RAM_1500_TRANSITION_LAT_FADE_END = 1.85 # reverses the requested lateral acceleration roughly once per second at # 29-30 m/s. Fade only rapid, high-speed turn-building torque so the EPS has # less stored torque to unwind while leaving steady curves and counter-torque. -KONA_NON_SCC_TRANSITION_TAPER_MAX = 0.34 +KONA_NON_SCC_TRANSITION_TURN_IN_TAPER_MAX = 0.24 +KONA_NON_SCC_TRANSITION_UNWIND_TAPER_MAX = 0.42 KONA_NON_SCC_TRANSITION_SPEED_ONSET = 22.0 KONA_NON_SCC_TRANSITION_SPEED_FULL = 29.0 KONA_NON_SCC_TRANSITION_JERK_ONSET = 0.35 KONA_NON_SCC_TRANSITION_JERK_FULL = 1.20 KONA_NON_SCC_TRANSITION_LAT_FADE_START = 0.45 KONA_NON_SCC_TRANSITION_LAT_FADE_END = 1.60 -KONA_NON_SCC_CENTER_TAPER_MAX = 0.20 +KONA_NON_SCC_CENTER_TAPER_MAX = 0.14 KONA_NON_SCC_CENTER_TAPER_LAT = 0.28 KONA_NON_SCC_CENTER_TAPER_SPEED_ONSET = 12.0 KONA_NON_SCC_CENTER_TAPER_SPEED_FULL = 24.0 @@ -1117,6 +1125,13 @@ def get_prius_friction_threshold(v_ego: float, desired_lateral_accel: float = 0. _flm_vehicle_knob("toyota_prius.unwind_threshold_increase_right", PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT), ) * transition_envelope * unwind_weight) + center_speed_weight = _prius_sigmoid((v_ego - PRIUS_CENTER_FRICTION_THRESHOLD_SPEED) / + PRIUS_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH) + center_lat_weight = _prius_sigmoid((PRIUS_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) / + PRIUS_CENTER_FRICTION_THRESHOLD_LAT_WIDTH) + threshold_scale += (_flm_vehicle_knob("toyota_prius.center_friction_threshold_gain", + PRIUS_CENTER_FRICTION_THRESHOLD_GAIN) * + center_speed_weight * center_lat_weight) return base_threshold * min(max(threshold_scale, 0.86), 1.16) @@ -1269,11 +1284,14 @@ def get_lexus_is_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: fl return 1.0 phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / LEXUS_IS_PHASE_SCALE) + turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) lat_weight = _sigmoid((abs(desired_lateral_accel) - LEXUS_IS_UNWIND_LAT_ONSET) / LEXUS_IS_UNWIND_LAT_WIDTH) speed_weight = _sigmoid((v_ego - LEXUS_IS_UNWIND_SPEED_ONSET) / LEXUS_IS_UNWIND_SPEED_WIDTH) + turn_in_boost = LEXUS_IS_TURN_IN_FF_BOOST_LEFT if desired_lateral_accel >= 0.0 else LEXUS_IS_TURN_IN_FF_BOOST_RIGHT reduction = LEXUS_IS_UNWIND_FF_REDUCTION_LEFT if desired_lateral_accel >= 0.0 else LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT - return 1.0 - (reduction * unwind_weight * lat_weight * speed_weight) + return (1.0 + (turn_in_boost * turn_in_weight * lat_weight * speed_weight) - + (reduction * unwind_weight * lat_weight * speed_weight)) def get_subaru_impreza_pid_output_scale(angle_error_deg: float) -> float: @@ -1298,7 +1316,10 @@ def get_kona_non_scc_highway_transition_output_scale(desired_lateral_accel: floa [KONA_NON_SCC_TRANSITION_JERK_ONSET, KONA_NON_SCC_TRANSITION_JERK_FULL], [0.0, 1.0])) lat_weight = 1.0 - float(np.interp(abs(desired_lateral_accel), [KONA_NON_SCC_TRANSITION_LAT_FADE_START, KONA_NON_SCC_TRANSITION_LAT_FADE_END], [0.0, 1.0])) - return 1.0 - (KONA_NON_SCC_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight) + taper_max = (KONA_NON_SCC_TRANSITION_UNWIND_TAPER_MAX + if desired_lateral_accel * desired_lateral_jerk < 0.0 + else KONA_NON_SCC_TRANSITION_TURN_IN_TAPER_MAX) + return 1.0 - (taper_max * speed_weight * jerk_weight * lat_weight) def get_kona_non_scc_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: @@ -3229,6 +3250,7 @@ FLM_SUPPORTED_VEHICLE_KNOBS = { "toyota_prius.turn_in_threshold_reduction_right": {"profile": "toyota_prius", "min": 0.0, "max": 0.50, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT}, "toyota_prius.unwind_threshold_increase_left": {"profile": "toyota_prius", "min": 0.0, "max": 0.90, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT}, "toyota_prius.unwind_threshold_increase_right": {"profile": "toyota_prius", "min": 0.0, "max": 0.90, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT}, + "toyota_prius.center_friction_threshold_gain": {"profile": "toyota_prius", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_CENTER_FRICTION_THRESHOLD_GAIN}, } diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f5fea01b52..a16337aff3 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -292,6 +292,9 @@ class LongControl: error = a_target - CS.aEgo self.update_mpc_mode(self.experimental_mode) self.vehicle_tuning.shape_volt_test_tune_integrator(self.pid, error, CS.vEgo) + self.vehicle_tuning.trim_volt_cruise_integrator( + self.pid, a_target, error, CS.vEgo, should_stop, has_lead, + ) self._trim_positive_overshoot_integrator(a_target, error, CS) self.vehicle_tuning.trim_gm_truck_positive_hold_integrator( self.pid, self.last_output_accel, a_target, error, CS, diff --git a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py index 57ec2e84de..c77e28b01b 100644 --- a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py @@ -34,6 +34,10 @@ TOYOTA_SIENNA_COMFORT_FILTER_MIN_TTC = 4.5 TOYOTA_SIENNA_COMFORT_FILTER_MAX_CLOSING_SPEED = 4.0 TOYOTA_SIENNA_COMFORT_FILTER_MAX_LEAD_BRAKE = 2.5 TOYOTA_SIENNA_COMFORT_FILTER_BRAKE_BYPASS = -2.5 +VOLT_CRUISE_INTEGRATOR_MIN_SPEED = 8.0 +VOLT_CRUISE_INTEGRATOR_TARGET_MAX = 0.12 +VOLT_CRUISE_INTEGRATOR_ERROR_MAX = 0.12 +VOLT_CRUISE_INTEGRATOR_LEAK = 0.995 def get_bolt_acc_pedal_friction_bias(output_accel, a_target, v_ego): @@ -260,6 +264,17 @@ class LongControlVehicleTuning: bleed = interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.82, 0.86, 0.90, 0.94]) pid.i *= bleed + def trim_volt_cruise_integrator(self, pid, a_target, error, v_ego, should_stop, has_lead): + """Release stale negative I during settled open-road speed holding.""" + if not self.is_volt or should_stop or has_lead: + return + if v_ego < VOLT_CRUISE_INTEGRATOR_MIN_SPEED: + return + if abs(a_target) > VOLT_CRUISE_INTEGRATOR_TARGET_MAX or abs(error) > VOLT_CRUISE_INTEGRATOR_ERROR_MAX: + return + if pid.i < 0.0: + pid.i *= VOLT_CRUISE_INTEGRATOR_LEAK + def trim_gm_truck_positive_hold_integrator(self, pid, last_output_accel, a_target, error, CS): if not self.is_gm_stock_truck or pid.i <= 0.0: return diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index c0eb468902..4f3a1fb859 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -51,10 +51,11 @@ def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead: return ConditionalExperimentalMode(planner) -def make_sm(traffic_mode_enabled: bool = False): +def make_sm(traffic_mode_enabled: bool = False, car_fingerprint: str = ""): return { "carState": SimpleNamespace(standstill=False, leftBlinker=False, rightBlinker=False, steeringAngleDeg=0.0), "starpilotCarState": SimpleNamespace(trafficModeEnabled=traffic_mode_enabled), + "carParams": SimpleNamespace(carFingerprint=car_fingerprint), } @@ -107,6 +108,30 @@ def test_predicted_stop_within_threshold_triggers_stop_light(): assert cem.stop_light_detected +def test_elantra_stop_light_filter_uses_earlier_approach_timing(): + v_ego = 36 * CV.MPH_TO_MS + model_length = v_ego * 4.0 + + default_cem = make_cem(model_length=model_length) + elantra_cem = make_cem(model_length=model_length) + default_sm = make_sm() + elantra_sm = make_sm(car_fingerprint="HYUNDAI_ELANTRA_2021") + + default_steps = None + elantra_steps = None + for step in range(1, 30): + default_cem.stop_sign_and_light(v_ego, default_sm, model_time=7.0) + elantra_cem.stop_sign_and_light(v_ego, elantra_sm, model_time=7.0) + if default_steps is None and default_cem.stop_light_detected: + default_steps = step + if elantra_steps is None and elantra_cem.stop_light_detected: + elantra_steps = step + + assert elantra_steps is not None + assert default_steps is not None + assert elantra_steps < default_steps + + def test_chattering_lead_does_not_trigger_stop_light(): v_ego = 22 * CV.MPH_TO_MS model_length = v_ego * 4.0 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 96bf53e769..f21e16789b 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -635,6 +635,12 @@ class TestLatControl: def test_prius_friction_curves(self): base_threshold = get_gm_base_friction_threshold(12.0) + low_speed_center_threshold = get_prius_friction_threshold(8.0, 0.0, 0.0) + high_speed_center_threshold = get_prius_friction_threshold(30.0, 0.0, 0.0) + high_speed_curve_threshold = get_prius_friction_threshold(30.0, 0.8, 0.0) + assert high_speed_center_threshold > get_gm_base_friction_threshold(30.0) + assert high_speed_center_threshold > low_speed_center_threshold + assert high_speed_curve_threshold < high_speed_center_threshold left_turn_in_threshold = get_prius_friction_threshold(6.0, 0.7, 0.8) right_turn_in_threshold = get_prius_friction_threshold(6.0, -0.7, -0.8) left_unwind_threshold = get_prius_friction_threshold(6.0, 0.7, -0.8) @@ -832,13 +838,13 @@ class TestLatControl: center_transition = get_kona_non_scc_highway_transition_output_scale(0.4, 1.25, 30.0) medium_transition = get_kona_non_scc_highway_transition_output_scale(1.1, -1.25, 30.0) - assert center_transition == pytest.approx(0.66) + assert center_transition == pytest.approx(0.76) assert center_transition < medium_transition < 1.0 assert get_kona_non_scc_highway_transition_output_scale(1.65, 2.5, 30.0) == pytest.approx(1.0) def test_kona_non_scc_center_taper_curve(self): assert get_kona_non_scc_center_taper_scale(0.0, 10.0) == pytest.approx(1.0) - assert get_kona_non_scc_center_taper_scale(0.0, 25.0) == pytest.approx(0.80) + assert get_kona_non_scc_center_taper_scale(0.0, 25.0) == pytest.approx(0.86) assert get_kona_non_scc_center_taper_scale(0.28, 25.0) == pytest.approx(1.0) assert get_kona_non_scc_center_taper_scale(0.10, 25.0) < get_kona_non_scc_center_taper_scale(0.10, 15.0) @@ -1018,11 +1024,13 @@ class TestLatControl: def test_lexus_is_ff_scale_curve(self): steady_left = get_lexus_is_ff_scale(0.6, 0.0, 22.0) turn_in_left = get_lexus_is_ff_scale(0.6, 0.5, 22.0) + turn_in_right = get_lexus_is_ff_scale(-0.6, -0.5, 22.0) unwind_left = get_lexus_is_ff_scale(0.6, -0.5, 22.0) unwind_right = get_lexus_is_ff_scale(-0.6, 0.5, 22.0) low_speed_unwind_right = get_lexus_is_ff_scale(-0.6, 0.5, 5.0) assert steady_left == 1.0 - assert turn_in_left == 1.0 + assert turn_in_left > steady_left + assert turn_in_right > steady_left assert unwind_right < unwind_left < steady_left assert unwind_right < low_speed_unwind_right < 1.0 diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index eb55881260..068a91b7b3 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -715,6 +715,68 @@ def test_non_interceptor_volt_testing_ground_handoff_freezes_integrator(monkeypa assert lc.vehicle_tuning.integrator_hold_frames > 0 +def test_volt_cruise_integrator_releases_stale_negative_bias(): + CP = car.CarParams.new_message() + CP.brand = "gm" + CP.carFingerprint = "CHEVROLET_VOLT_ASCM" + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.0] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.5] + + lc = LongControl(CP) + lc.long_control_state = LongCtrlState.pid + lc.pid.i = -0.20 + CS = car.CarState.new_message(vEgo=18.0, aEgo=0.0, brakePressed=False, gasPressed=False) + CS.cruiseState.standstill = False + + output_accel = lc.update( + active=True, + CS=CS, + a_target=0.0, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(), + has_lead=False, + ) + + assert lc.pid.i > -0.20 + assert output_accel > -0.20 + + +@pytest.mark.parametrize("kwargs", [ + {"has_lead": True}, + {"should_stop": True}, + {"a_target": -0.25}, + {"aEgo": 0.25}, + {"vEgo": 4.0}, +]) +def test_volt_cruise_integrator_does_not_release_outside_settled_open_road(kwargs): + CP = car.CarParams.new_message() + CP.brand = "gm" + CP.carFingerprint = "CHEVROLET_VOLT_ASCM" + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.0] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.5] + + lc = LongControl(CP) + pid = SimpleNamespace(i=-0.20) + v_ego = kwargs.get("vEgo", 18.0) + a_ego = kwargs.get("aEgo", 0.0) + a_target = kwargs.get("a_target", 0.0) + lc.vehicle_tuning.trim_volt_cruise_integrator( + pid, + a_target=a_target, + error=a_target - a_ego, + v_ego=v_ego, + should_stop=kwargs.get("should_stop", False), + has_lead=kwargs.get("has_lead", False), + ) + + assert pid.i == pytest.approx(-0.20) + + def test_negative_target_unwinds_positive_accel_command_after_sign_flip(): CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) CP.longitudinalTuning.kpBP = [0.0] diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index 507a971575..8a49378da2 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -3,7 +3,7 @@ from pathlib import Path from types import SimpleNamespace from openpilot.common.realtime import DT_MDL -from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner +from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner, get_force_stop_jerk_scale import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module @@ -33,6 +33,11 @@ def make_toggles(**overrides): return SimpleNamespace(**defaults) +def test_force_stop_jerk_scale_is_platform_specific(): + assert get_force_stop_jerk_scale(SimpleNamespace(carFingerprint="HYUNDAI_ELANTRA_2021")) == 0.60 + assert get_force_stop_jerk_scale(SimpleNamespace(carFingerprint="OTHER_CAR")) == 0.32 + + def make_sm(planner, *, frame: int, v_ego: float, left_blinker: bool, right_blinker: bool = False, standstill: bool = False): return FakeSM(frame, { "radarState": SimpleNamespace( diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index b13fec29e1..bdb1780278 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -64,6 +64,10 @@ class ConditionalExperimentalMode: TURN_STOP_LIGHT_VETO_MAX_SPEED = 15 * CV.MPH_TO_MS TURN_STOP_LIGHT_VETO_STEERING_ANGLE = 45.0 + STOP_LIGHT_FILTER_TIME_OVERRIDES = { + "HYUNDAI_ELANTRA_2021": 0.25, + } + # ===== END TUNING PARAMETERS ===== # Current active values @@ -436,6 +440,15 @@ class ConditionalExperimentalMode: filter_time_curves = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_curves]) filter_time_leads = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_leads]) filter_time_lights = interp(speed_mph, bp, [self.LOW_SPEED_LIGHT_FILTER_TIME, self.LOW_SPEED_LIGHT_FILTER_TIME, tuned_filter_time_lights]) + try: + car_params = sm["carParams"] + except (KeyError, IndexError, TypeError, AttributeError): + car_params = None + car_fingerprint = str(getattr(car_params, "carFingerprint", "")) + filter_time_lights = min( + filter_time_lights, + self.STOP_LIGHT_FILTER_TIME_OVERRIDES.get(car_fingerprint, filter_time_lights), + ) lead_clear_filter_time = interp(speed_mph, bp, [self.LEAD_CLEAR_FILTER_TIME_LOW, self.LEAD_CLEAR_FILTER_TIME_LOW, self.LEAD_CLEAR_FILTER_TIME_HIGH]) light_boost = interp(speed_mph, bp, [low_boost, low_boost, tuned_boost]) cap_factor = interp(speed_mph, bp, [low_cap_factor, low_cap_factor, tuned_cap_factor]) diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 73ce4e0edd..82d49a5611 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -31,6 +31,14 @@ from openpilot.starpilot.controls.lib.weather_checker import WeatherChecker RADARLESS_TRACK_HOLD_TIME = 0.45 FORCE_STOP_JERK_SCALE = 0.32 # accel-change cost multiplier while forcing_stop (125 -> ~40) +FORCE_STOP_JERK_SCALE_OVERRIDES = { + "HYUNDAI_ELANTRA_2021": 0.60, +} + + +def get_force_stop_jerk_scale(car_params): + fingerprint = str(getattr(car_params, "carFingerprint", "")) + return FORCE_STOP_JERK_SCALE_OVERRIDES.get(fingerprint, FORCE_STOP_JERK_SCALE) def _sanitize_json_value(value): @@ -287,7 +295,14 @@ class StarPilotPlanner: # While committed to a Force Stop, cut the MPC's accel-change penalty so terminal # braking can ramp faster. 0.32 lands near 40, what long_mpc uses in blended mode. - jerk_scale = FORCE_STOP_JERK_SCALE if self.starpilot_vcruise.forcing_stop else 1.0 + if self.starpilot_vcruise.forcing_stop: + try: + car_params = sm["carParams"] + except (KeyError, IndexError, TypeError, AttributeError): + car_params = None + jerk_scale = get_force_stop_jerk_scale(car_params) + else: + jerk_scale = 1.0 starpilotPlan.accelerationJerk = float(A_CHANGE_COST * self.starpilot_following.acceleration_jerk * jerk_scale) starpilotPlan.dangerFactor = float(self.starpilot_following.danger_factor) starpilotPlan.dangerJerk = float(DANGER_ZONE_COST * self.starpilot_following.danger_jerk) diff --git a/starpilot/navigation/test_destination_store.py b/starpilot/navigation/test_destination_store.py index cab470df76..9841aee6ef 100644 --- a/starpilot/navigation/test_destination_store.py +++ b/starpilot/navigation/test_destination_store.py @@ -46,3 +46,18 @@ def test_recent_destinations_dedupe_and_cap(): assert len(updated) == 10 assert [entry["place_name"] for entry in updated].count("Home") == 1 assert updated[-1]["place_name"] == "Old 9" + + +def test_recent_destinations_retain_coordinates_for_saved_name(): + updated = update_recent_destinations("[]", { + "name": "Renamed favorite", + "latitude": 41.881832, + "longitude": -87.623177, + }) + + assert updated[0] == { + "place_name": "Renamed favorite", + "name": "Renamed favorite", + "latitude": 41.881832, + "longitude": -87.623177, + } diff --git a/starpilot/starpilot_process.py b/starpilot/starpilot_process.py index dc4d94f9c8..c2db1b463e 100644 --- a/starpilot/starpilot_process.py +++ b/starpilot/starpilot_process.py @@ -252,7 +252,7 @@ def starpilot_thread(): config_realtime_process(5, Priority.CTRL_LOW) pm = messaging.PubMaster(["starpilotPlan"]) - sm = messaging.SubMaster(["carControl", "carState", "controlsState", "deviceState", "driverMonitoringState", + sm = messaging.SubMaster(["carControl", "carParams", "carState", "controlsState", "deviceState", "driverMonitoringState", "gpsLocation", "gpsLocationExternal", "liveParameters", "managerState", "modelV2", "onroadEvents", "pandaStates", "radarState", "selfdriveState", "starpilotCarState", "starpilotRadarState", "starpilotSelfdriveState", "starpilotModelV2", "starpilotOnroadEvents", "mapdOut"], diff --git a/starpilot/system/the_galaxy/assets/components/navigation/navigation_destination.js b/starpilot/system/the_galaxy/assets/components/navigation/navigation_destination.js index de8f9f7ff9..87cf11986d 100644 --- a/starpilot/system/the_galaxy/assets/components/navigation/navigation_destination.js +++ b/starpilot/system/the_galaxy/assets/components/navigation/navigation_destination.js @@ -407,7 +407,19 @@ export function NavDestination() { try { const prev = JSON.parse(data.previousDestinations); - state.previousDestinations = prev.map(d => ({ name: d.place_name })); + state.previousDestinations = prev.map(d => { + const name = cleanSuggestionText(d?.place_name || d?.name || ""); + if (!name) return null; + + const latitude = Number(d?.latitude); + const longitude = Number(d?.longitude); + return { + ...d, + name, + place_name: name, + ...(Number.isFinite(latitude) && Number.isFinite(longitude) ? { latitude, longitude } : {}) + }; + }).filter(Boolean); state.suggestions = JSON.stringify(state.previousDestinations); } catch { } try { @@ -623,12 +635,14 @@ export function NavDestination() { async function selectSuggestion(sugg) { const label = sugg.full_address || sugg.name || sugg.address || "Unnamed Location"; let coords; - if (sugg.routeId) { + const savedLatitude = Number(sugg.latitude); + const savedLongitude = Number(sugg.longitude); + if (Number.isFinite(savedLatitude) && Number.isFinite(savedLongitude)) { initiateNavigation({ - name: sugg.name, - longitude: sugg.longitude, - latitude: sugg.latitude, - routeId: sugg.routeId + name: sugg.name || label, + longitude: savedLongitude, + latitude: savedLatitude, + routeId: sugg.routeId || null }); return; }