From 09b53ccf9f0e1e573c178384f3bf3e855d905162 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 16 Sep 2026 13:12:42 -0500 Subject: [PATCH] hackathon --- .../controls/lib/longcontrol_vehicle_tunes.py | 1 + selfdrive/controls/tests/test_longcontrol.py | 7 ++ starpilot/car/ford/lateral.py | 76 ++++++++++++++++--- starpilot/car/ford/tests/test_lateral.py | 69 ++++++++++++++++- 4 files changed, 142 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py index 2abb1ed91b..ed1ccc3b4c 100644 --- a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py @@ -212,6 +212,7 @@ class LongControlVehicleTuning: if ( self.is_hyundai_elantra_2021 and should_stop and + not has_lead and v_ego < HYUNDAI_ELANTRA_FINAL_STOP_MAX_SPEED and a_target <= 0.1 ): diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index 6249de8096..f4d7992bdb 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -775,6 +775,13 @@ def test_elantra_final_stop_cap_softens_normal_low_speed_stop(): assert tuning.shape_stopping_accel(-0.85, -0.25, False, 0.5, False, -0.85) == pytest.approx(-0.85) +def test_elantra_final_stop_cap_does_not_release_brakes_behind_lead(): + CP = make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021") + tuning = vehicle_tunes.LongControlVehicleTuning(CP) + + assert tuning.shape_stopping_accel(-0.61, -0.57, True, 0.41, True, -0.85) == pytest.approx(-0.61) + + def test_elantra_stopped_lead_handoff_holds_braking_direction_without_touching_brakes(): CP = make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021") tuning = vehicle_tunes.LongControlVehicleTuning(CP) diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 9212183c9d..4129430921 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -63,12 +63,27 @@ MACH_E_LOW_SPEED_DIRECTION_CHANGE_FADE_SPEED = 3.5 MACH_E_LOW_SPEED_DIRECTION_CHANGE_MIN_CURVATURE = 0.0004 MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_CURVATURE = 0.0006 MACH_E_LOW_SPEED_DIRECTION_CHANGE_MAX_CURVATURE = 0.0015 +MACH_E_SHARP_DIRECTION_CHANGE_START_SPEED = 1.5 +MACH_E_SHARP_DIRECTION_CHANGE_FULL_SPEED = 1.8 +MACH_E_SHARP_DIRECTION_CHANGE_HOLD_SPEED = 3.0 +MACH_E_SHARP_DIRECTION_CHANGE_FADE_SPEED = 4.0 +MACH_E_SHARP_DIRECTION_CHANGE_MIN_CURVATURE = 0.0002 +MACH_E_SHARP_DIRECTION_CHANGE_FULL_CURVATURE = 0.0005 +MACH_E_SHARP_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE = 0.008 +MACH_E_SHARP_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE = 0.012 +MACH_E_SHARP_DIRECTION_CHANGE_MIN_ACCEL = 1.8 +MACH_E_SHARP_DIRECTION_CHANGE_FULL_ACCEL = 2.2 +MACH_E_SHARP_DIRECTION_CHANGE_MIN_LAG_CURVATURE = -0.0005 +MACH_E_SHARP_DIRECTION_CHANGE_FULL_LAG_CURVATURE = 0.0008 FORD_CURVATURE_LOOKAHEAD = { CAR.FORD_EXPLORER_MK6: 0.20, } FORD_CONSERVATIVE_PREVIEW_CARS = frozenset({ CAR.FORD_MUSTANG_MACH_E_MK1, }) +FORD_SHARP_DIRECTION_CHANGE_CARS = frozenset({ + CAR.FORD_MUSTANG_MACH_E_MK1, +}) FORD_MANUAL_TURN_LATCH_CARS = frozenset({ CAR.FORD_MUSTANG_MACH_E_MK1, }) @@ -252,21 +267,26 @@ class FordLateralController: )) def _direction_change_preview_weight(self, desired: float, preview: float, current: float, - allow_rising_desired: bool = False, early_handoff_weight: float = 0.0) -> float: + allow_rising_desired: bool = False, early_handoff_weight: float = 0.0, + sharp_handoff: bool = False) -> float: if self.CP.carFingerprint not in FORD_CONSERVATIVE_PREVIEW_CARS: return 0.0 if desired * preview >= 0.0 or desired * self.desired_curvature_last <= 0.0 or desired * current <= 0.0: return 0.0 early_handoff_weight = float(np.clip(early_handoff_weight, 0.0, 1.0)) lag = abs(current) - abs(desired) - lag_min = float(np.interp( - early_handoff_weight, [0.0, 1.0], - [MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_MIN_LAG_CURVATURE], - )) - lag_full = float(np.interp( - early_handoff_weight, [0.0, 1.0], - [MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_FULL_LAG_CURVATURE], - )) + if sharp_handoff: + lag_min = MACH_E_SHARP_DIRECTION_CHANGE_MIN_LAG_CURVATURE + lag_full = MACH_E_SHARP_DIRECTION_CHANGE_FULL_LAG_CURVATURE + else: + lag_min = float(np.interp( + early_handoff_weight, [0.0, 1.0], + [MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_MIN_LAG_CURVATURE], + )) + lag_full = float(np.interp( + early_handoff_weight, [0.0, 1.0], + [MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_FULL_LAG_CURVATURE], + )) desired_rising = abs(desired) >= abs(self.desired_curvature_last) rising_handoff = (allow_rising_desired and abs(desired) > abs(self.desired_curvature_last) and abs(desired) <= MACH_E_LOW_SPEED_DIRECTION_CHANGE_MAX_CURVATURE) @@ -279,6 +299,12 @@ class FordLateralController: [MACH_E_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, MACH_E_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE], [0.0, 1.0], )) + if sharp_handoff: + preview_weight *= float(np.interp( + abs(preview), + [MACH_E_SHARP_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, MACH_E_SHARP_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE], + [0.0, 1.0], + )) lag_weight = float(np.interp( lag, [lag_min, lag_full], @@ -301,6 +327,31 @@ class FordLateralController: )) return speed_weight * curvature_weight + @staticmethod + def _sharp_direction_change_weight(v_ego: float, a_ego: float, desired: float, preview: float) -> float: + speed_weight = float(np.interp( + v_ego, + [MACH_E_SHARP_DIRECTION_CHANGE_START_SPEED, MACH_E_SHARP_DIRECTION_CHANGE_FULL_SPEED, + MACH_E_SHARP_DIRECTION_CHANGE_HOLD_SPEED, MACH_E_SHARP_DIRECTION_CHANGE_FADE_SPEED], + [0.0, 1.0, 1.0, 0.0], + )) + curvature_weight = float(np.interp( + abs(desired), + [MACH_E_SHARP_DIRECTION_CHANGE_MIN_CURVATURE, MACH_E_SHARP_DIRECTION_CHANGE_FULL_CURVATURE], + [0.0, 1.0], + )) + preview_weight = float(np.interp( + abs(preview), + [MACH_E_SHARP_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, MACH_E_SHARP_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE], + [0.0, 1.0], + )) + acceleration_weight = float(np.interp( + a_ego, + [MACH_E_SHARP_DIRECTION_CHANGE_MIN_ACCEL, MACH_E_SHARP_DIRECTION_CHANGE_FULL_ACCEL], + [0.0, 1.0], + )) + return speed_weight * curvature_weight * preview_weight * acceleration_weight + def _manual_turn(self, CC, CS) -> bool: if not CC.latActive: self.human_turn.reset() @@ -369,10 +420,15 @@ class FordLateralController: turn_in_predicted = self._predicted_curvature(v_ego, lookahead + MACH_E_TURN_IN_LOOKAHEAD_EXTRA) direction_change_predicted = turn_in_predicted direction_change_weight = 0.0 + sharp_direction_change_weight = 0.0 direction_change_speed_weight = float(v_ego > MACH_E_DIRECTION_CHANGE_MIN_SPEED) low_speed_direction_change = direction_change_speed_weight == 0.0 if direction_change_speed_weight == 0.0: direction_change_speed_weight = self._low_speed_direction_change_weight(v_ego, desired) + if self.CP.carFingerprint in FORD_SHARP_DIRECTION_CHANGE_CARS: + sharp_direction_change_weight = self._sharp_direction_change_weight( + v_ego, float(CS.out.aEgo), desired, turn_in_predicted) + direction_change_speed_weight = max(direction_change_speed_weight, sharp_direction_change_weight) if direction_change_speed_weight > 0.0 and not CS.out.steeringPressed and not self._lane_change()[0]: direction_change_lookahead_extra = self._direction_change_lookahead_extra(v_ego) early_handoff_weight = float(np.interp( @@ -389,7 +445,7 @@ class FordLateralController: direction_change_predicted = self._predicted_curvature(v_ego, lookahead + direction_change_lookahead_extra) direction_change_weight = self._direction_change_preview_weight( desired, direction_change_predicted, current, allow_rising_desired=low_speed_direction_change, - early_handoff_weight=early_handoff_weight) + early_handoff_weight=early_handoff_weight, sharp_handoff=sharp_direction_change_weight > 0.0) direction_change_weight *= direction_change_speed_weight if direction_change_weight > 0.0: predicted = float(np.interp(direction_change_weight, [0.0, 1.0], [predicted, direction_change_predicted])) diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index c8fa07aebe..9c1792184a 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -32,10 +32,11 @@ def controller(monkeypatch): return controller -def car_state(speed=15.0, curvature=0.0, steering_pressed=False, steering_angle=0.0, +def car_state(speed=15.0, accel=0.0, curvature=0.0, steering_pressed=False, steering_angle=0.0, steering_torque=0.0, left_blinker=False, right_blinker=False): return SimpleNamespace(out=SimpleNamespace( vEgoRaw=speed, + aEgo=accel, yawRate=-curvature * speed, steeringPressed=steering_pressed, steeringAngleDeg=steering_angle, @@ -214,6 +215,20 @@ def test_mach_e_low_speed_direction_change_weight(controller, speed, desired, ex assert controller._low_speed_direction_change_weight(speed, desired) == pytest.approx(expected) +@pytest.mark.parametrize("speed,accel,desired,preview,expected", ( + (1.5, 2.4, 0.0006, -0.012, 0.0), + (1.8, 1.8, 0.0006, -0.012, 0.0), + (1.8, 2.0, 0.0006, -0.012, 0.5), + (1.8, 2.2, 0.0002, -0.012, 0.0), + (1.8, 2.2, 0.00035, -0.012, 0.5), + (1.8, 2.2, 0.0006, -0.010, 0.5), + (3.5, 2.2, 0.0006, -0.012, 0.5), + (4.0, 2.2, 0.0006, -0.012, 0.0), +)) +def test_mach_e_sharp_direction_change_weight(controller, speed, accel, desired, preview, expected): + assert controller._sharp_direction_change_weight(speed, accel, desired, preview) == pytest.approx(expected) + + @pytest.mark.parametrize("sign", (1.0, -1.0)) def test_mach_e_direction_change_preview_leads_a_lagging_unwind(controller, sign): controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 @@ -366,6 +381,58 @@ def test_mach_e_direction_change_preview_leads_rising_low_speed_handoff(controll pytest.approx(0.003), True)] +def test_mach_e_sharp_accelerating_direction_change_leads_below_existing_speed_gate(controller, monkeypatch): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.sm["liveDelay"].lateralDelay = 0.4 + controller.desired_curvature_last = 0.0010 + blend_inputs = [] + + def predicted_curvature(_v_ego, lookahead): + return {0.4: 0.0005, 1.2: -0.0106}[round(lookahead, 1)] + + monkeypatch.setattr(controller, "_predicted_curvature", predicted_curvature) + monkeypatch.setattr( + controller, "_blend_and_scale", + lambda desired, predicted, v_ego, current, allow_opposite_preview=False: + blend_inputs.append((desired, predicted, v_ego, current, allow_opposite_preview)) or (0.0, 1), + ) + + controller.update( + SimpleNamespace(latActive=True), car_state(speed=1.85, accel=2.4, curvature=0.00075), + SimpleNamespace(curvature=0.000426), + ) + + assert len(blend_inputs) == 1 + assert blend_inputs[0][0] == pytest.approx(0.000426) + assert blend_inputs[0][1] < 0.0 + assert blend_inputs[0][2:] == (pytest.approx(1.85), pytest.approx(0.00075), True) + + +def test_mach_e_sharp_direction_change_requires_hard_acceleration(controller, monkeypatch): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.sm["liveDelay"].lateralDelay = 0.4 + controller.desired_curvature_last = 0.0010 + blend_inputs = [] + + def predicted_curvature(_v_ego, lookahead): + return {0.4: 0.0005, 1.2: -0.0106}[round(lookahead, 1)] + + monkeypatch.setattr(controller, "_predicted_curvature", predicted_curvature) + monkeypatch.setattr( + controller, "_blend_and_scale", + lambda desired, predicted, v_ego, current, allow_opposite_preview=False: + blend_inputs.append((desired, predicted, v_ego, current, allow_opposite_preview)) or (0.0, 1), + ) + + controller.update( + SimpleNamespace(latActive=True), car_state(speed=1.85, accel=1.5, curvature=0.00075), + SimpleNamespace(curvature=0.000426), + ) + + assert blend_inputs == [(pytest.approx(0.000426), pytest.approx(0.0005), pytest.approx(1.85), + pytest.approx(0.00075), False)] + + def test_mach_e_direction_change_preview_does_not_lead_rising_high_speed_path(controller, monkeypatch): controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 controller.sm["liveDelay"].lateralDelay = 0.4