diff --git a/selfdrive/controls/tests/test_speed_limit_controller.py b/selfdrive/controls/tests/test_speed_limit_controller.py index 4d5a9ac50c..19f7b87ebe 100644 --- a/selfdrive/controls/tests/test_speed_limit_controller.py +++ b/selfdrive/controls/tests/test_speed_limit_controller.py @@ -704,6 +704,71 @@ def test_confirmation_accel_press_does_not_arm_set_speed_override(): controller.shutdown() +@pytest.mark.parametrize("accepted_by_accel_button", [True, False]) +def test_higher_confirmation_raises_cruise_speed_to_target_with_offset(accepted_by_accel_button): + controller = make_controller( + speed_limit_confirmation_higher=True, + speed_limit_offset4=mph(5), + ) + try: + controller.source = "Dashboard" + controller.target = mph(35) + controller.previous_source = "Dashboard" + controller.previous_target = mph(35) + controller.last_valid_limit = mph(35) + if not accepted_by_accel_button: + controller.starpilot_planner.params_memory.values["SpeedLimitAccepted"] = True + + controller.handle_limit_change( + "Dashboard", mph(45), "", mph(40), + make_sm( + gas_pressed=False, + accel_pressed=accepted_by_accel_button, + v_cruise_kph=40 * CV.MPH_TO_KPH, + ), + ) + + assert controller.target == pytest.approx(mph(45)) + assert controller.starpilot_planner.params_memory.get_float("SLCForceCruiseSpeed") == pytest.approx(mph(50)) + finally: + controller.shutdown() + + +@pytest.mark.parametrize( + ("current_limit", "desired_limit", "set_speed", "toggle_overrides", "sm_overrides"), + [ + (45, 65, 75, {"speed_limit_confirmation_higher": True}, {"accel_pressed": True}), + (65, 45, 65, {"speed_limit_confirmation_lower": True}, {"accel_pressed": True}), + (35, 45, 40, {}, {}), + (35, 45, 40, {"speed_limit_confirmation_higher": True}, {"long_active": False, "enabled": False}), + ], +) +def test_limit_changes_that_do_not_raise_max_do_not_force_cruise_speed( + current_limit, desired_limit, set_speed, toggle_overrides, sm_overrides, +): + controller = make_controller(**toggle_overrides) + try: + controller.source = "Dashboard" + controller.target = mph(current_limit) + controller.previous_source = "Dashboard" + controller.previous_target = mph(current_limit) + controller.last_valid_limit = mph(current_limit) + + controller.handle_limit_change( + "Dashboard", mph(desired_limit), "", mph(set_speed), + make_sm( + gas_pressed=False, + v_cruise_kph=set_speed * CV.MPH_TO_KPH, + **sm_overrides, + ), + ) + + assert controller.target == pytest.approx(mph(desired_limit)) + assert "SLCForceCruiseSpeed" not in controller.starpilot_planner.params_memory.values + finally: + controller.shutdown() + + def test_adopt_speed_limit_clears_complete_override_state(): controller = make_controller() try: diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index 2169a85dd2..5aa9b4219e 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/starpilot/controls/lib/speed_limit_controller.py @@ -285,6 +285,7 @@ class SpeedLimitController: (desired_target < self.target and self.starpilot_toggles.speed_limit_confirmation_lower) or (desired_target > self.target and self.starpilot_toggles.speed_limit_confirmation_higher) ) + higher_confirmation = confirmation_required and desired_target > self.target speed_limit_accepted = accepted_by_accel_button if not speed_limit_accepted and self._slc_adopt_counter % 4 == 0: speed_limit_accepted = self.starpilot_planner.params_memory.get_bool("SpeedLimitAccepted") @@ -297,6 +298,15 @@ class SpeedLimitController: self.source = desired_source self.target = desired_target self.clear_persistent_override_for_limit_change(previous_limit, desired_target) + set_speed_kph = float(sm["carState"].vCruise) + target_with_offset = self.target + self.offset + if ( + higher_confirmation + and long_active + and 0 < set_speed_kph < V_CRUISE_UNSET + and set_speed_kph * CV.KPH_TO_MS < target_with_offset + ): + self.starpilot_planner.params_memory.put_float("SLCForceCruiseSpeed", target_with_offset) if accepted_by_accel_button and confirmation_required: self._set_speed_override_input_consumed = True