diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 38729266e..04ca316a8 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -22,7 +22,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, SEND_BUTTON_DECREASE, SEND_BUTTON_INCREASE +from openpilot.selfdrive.car.redneck_cruise import RedneckCruise, SEND_BUTTON_DECREASE, SEND_BUTTON_INCREASE, select_redneck_target_speed from openpilot.selfdrive.car.car_specific import MockCarState from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles, update_starpilot_toggles @@ -397,23 +397,21 @@ class Car: self.CI.CS.redneck_v_target = v_target def _get_redneck_target_speed(self, CS: car.CarState) -> float: - fallback_target_speed = float(CS.cruiseState.speedCluster) - + starpilot_target_speed = 0.0 if self.sm.seen['starpilotPlan'] and self.sm.valid['starpilotPlan']: starpilot_target_speed = float(self.sm['starpilotPlan'].vCruise) - if starpilot_target_speed > 0.0: - fallback_target_speed = starpilot_target_speed + plan_speeds = [] if self.sm.seen['longitudinalPlan'] and self.sm.valid['longitudinalPlan']: plan_speeds = [float(speed) for speed in self.sm['longitudinalPlan'].speeds if math.isfinite(float(speed))] - if len(plan_speeds) > 0: - target_speed = plan_speeds[0] - decrease_target_speed = min(plan_speeds[:REDNECK_DECREASE_LOOKAHEAD_POINTS]) - if decrease_target_speed < min(target_speed, float(CS.cruiseState.speedCluster)): - return decrease_target_speed - return target_speed - return fallback_target_speed + return select_redneck_target_speed( + float(getattr(CS, "vCruise", 0.0)), + float(CS.cruiseState.speedCluster), + starpilot_target_speed, + plan_speeds, + REDNECK_DECREASE_LOOKAHEAD_POINTS, + ) def _advance_redneck_button_feedback_filter(self) -> None: if self.redneck_cruise is None: diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 29d5f1116..a372a7798 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -53,6 +53,15 @@ class VCruiseHelper: def _get_cruise_delta_interval(interval: float | None) -> float: return interval if isinstance(interval, (int, float)) and interval > 0 else 1.0 + def _get_cruise_delta_intervals(self, starpilot_toggles: SimpleNamespace) -> tuple[float, float]: + short_interval = self._get_cruise_delta_interval(getattr(starpilot_toggles, "cruise_increase", None)) + long_interval = self._get_cruise_delta_interval(getattr(starpilot_toggles, "cruise_increase_long", None)) + + if getattr(starpilot_toggles, "reverse_cruise_increase", False): + return long_interval, short_interval + + return short_interval, long_interval + @property def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET @@ -119,8 +128,8 @@ class VCruiseHelper: if not self.button_change_states[button_type]["enabled"]: return - delta_interval = starpilot_toggles.cruise_increase_long if long_press else starpilot_toggles.cruise_increase - v_cruise_delta_interval = self._get_cruise_delta_interval(delta_interval) + short_interval, long_interval = self._get_cruise_delta_intervals(starpilot_toggles) + v_cruise_delta_interval = long_interval if long_press else short_interval v_cruise_delta = v_cruise_delta * v_cruise_delta_interval if v_cruise_delta_interval % 5 == 0 and self.v_cruise_kph % v_cruise_delta != 0: # partial interval self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta diff --git a/selfdrive/car/redneck_cruise.py b/selfdrive/car/redneck_cruise.py index ac5efe1ac..52f70c132 100644 --- a/selfdrive/car/redneck_cruise.py +++ b/selfdrive/car/redneck_cruise.py @@ -23,6 +23,23 @@ CRUISE_BUTTON_TIMERS = { } +def select_redneck_target_speed(v_cruise_kph: float, speed_cluster_ms: float, + starpilot_target_speed_ms: float, plan_speeds_ms: list[float], + lookahead_points: int) -> float: + target_speed_ms = float(speed_cluster_ms) + if v_cruise_kph > 0: + target_speed_ms = float(v_cruise_kph) * CV.KPH_TO_MS + elif starpilot_target_speed_ms > 0: + target_speed_ms = float(starpilot_target_speed_ms) + + if len(plan_speeds_ms) > 0: + decrease_target_speed_ms = min(plan_speeds_ms[:lookahead_points]) + if decrease_target_speed_ms < min(target_speed_ms, float(speed_cluster_ms)): + return decrease_target_speed_ms + + return target_speed_ms + + def get_minimum_set_speed(is_metric: bool) -> int: return 30 if is_metric else 20 diff --git a/selfdrive/car/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py index 5a201f8bc..7bb069edc 100644 --- a/selfdrive/car/tests/test_cruise_speed.py +++ b/selfdrive/car/tests/test_cruise_speed.py @@ -55,6 +55,7 @@ class TestVCruiseHelper: cruise_increase=1, cruise_increase_long=5, is_metric=False, + reverse_cruise_increase=False, set_speed_limit=False, ) self.reset_cruise_speed_state() @@ -315,6 +316,7 @@ class TestVCruiseHelperRedneck: cruise_increase=1, cruise_increase_long=5, is_metric=False, + reverse_cruise_increase=False, set_speed_limit=False, ) @@ -344,3 +346,34 @@ class TestVCruiseHelperRedneck: assert self.v_cruise_helper.v_cruise_kph == pytest.approx(62 * CV.MPH_TO_KPH) assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(62 * CV.MPH_TO_KPH) + + def test_reverse_cruise_increase_swaps_short_and_long_press_intervals(self): + self.enable(55 * CV.MPH_TO_MS, experimental_mode=False) + initial_v_cruise_kph = self.v_cruise_helper.v_cruise_kph + self.starpilot_toggles.cruise_increase = 1 + self.starpilot_toggles.cruise_increase_long = 5 + self.starpilot_toggles.reverse_cruise_increase = True + + pressed_cs = car.CarState(cruiseState={"available": True}) + pressed_cs.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=True)] + self.v_cruise_helper.update_v_cruise( + pressed_cs, + enabled=True, + is_metric=False, + speed_limit_changed=False, + starpilot_toggles=self.starpilot_toggles, + ) + + released_cs = car.CarState(cruiseState={"available": True}) + released_cs.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=False)] + self.v_cruise_helper.update_v_cruise( + released_cs, + enabled=True, + is_metric=False, + speed_limit_changed=False, + starpilot_toggles=self.starpilot_toggles, + ) + + reversed_interval = 5 * IMPERIAL_INCREMENT + expected_kph = math.ceil(initial_v_cruise_kph / reversed_interval) * reversed_interval + assert self.v_cruise_helper.v_cruise_kph == pytest.approx(expected_kph) diff --git a/selfdrive/car/tests/test_redneck_cruise.py b/selfdrive/car/tests/test_redneck_cruise.py index 0228ea4f3..d24596bf6 100644 --- a/selfdrive/car/tests/test_redneck_cruise.py +++ b/selfdrive/car/tests/test_redneck_cruise.py @@ -11,6 +11,7 @@ from openpilot.selfdrive.car.redneck_cruise import ( SEND_BUTTON_DECREASE, SEND_BUTTON_INCREASE, SEND_BUTTON_NONE, + select_redneck_target_speed, ) @@ -108,6 +109,26 @@ class TestRedneckCruise(unittest.TestCase): self.assertEqual(SEND_BUTTON_NONE, send_button) self.assertEqual(0, v_target) + def test_target_speed_returns_internal_max_when_plan_only_wants_to_speed_back_up(self): + target_speed = select_redneck_target_speed( + 120.0, + 77.0 * CV.MPH_TO_MS, + 0.0, + [78.3 * CV.MPH_TO_MS, 78.2 * CV.MPH_TO_MS, 78.1 * CV.MPH_TO_MS], + 10, + ) + self.assertAlmostEqual(120.0 * CV.KPH_TO_MS, target_speed) + + def test_target_speed_returns_plan_minimum_when_slowing_down(self): + target_speed = select_redneck_target_speed( + 120.0, + 75.0 * CV.MPH_TO_MS, + 0.0, + [74.0 * CV.MPH_TO_MS, 72.0 * CV.MPH_TO_MS, 71.0 * CV.MPH_TO_MS], + 10, + ) + self.assertAlmostEqual(71.0 * CV.MPH_TO_MS, target_speed) + if __name__ == "__main__": unittest.main() diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 45a0025e8..12615885f 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -361,20 +361,20 @@ IONIQ_5_FF_CUTOFF = 1.20 IONIQ_5_FF_CUTOFF_WIDTH = 0.30 IONIQ_5_TRANSITION_SPEED = 11.0 IONIQ_5_PHASE_SCALE = 0.10 -IONIQ_5_FF_REDUCTION_LEFT = 0.14 -IONIQ_5_FF_REDUCTION_RIGHT = 0.18 -IONIQ_5_TURN_IN_BOOST_LEFT = 0.04 +IONIQ_5_FF_REDUCTION_LEFT = 0.12 +IONIQ_5_FF_REDUCTION_RIGHT = 0.20 +IONIQ_5_TURN_IN_BOOST_LEFT = 0.08 IONIQ_5_TURN_IN_BOOST_RIGHT = 0.00 -IONIQ_5_UNWIND_TAPER_LEFT = 0.52 -IONIQ_5_UNWIND_TAPER_RIGHT = 0.82 -IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.05 +IONIQ_5_UNWIND_TAPER_LEFT = 0.60 +IONIQ_5_UNWIND_TAPER_RIGHT = 0.72 +IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.08 IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.00 -IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT = 0.24 -IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.38 -IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT = 0.02 +IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT = 0.28 +IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.30 +IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT = 0.04 IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 -IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.22 -IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.32 +IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.26 +IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.26 IONIQ_5_CENTER_TAPER_MAX = 0.08 IONIQ_5_CENTER_TAPER_LAT = 0.16 IONIQ_5_CENTER_TAPER_LAT_WIDTH = 0.03 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 1e504df5c..170977ac0 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -374,7 +374,7 @@ class TestLatControl: assert turn_in_left_scale > 1.0 assert turn_in_right_scale == pytest.approx(1.0) assert unwind_left_scale < 1.0 - assert unwind_right_scale < unwind_left_scale + assert unwind_right_scale <= unwind_left_scale def test_ioniq_5_center_taper_curve(self): assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.0, 10.0) diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 6de0fc179..9211902a7 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -1099,8 +1099,9 @@ class StarPilotVariables: toggle.pause_lateral_signal_delay = self.get_value("LateralResumeDelay", cast=float, condition=toggle.pause_lateral_below_signal, default=0.0, min=0.0, max=5.0) quality_of_life_longitudinal = toggle.openpilot_longitudinal and self.get_value("QOLLongitudinal") - toggle.cruise_increase = self.get_value("CustomCruise", cast=float, condition=(quality_of_life_longitudinal and not pcm_cruise)) - toggle.cruise_increase_long = self.get_value("CustomCruiseLong", cast=float, condition=(quality_of_life_longitudinal and not pcm_cruise)) + quality_of_life_cruise = self.get_value("QOLLongitudinal") and (toggle.openpilot_longitudinal or not FPCP.pcmCruiseSpeed) + toggle.cruise_increase = self.get_value("CustomCruise", cast=float, condition=quality_of_life_cruise, default=1.0) + toggle.cruise_increase_long = self.get_value("CustomCruiseLong", cast=float, condition=quality_of_life_cruise, default=5.0) toggle.force_stops = self.get_value("ForceStops", condition=quality_of_life_longitudinal) toggle.force_stop_distance_offset = self.get_value("ForceStopDistanceOffset", cast=int, condition=(quality_of_life_longitudinal and toggle.force_stops)) toggle.force_standstill = self.get_value("ForceStandstill", condition=quality_of_life_longitudinal) @@ -1108,7 +1109,7 @@ class StarPilotVariables: map_gears = self.get_value("MapGears", condition=quality_of_life_longitudinal) toggle.map_acceleration = self.get_value("MapAcceleration", condition=map_gears) toggle.map_deceleration = self.get_value("MapDeceleration", condition=map_gears) - toggle.reverse_cruise_increase = self.get_value("ReverseCruise", condition=quality_of_life_longitudinal and toggle.car_make == "toyota" and pcm_cruise) + toggle.reverse_cruise_increase = self.get_value("ReverseCruise", condition=quality_of_life_cruise) toggle.set_speed_offset = self.get_value("SetSpeedOffset", cast=float, condition=(quality_of_life_longitudinal and not pcm_cruise), conversion=(1 if toggle.is_metric else CV.MPH_TO_KPH)) toggle.weather_presets = self.get_value("WeatherPresets", condition=quality_of_life_longitudinal) toggle.increase_following_distance_low_visibility = self.get_value("IncreaseFollowingLowVisibility", cast=float, condition=toggle.weather_presets)