diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index cfbef1016..1480d361e 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -434,10 +434,13 @@ class Car: starpilot_plan = self.sm['starpilotPlan'] starpilot_target_speed = float(starpilot_plan.vCruise) if self.starpilot_toggles.speed_limit_controller: - slc_target_speed = max( - float(starpilot_plan.slcOverriddenSpeed), - float(starpilot_plan.slcSpeedLimit) + float(starpilot_plan.slcSpeedLimitOffset), + overridden_speed = float(starpilot_plan.slcOverriddenSpeed) + slc_limit = float(starpilot_plan.slcSpeedLimit) + float(starpilot_plan.slcSpeedLimitOffset) + allow_lower_override = ( + getattr(self.starpilot_toggles, "redneck_cruise", False) and + getattr(self.starpilot_toggles, "speed_limit_controller_override_set_speed", False) ) + slc_target_speed = overridden_speed if allow_lower_override and overridden_speed > 0 else max(overridden_speed, slc_limit) # Use acceleration projection only when SLC has no resolved target. if self.CP.openpilotLongitudinalControl and slc_target_speed <= 0.0: diff --git a/selfdrive/car/tests/test_redneck_cruise.py b/selfdrive/car/tests/test_redneck_cruise.py index c192dbb3e..0ca59c45a 100644 --- a/selfdrive/car/tests/test_redneck_cruise.py +++ b/selfdrive/car/tests/test_redneck_cruise.py @@ -248,6 +248,41 @@ class TestRedneckCruise(unittest.TestCase): self.assertAlmostEqual(slc_target, target_speed) self.assertFalse(lead_present) + def test_card_target_speed_honors_lower_redneck_slc_override(self): + starpilot_plan = SimpleNamespace( + vCruise=65.0 * CV.KPH_TO_MS, + slcOverriddenSpeed=55.0 * CV.KPH_TO_MS, + slcSpeedLimit=65.0 * CV.KPH_TO_MS, + slcSpeedLimitOffset=0.0, + ) + sm = MagicMock() + sm.seen = {"starpilotPlan": True, "longitudinalPlan": False, "radarState": False} + sm.valid = sm.seen.copy() + sm.__getitem__.side_effect = {"starpilotPlan": starpilot_plan}.__getitem__ + card = SimpleNamespace( + CP=SimpleNamespace(openpilotLongitudinalControl=True), + sm=sm, + starpilot_toggles=SimpleNamespace( + speed_limit_controller=True, + redneck_cruise=True, + speed_limit_controller_override_set_speed=True, + ), + ) + car_state = SimpleNamespace( + vEgo=60.0 * CV.KPH_TO_MS, + vCruise=65.0, + cruiseState=SimpleNamespace(speedCluster=65.0 * CV.KPH_TO_MS), + ) + car_control = SimpleNamespace( + actuators=SimpleNamespace(accel=0.0), + hudControl=SimpleNamespace(leadVisible=False), + ) + + target_speed, lead_present = Car._get_redneck_target_speed(card, car_state, car_control) + + self.assertAlmostEqual(55.0 * CV.KPH_TO_MS, target_speed) + self.assertFalse(lead_present) + def test_target_speed_returns_plan_minimum_when_slowing_down(self): target_speed = select_redneck_target_speed( 120.0, diff --git a/selfdrive/controls/tests/test_speed_limit_controller.py b/selfdrive/controls/tests/test_speed_limit_controller.py index 29b93bb51..74cbd15f7 100644 --- a/selfdrive/controls/tests/test_speed_limit_controller.py +++ b/selfdrive/controls/tests/test_speed_limit_controller.py @@ -49,6 +49,7 @@ def make_toggles(**overrides): "speed_limit_confirmation_lower": False, "speed_limit_controller_override_manual": True, "speed_limit_controller_override_set_speed": False, + "redneck_cruise": False, "speed_limit_filler": False, "speed_limit_offset1": 0.0, "speed_limit_offset2": 0.0, @@ -487,6 +488,30 @@ def test_set_speed_override_clears_on_new_speed_zone(): controller.shutdown() +def test_redneck_set_speed_mode_overrides_in_both_directions(): + controller = make_controller( + speed_limit_controller_override_manual=False, + speed_limit_controller_override_set_speed=True, + redneck_cruise=True, + ) + try: + controller.source = "Dashboard" + controller.target = mph(45) + controller.last_valid_limit = mph(45) + + controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False)) + controller.update_override(mph(60), 0.0, mph(45), 0.0, make_sm(gas_pressed=False)) + assert controller.override_slc + assert controller.overridden_speed == pytest.approx(mph(60)) + + # A manual decrease below the posted limit must become the new redneck target. + controller.update_override(mph(35), 0.0, mph(45), 0.0, make_sm(gas_pressed=False)) + assert controller.override_slc + assert controller.overridden_speed == pytest.approx(mph(35)) + finally: + controller.shutdown() + + def test_gas_pedal_mode_ignores_set_speed_without_gas(): # Set With Gas Pedal mode: a high set speed alone must NOT override; gas is still required. controller = make_controller( diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 54170fbd9..5b87fee58 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -244,6 +244,20 @@ def test_active_slc_control_target_applies_offset_and_cluster_diff(): assert target == pytest.approx((48.0 * CV.MPH_TO_MS) - 0.4) +def test_active_slc_control_target_allows_lower_redneck_override(): + target = get_active_slc_control_target( + speed_limit_controller=True, + set_speed_limit=False, + slc_target=65.0 * CV.MPH_TO_MS, + slc_offset=0.0, + overridden_speed=35.0 * CV.MPH_TO_MS, + v_ego_diff=0.4, + allow_lower_override=True, + ) + + assert target == pytest.approx((35.0 * CV.MPH_TO_MS) - 0.4) + + def test_slc_lead_drop_relaxed_target_softens_map_stepdown_for_harmless_lead(): raw_target = 55.0 * CV.MPH_TO_MS previous_target = 65.0 * CV.MPH_TO_MS diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index 295e2ff78..40758e7f5 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/starpilot/controls/lib/speed_limit_controller.py @@ -129,7 +129,15 @@ class SpeedLimitController: target_with_offset = target_to_use + self.get_offset(target_to_use) if target_with_offset <= 0 or not self.override_mode_enabled: return False - return self.overridden_speed > target_with_offset or (gas_pressed and v_ego > target_with_offset) + bidirectional_set_speed = ( + getattr(self.starpilot_toggles, "redneck_cruise", False) and + getattr(self.starpilot_toggles, "speed_limit_controller_override_set_speed", False) + ) + return ( + (bidirectional_set_speed and self.overridden_speed > 0) or + self.overridden_speed > target_with_offset or + (gas_pressed and v_ego > target_with_offset) + ) def clear_override_for_source_limit(self, desired_source, desired_target, had_override): if desired_source == "None" or desired_target <= 0: @@ -484,12 +492,12 @@ class SpeedLimitController: self.map_speed_limit = self.next_speed_limit def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm): - # A +/- press that raises the set speed is the gesture that (re)arms Max Set Speed - # override. Detect the rising edge on the raw set speed (button-driven, no cluster jitter); - # requiring a fresh edge is what makes the override clear per speed zone — once a new posted - # limit wipes it (clear_override_for_source_limit), a steady high set speed will not re-arm. + # Detect +/- changes on the raw set speed (button-driven, no cluster jitter). Requiring a + # fresh edge is what makes the override clear per speed zone — once a new posted limit wipes + # it, a steady set speed will not re-arm. prev_v_cruise = self._prev_v_cruise self._prev_v_cruise = v_cruise + set_speed_changed = prev_v_cruise is not None and abs(v_cruise - prev_v_cruise) > SET_SPEED_RAISE_EPS set_speed_raised = prev_v_cruise is not None and v_cruise > prev_v_cruise + SET_SPEED_RAISE_EPS if not sm["selfdriveState"].enabled: @@ -508,13 +516,24 @@ class SpeedLimitController: target_to_use = self.target_to_use offset = self.get_offset(target_to_use) set_speed = v_cruise + v_cruise_diff - self.override_slc = self.override_slc and self.overridden_speed > target_to_use + offset > 0 + bidirectional_set_speed = ( + getattr(self.starpilot_toggles, "redneck_cruise", False) and + getattr(self.starpilot_toggles, "speed_limit_controller_override_set_speed", False) + ) + self.override_slc = self.override_slc and ( + (bidirectional_set_speed and self.overridden_speed > 0) or + self.overridden_speed > target_to_use + offset > 0 + ) self.override_slc |= not self.override_requires_gas_release and sm["carState"].gasPressed and v_ego > target_to_use + offset > 0 - # Max Set Speed mode: raising the set speed (+/-) above the posted limit overrides the - # SLC hold directly, no gas pedal required. Only a fresh +/- press arms it, so entering a - # new speed zone clears the override until the driver raises the set speed again. - self.override_slc |= (self.starpilot_toggles.speed_limit_controller_override_set_speed - and set_speed_raised and set_speed > target_to_use + offset > 0) + # Redneck Max Set Speed mode uses +/- as a direct, bidirectional SLC override. The normal + # mode retains its existing upward-only behavior for full-long cars. + self.override_slc |= ( + self.starpilot_toggles.speed_limit_controller_override_set_speed and + target_to_use + offset > 0 and + set_speed > 0 and + ((bidirectional_set_speed and set_speed_changed) or + (not bidirectional_set_speed and set_speed_raised and set_speed > target_to_use + offset > 0)) + ) if self.override_slc: if self.starpilot_toggles.speed_limit_controller_override_manual: diff --git a/starpilot/controls/lib/starpilot_acceleration.py b/starpilot/controls/lib/starpilot_acceleration.py index 089354b67..30c398738 100644 --- a/starpilot/controls/lib/starpilot_acceleration.py +++ b/starpilot/controls/lib/starpilot_acceleration.py @@ -221,6 +221,8 @@ class StarPilotAcceleration: getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0), getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0), v_ego_diff, + allow_lower_override=(getattr(starpilot_toggles, "redneck_cruise", False) and + getattr(starpilot_toggles, "speed_limit_controller_override_set_speed", False)), ) v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise) if effective_slc_target > 0.0: diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 4e4dced0f..c45ec174e 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -70,13 +70,17 @@ OFFSET_FT_MIN = -20 OFFSET_FT_MAX = 20 -def get_active_slc_control_target(speed_limit_controller, set_speed_limit, slc_target, slc_offset, overridden_speed, v_ego_diff): +def get_active_slc_control_target(speed_limit_controller, set_speed_limit, slc_target, slc_offset, overridden_speed, + v_ego_diff, allow_lower_override=False): # `SetSpeedLimit` only controls engage-time set-speed initialization. Ongoing # SLC speed matching must remain active whenever Speed Limit Controller is on. if not speed_limit_controller: return 0.0 - base_target = max(float(overridden_speed), float(slc_target) + float(slc_offset)) + if allow_lower_override and overridden_speed > 0: + base_target = float(overridden_speed) + else: + base_target = max(float(overridden_speed), float(slc_target) + float(slc_offset)) if base_target <= 0.0: return 0.0 @@ -590,6 +594,8 @@ class StarPilotVCruise: self.slc_offset, self.slc.overridden_speed, v_ego_diff, + allow_lower_override=(getattr(starpilot_toggles, "redneck_cruise", False) and + getattr(starpilot_toggles, "speed_limit_controller_override_set_speed", False)), ) slc_control_target = get_slc_lead_drop_relaxed_target( slc_control_target, diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index 106acd30b..444bd5e16 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -18,6 +18,8 @@ from openpilot.starpilot.common.favorite_slots import FAVORITE_ACTION_TRAFFIC_MO from openpilot.starpilot.common.starpilot_utilities import is_FrogsGoMoo from openpilot.starpilot.common.starpilot_variables import ERROR_LOGS_PATH, GearShifter, NON_DRIVING_GEARS +HYUNDAI_MAIN_CRUISE_AOL_CONFIRM_TIMEOUT_FRAMES = 100 + class StarPilotCard: @staticmethod @@ -44,6 +46,9 @@ class StarPilotCard: self.CP.brand == "hyundai" and not (hyundai_flags & HyundaiFlags.CANFD) and not hyundai_aol_before_engagement ) self.hyundai_aol_ready = False + self.g70_main_cruise_aol_pending = False + self.g70_main_cruise_aol_pending_frames = 0 + self.prev_cruise_available = None self.prev_active = False self.prev_cruise_enabled = False self.decel_pressed = False @@ -130,6 +135,15 @@ class StarPilotCard: button_event_types = [self._button_type_raw(be) for be in carState.buttonEvents] button_aol_supported = self.CP.brand == "hyundai" or starpilot_toggles.lkas_allowed_for_aol button_managed_aol = starpilot_toggles.always_on_lateral_lkas or (button_aol_supported and starpilot_toggles.main_cruise_aol_toggle) + g70_main_cruise_aol_managed = ( + getattr(self.CP, "carFingerprint", None) == HYUNDAI_CAR.GENESIS_G70_2020 + and starpilot_toggles.main_cruise_aol_toggle + ) + + if carState.gearShifter in NON_DRIVING_GEARS or not g70_main_cruise_aol_managed: + self.g70_main_cruise_aol_pending = False + self.g70_main_cruise_aol_pending_frames = 0 + hyundai_aol_needs_engagement = self.hyundai_aol_needs_engagement and not starpilot_toggles.always_on_lateral_lkas if hyundai_aol_needs_engagement: @@ -153,10 +167,28 @@ class StarPilotCard: if starpilot_toggles.main_cruise_aol_toggle: if hyundai_aol_needs_engagement: self.hyundai_aol_ready = True - self.always_on_lateral_allowed = not self.always_on_lateral_allowed + if g70_main_cruise_aol_managed: + # The G70 reports the main-cruise transition after the button press. + # Wait for that state change before sending active LKAS11 torque. + self.g70_main_cruise_aol_pending = True + self.g70_main_cruise_aol_pending_frames = 0 + else: + self.always_on_lateral_allowed = not self.always_on_lateral_allowed elif starpilot_toggles.main_cruise_slc_adopt and starpilot_toggles.speed_limit_controller: self.params_memory.put_bool("SLCAdoptSpeedLimit", True) + cruise_available_changed = self.prev_cruise_available is not None and carState.cruiseState.available != self.prev_cruise_available + if self.g70_main_cruise_aol_pending: + if cruise_available_changed: + self.always_on_lateral_allowed = carState.cruiseState.available + self.g70_main_cruise_aol_pending = False + self.g70_main_cruise_aol_pending_frames = 0 + else: + self.g70_main_cruise_aol_pending_frames += 1 + if self.g70_main_cruise_aol_pending_frames >= HYUNDAI_MAIN_CRUISE_AOL_CONFIRM_TIMEOUT_FRAMES: + self.g70_main_cruise_aol_pending = False + self.g70_main_cruise_aol_pending_frames = 0 + if starpilot_toggles.always_on_lateral_main and not button_managed_aol: car_fingerprint = getattr(self.CP, "carFingerprint", None) pcm_cruise = getattr(self.CP, "pcmCruise", False) @@ -179,6 +211,7 @@ class StarPilotCard: self.prev_active = sm["selfdriveState"].active self.prev_cruise_enabled = carState.cruiseState.enabled + self.prev_cruise_available = carState.cruiseState.available self.always_on_lateral_enabled = self.always_on_lateral_allowed and self.always_on_lateral_set self.always_on_lateral_enabled &= carState.gearShifter not in NON_DRIVING_GEARS diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index 46d4e2659..a640f2dbc 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -409,14 +409,51 @@ def test_genesis_g90_main_cruise_button_toggles_aol_immediately(monkeypatch, tmp assert ret.alwaysOnLateralEnabled is True -@pytest.mark.parametrize("fingerprint", [spc.HYUNDAI_CAR.GENESIS_G70_2020, spc.HYUNDAI_CAR.HYUNDAI_PALISADE]) -def test_legacy_hyundai_main_cruise_button_toggles_aol_immediately(monkeypatch, tmp_path, fingerprint): +def test_genesis_g70_main_cruise_button_waits_for_cruise_availability(monkeypatch, tmp_path): monkeypatch.setattr(spc, "Params", FakeParams) monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False) monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) card = spc.StarPilotCard( - SimpleNamespace(brand="hyundai", carFingerprint=fingerprint), + SimpleNamespace(brand="hyundai", carFingerprint=spc.HYUNDAI_CAR.GENESIS_G70_2020), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + + car_state = make_car_state(button_events=[SimpleNamespace(type=spc.ButtonType.mainCruise, pressed=True)]) + starpilot_car_state = SimpleNamespace(distancePressed=False) + sm = make_sm() + toggles = make_toggles(always_on_lateral=True, main_cruise_aol_toggle=True) + + card.update(make_car_state(), starpilot_car_state, sm, toggles) + ret = card.update(car_state, starpilot_car_state, sm, toggles) + assert ret.alwaysOnLateralAllowed is False + assert ret.alwaysOnLateralEnabled is False + + car_state.buttonEvents = [] + car_state.cruiseState.available = True + ret = card.update(car_state, starpilot_car_state, sm, toggles) + assert ret.alwaysOnLateralAllowed is True + assert ret.alwaysOnLateralEnabled is True + + car_state.buttonEvents = [SimpleNamespace(type=spc.ButtonType.mainCruise, pressed=True)] + ret = card.update(car_state, starpilot_car_state, sm, toggles) + assert ret.alwaysOnLateralAllowed is True + assert ret.alwaysOnLateralEnabled is True + + car_state.buttonEvents = [] + car_state.cruiseState.available = False + ret = card.update(car_state, starpilot_car_state, sm, toggles) + assert ret.alwaysOnLateralAllowed is False + assert ret.alwaysOnLateralEnabled is False + + +def test_legacy_hyundai_main_cruise_button_toggles_aol_immediately(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard( + SimpleNamespace(brand="hyundai", carFingerprint=spc.HYUNDAI_CAR.HYUNDAI_PALISADE), SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), )