mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-18 05:23:57 +08:00
Code Issues Packages Projects Releases Wiki Activity

hackathon

Browse Source
This commit is contained in:
firestar5683
2026-09-16 13:12:42 -05:00
parent 0fee545400
commit 09b53ccf9f
4 changed files with 142 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longcontrol_vehicle_tunes.py
+1
View File
@@ -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
):
selfdrive/controls/tests/test_longcontrol.py
+7
View File
@@ -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)
starpilot/car/ford/lateral.py
+66 -10
View File
@@ -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]))
starpilot/car/ford/tests/test_lateral.py
+68 -1
View File
@@ -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