diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index d8201bbc7..adc677e68 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -66,8 +66,6 @@ REDNECK_BUTTON_COPIES_TIME_METRIC = [REDNECK_BUTTON_COPIES_TIME, 40] ANGLE_SAFETY_BASELINE_MODEL = str(CAR.KIA_SPORTAGE_HEV_2026) DEFAULT_ANGLE_SMOOTHING_VEGO_BP = [5.0, 10.0, 20.0] DEFAULT_ANGLE_SMOOTHING_ALPHA_V = [0.2, 0.1, 0.0] -KIA_EV9_ANGLE_SMOOTHING_VEGO_BP = [0.0, 8.5, 11.0, 13.8, 18.0] -KIA_EV9_ANGLE_SMOOTHING_ALPHA_V = [0.05, 0.1, 0.3, 0.6, 1.0] def egmp_dynamic_longitudinal_tuning(CP) -> bool: @@ -225,8 +223,6 @@ def get_baseline_safety_cp(): def get_angle_smoothing_alpha(CP, v_ego: float) -> float: - if CP.carFingerprint == CAR.KIA_EV9: - return float(np.interp(v_ego, KIA_EV9_ANGLE_SMOOTHING_VEGO_BP, KIA_EV9_ANGLE_SMOOTHING_ALPHA_V)) return float(np.interp(v_ego, DEFAULT_ANGLE_SMOOTHING_VEGO_BP, DEFAULT_ANGLE_SMOOTHING_ALPHA_V)) diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index f61158134..784d3b4bf 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -278,13 +278,13 @@ class TestHyundaiFingerprint: assert CP.steerControlType == CarParams.SteerControlType.angle assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANFD_ANGLE_STEERING - def test_ev9_angle_smoothing_is_ev9_specific(self): + def test_ev9_uses_shared_angle_smoothing(self): ev9_cp = SimpleNamespace(carFingerprint=CAR.KIA_EV9) other_cp = SimpleNamespace(carFingerprint=CAR.KIA_SPORTAGE_HEV_2026) - assert get_angle_smoothing_alpha(ev9_cp, 0.0) == pytest.approx(0.05) - assert get_angle_smoothing_alpha(ev9_cp, 13.8) == pytest.approx(0.6) - assert get_angle_smoothing_alpha(ev9_cp, 20.0) == pytest.approx(1.0) + assert get_angle_smoothing_alpha(ev9_cp, 0.0) == pytest.approx(get_angle_smoothing_alpha(other_cp, 0.0)) + assert get_angle_smoothing_alpha(ev9_cp, 13.8) == pytest.approx(get_angle_smoothing_alpha(other_cp, 13.8)) + assert get_angle_smoothing_alpha(ev9_cp, 20.0) == pytest.approx(get_angle_smoothing_alpha(other_cp, 20.0)) assert get_angle_smoothing_alpha(other_cp, 20.0) == pytest.approx(0.0) def test_checked_angle_limiter_blocks_rate_accel_conflict(self): diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index edcf6fe8d..49b9fd88b 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -453,25 +453,25 @@ IONIQ_5_FF_CUTOFF = 1.20 IONIQ_5_FF_CUTOFF_WIDTH = 0.30 IONIQ_5_TRANSITION_SPEED = 12.5 IONIQ_5_PHASE_SCALE = 0.10 -IONIQ_5_FF_REDUCTION_LEFT = 0.12 -IONIQ_5_FF_REDUCTION_RIGHT = 0.22 -IONIQ_5_TURN_IN_BOOST_LEFT = 0.14 -IONIQ_5_TURN_IN_BOOST_RIGHT = 0.06 -IONIQ_5_UNWIND_TAPER_LEFT = 0.76 -IONIQ_5_UNWIND_TAPER_RIGHT = 0.86 -IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.08 -IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.05 -IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT = 0.36 -IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.38 -IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT = 0.04 -IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.03 -IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.34 -IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.34 -IONIQ_5_CENTER_TAPER_MAX = 0.14 -IONIQ_5_CENTER_TAPER_LAT = 0.12 -IONIQ_5_CENTER_TAPER_LAT_WIDTH = 0.03 -IONIQ_5_CENTER_TAPER_SPEED = 16.0 -IONIQ_5_CENTER_TAPER_SPEED_WIDTH = 2.5 +IONIQ_5_FF_REDUCTION_LEFT = 0.15 +IONIQ_5_FF_REDUCTION_RIGHT = 0.25 +IONIQ_5_TURN_IN_BOOST_LEFT = 0.10 +IONIQ_5_TURN_IN_BOOST_RIGHT = 0.04 +IONIQ_5_UNWIND_TAPER_LEFT = 0.92 +IONIQ_5_UNWIND_TAPER_RIGHT = 1.04 +IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.05 +IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.03 +IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT = 0.46 +IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.50 +IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT = 0.02 +IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.01 +IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.42 +IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.44 +IONIQ_5_CENTER_TAPER_MAX = 0.18 +IONIQ_5_CENTER_TAPER_LAT = 0.16 +IONIQ_5_CENTER_TAPER_LAT_WIDTH = 0.04 +IONIQ_5_CENTER_TAPER_SPEED = 15.0 +IONIQ_5_CENTER_TAPER_SPEED_WIDTH = 2.2 IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.16 IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.16 @@ -607,27 +607,27 @@ IONIQ_6_OUTPUT_DIRECTIONAL_TAPER_BLEND = 0.97 KIA_EV6_LATERAL_TESTING_GROUND_ID = testing_ground.id_6 KIA_EV6_LATERAL_TESTING_GROUND_VARIANT = "C" -KIA_EV6_FF_GAIN_LEFT = 0.10 -KIA_EV6_FF_GAIN_RIGHT = 0.14 +KIA_EV6_FF_GAIN_LEFT = 0.12 +KIA_EV6_FF_GAIN_RIGHT = 0.17 KIA_EV6_FF_ONSET = 0.08 KIA_EV6_FF_ONSET_WIDTH = 0.04 -KIA_EV6_FF_CUTOFF = 1.55 -KIA_EV6_FF_CUTOFF_WIDTH = 0.34 -KIA_EV6_TRANSITION_SPEED = 12.5 +KIA_EV6_FF_CUTOFF = 1.90 +KIA_EV6_FF_CUTOFF_WIDTH = 0.40 +KIA_EV6_TRANSITION_SPEED = 14.5 KIA_EV6_PHASE_SCALE = 0.09 -KIA_EV6_TURN_IN_BOOST_LEFT = 0.34 -KIA_EV6_TURN_IN_BOOST_RIGHT = 0.42 +KIA_EV6_TURN_IN_BOOST_LEFT = 0.48 +KIA_EV6_TURN_IN_BOOST_RIGHT = 0.60 KIA_EV6_UNWIND_TAPER_LEFT = 0.56 KIA_EV6_UNWIND_TAPER_RIGHT = 0.54 KIA_EV6_FRICTION_MULT = 1.01 KIA_EV6_FRICTION_LAT_RISE = 0.18 KIA_EV6_FRICTION_JERK_RISE = 0.22 -KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16 -KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.30 +KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.24 +KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.40 KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT = 0.28 KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.24 -KIA_EV6_TURN_IN_FRICTION_BOOST_LEFT = 0.08 -KIA_EV6_TURN_IN_FRICTION_BOOST_RIGHT = 0.16 +KIA_EV6_TURN_IN_FRICTION_BOOST_LEFT = 0.12 +KIA_EV6_TURN_IN_FRICTION_BOOST_RIGHT = 0.24 KIA_EV6_UNWIND_FRICTION_REDUCTION_LEFT = 0.28 KIA_EV6_UNWIND_FRICTION_REDUCTION_RIGHT = 0.22 KIA_EV6_CENTER_TAPER_MAX = 0.12 @@ -642,13 +642,13 @@ KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 8.5 KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.4 VOLT_PLEXY_LATERAL_TESTING_GROUND_ID = testing_ground.id_7 -VOLT_PLEXY_FF_EXTRA_MULT_LEFT = 1.00 +VOLT_PLEXY_FF_EXTRA_MULT_LEFT = 0.97 VOLT_PLEXY_FF_EXTRA_MULT_RIGHT = 1.16 VOLT_PLEXY_FRICTION_SCALE_MULT_LEFT = 1.00 VOLT_PLEXY_FRICTION_SCALE_MULT_RIGHT = 1.05 VOLT_PLEXY_FRICTION_THRESHOLD_MULT_LEFT = 1.00 VOLT_PLEXY_FRICTION_THRESHOLD_MULT_RIGHT = 0.96 -VOLT_PLEXY_CENTER_TAPER_REDUCTION_MULT = 0.94 +VOLT_PLEXY_CENTER_TAPER_REDUCTION_MULT = 0.98 PRIUS_TRANSITION_SPEED = 8.5 PRIUS_PHASE_SCALE = 0.09 PRIUS_FF_GAIN_LEFT = 0.08 diff --git a/selfdrive/controls/tests/test_speed_limit_controller.py b/selfdrive/controls/tests/test_speed_limit_controller.py index 5056e3309..bc6bd3937 100644 --- a/selfdrive/controls/tests/test_speed_limit_controller.py +++ b/selfdrive/controls/tests/test_speed_limit_controller.py @@ -4,6 +4,7 @@ from types import SimpleNamespace import pytest from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitController @@ -296,3 +297,50 @@ def test_higher_limit_does_not_clear_override(): controller_overridden_below.shutdown() finally: controller.shutdown() + + +def test_manual_override_survives_brief_enabled_flicker(): + controller = make_controller() + try: + controller.source = "Dashboard" + controller.target = mph(45) + controller.previous_source = "Dashboard" + controller.previous_target = mph(45) + controller.overridden_speed = mph(55) + controller.override_slc = True + + disabled_sm = make_sm(gas_pressed=False, enabled=False) + for _ in range(int(0.5 / DT_MDL)): + controller.update_override(mph(75), 0.0, mph(65), 0.0, disabled_sm) + + assert controller.overridden_speed == pytest.approx(mph(55)) + assert controller.override_slc + + controller.update_override(mph(75), 0.0, mph(65), 0.0, make_sm(gas_pressed=False, enabled=True)) + + assert controller.overridden_speed == pytest.approx(mph(55)) + assert controller.override_slc + finally: + controller.shutdown() + + +def test_manual_override_clears_after_sustained_disengage(): + controller = make_controller() + try: + controller.source = "Dashboard" + controller.target = mph(45) + controller.previous_source = "Dashboard" + controller.previous_target = mph(45) + controller.overridden_speed = mph(55) + controller.override_slc = True + controller.override_requires_gas_release = True + + disabled_sm = make_sm(gas_pressed=False, enabled=False) + for _ in range(int(1.0 / DT_MDL) + 1): + controller.update_override(mph(75), 0.0, mph(65), 0.0, disabled_sm) + + assert controller.overridden_speed == 0 + assert not controller.override_slc + assert not controller.override_requires_gas_release + finally: + controller.shutdown() diff --git a/selfdrive/ui/lib/starpilot_status.py b/selfdrive/ui/lib/starpilot_status.py index b93dd5e61..98ac23486 100644 --- a/selfdrive/ui/lib/starpilot_status.py +++ b/selfdrive/ui/lib/starpilot_status.py @@ -52,9 +52,11 @@ def get_screen_edge_color(state: UIState): return SWITCHBACK_COLOR if state.always_on_lateral_active: return AOL_COLOR - if state.conditional_status in CEM_DISABLED_OVERRIDE_STATUSES: + # Keep the screen edge disengaged-blue when experimental mode is only the + # requested longitudinal mode, not the active driving state. + if enabled and state.conditional_status in CEM_DISABLED_OVERRIDE_STATUSES: return CEM_OVERRIDE_COLOR - if state.sm["selfdriveState"].experimentalMode: + if enabled and state.sm["selfdriveState"].experimentalMode: return EXPERIMENTAL_COLOR if state.traffic_mode_enabled and enabled: return TRAFFIC_COLOR diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 711ae41c7..6fd83821f 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -62,21 +62,22 @@ void OnroadWindow::updateState(const UIState &s, const StarPilotUIState &fs) { const StarPilotUIScene &starpilot_scene = fs.starpilot_scene; const QJsonObject &starpilot_toggles = starpilot_scene.starpilot_toggles; const auto selfdriveState = (*s.sm)["selfdriveState"].getSelfdriveState(); + const bool enabled = selfdriveState.getEnabled(); QColor bgColor = bg_colors[s.status]; const bool conditional_experimental_mode = starpilot_toggles.value("conditional_experimental_mode").toBool(); const bool conditional_chill_mode = starpilot_toggles.value("conditional_chill_mode").toBool(); const bool highlight_override = - (conditional_experimental_mode && starpilot_scene.conditional_status == 1) || - (conditional_chill_mode && (starpilot_scene.conditional_status == 1 || starpilot_scene.conditional_status == 2)); - if (starpilot_scene.switchback_mode_enabled && (selfdriveState.getEnabled() || starpilot_scene.always_on_lateral_active)) { + enabled && ((conditional_experimental_mode && starpilot_scene.conditional_status == 1) || + (conditional_chill_mode && (starpilot_scene.conditional_status == 1 || starpilot_scene.conditional_status == 2))); + if (starpilot_scene.switchback_mode_enabled && (enabled || starpilot_scene.always_on_lateral_active)) { bgColor = bg_colors[STATUS_SWITCHBACK_MODE_ENABLED]; } else if (starpilot_scene.always_on_lateral_active) { bgColor = bg_colors[STATUS_ALWAYS_ON_LATERAL_ACTIVE]; } else if (highlight_override) { bgColor = bg_colors[STATUS_CEM_DISABLED]; - } else if (selfdriveState.getExperimentalMode()) { + } else if (enabled && selfdriveState.getExperimentalMode()) { bgColor = bg_colors[STATUS_EXPERIMENTAL_MODE_ENABLED]; - } else if (starpilot_scene.traffic_mode_enabled && selfdriveState.getEnabled()) { + } else if (starpilot_scene.traffic_mode_enabled && enabled) { bgColor = bg_colors[STATUS_TRAFFIC_MODE_ENABLED]; } diff --git a/starpilot/common/tests/test_starpilot_process.py b/starpilot/common/tests/test_starpilot_process.py index d2cd0d6ce..91bfb02b6 100644 --- a/starpilot/common/tests/test_starpilot_process.py +++ b/starpilot/common/tests/test_starpilot_process.py @@ -54,6 +54,44 @@ def test_transition_offroad_skips_invalid_gps_persist(): assert params.writes == [] +def test_transition_offroad_skips_missing_gps_persist(): + params = FakeParams() + planner = SimpleNamespace(gps_position=None) + toggles = SimpleNamespace(lock_doors_timer=0, random_themes=False) + + starpilot_process.transition_offroad( + planner, + FakeModelManager(), + FakeThemeManager(), + FakeThreadManager(), + False, + None, + params, + toggles, + ) + + assert params.writes == [] + + +def test_transition_offroad_skips_malformed_gps_persist(): + params = FakeParams() + planner = SimpleNamespace(gps_position={"hasFix": True}) + toggles = SimpleNamespace(lock_doors_timer=0, random_themes=False) + + starpilot_process.transition_offroad( + planner, + FakeModelManager(), + FakeThemeManager(), + FakeThreadManager(), + False, + None, + params, + toggles, + ) + + assert params.writes == [] + + def test_transition_offroad_persists_valid_gps(): params = FakeParams() gps_position = { diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index 5ccd938b7..63625e828 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/starpilot/controls/lib/speed_limit_controller.py @@ -35,6 +35,8 @@ OFFSET_MAP_METRIC = [ (33.1, 38.9, "speed_limit_offset7"), # 120–140 ] +SLC_OVERRIDE_DISABLE_CLEAR_TIME = 0.75 + class SpeedLimitController: def __init__(self, StarPilotVCruise): self.starpilot_planner = StarPilotVCruise.starpilot_planner @@ -43,6 +45,7 @@ class SpeedLimitController: self.calling_mapbox = False self.override_slc = False self.override_requires_gas_release = False + self.override_disable_timer = 0.0 self.denied_target = 0 self.map_speed_limit = 0 @@ -453,11 +456,15 @@ class SpeedLimitController: def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm): if not sm["selfdriveState"].enabled: - self.override_slc = False - self.overridden_speed = 0 - self.override_requires_gas_release = False + self.override_disable_timer += DT_MDL + if self.override_disable_timer >= SLC_OVERRIDE_DISABLE_CLEAR_TIME: + self.override_slc = False + self.overridden_speed = 0 + self.override_requires_gas_release = False return + self.override_disable_timer = 0.0 + if not sm["carState"].gasPressed: self.override_requires_gas_release = False diff --git a/starpilot/starpilot_process.py b/starpilot/starpilot_process.py index 0ed04a376..370a24d08 100644 --- a/starpilot/starpilot_process.py +++ b/starpilot/starpilot_process.py @@ -75,7 +75,11 @@ def build_gps_position(gps_location, speed): def gps_position_valid(gps_position): - return bool(gps_position["hasFix"]) and (gps_position["latitude"] != 0 or gps_position["longitude"] != 0) + if not gps_position: + return False + latitude = gps_position.get("latitude") + longitude = gps_position.get("longitude") + return bool(gps_position.get("hasFix")) and latitude is not None and longitude is not None and (latitude != 0 or longitude != 0) def gps_position_signature(gps_position):