mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-10-10 00:03:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

Overrun

Browse Source
This commit is contained in:
firestar5683
2026-10-07 23:46:25 -05:00
parent 6de77049b3
commit e75470f2e8
6 changed files with 131 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
+2
View File
@@ -28,6 +28,8 @@ casadi_cost_0 = [
casadi_constraints = [
f'{gen}/long_constraints/long_constr_h_fun.c',
f'{gen}/long_constraints/long_constr_h_fun_jac_uxt_zt.c',
f'{gen}/long_constraints/long_constr_h_e_fun.c',
f'{gen}/long_constraints/long_constr_h_e_fun_jac_uxt_zt.c',
]
build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e + \
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+9 -1
View File
@@ -387,6 +387,7 @@ def gen_long_ocp():
(a_max - a_ego),
((x_obstacle - x_ego) - lead_danger_factor * (desired_dist_comfort)) / (v_ego + 10.))
ocp.model.con_h_expr = constraints
ocp.model.con_h_expr_e = constraints
x0 = np.zeros(X_DIM)
ocp.constraints.x0 = x0
@@ -399,10 +400,17 @@ def gen_long_ocp():
ocp.cost.Zl = cost_weights
ocp.cost.Zu = cost_weights
ocp.cost.zu = cost_weights
ocp.cost.zl_e = cost_weights
ocp.cost.Zl_e = cost_weights
ocp.cost.Zu_e = cost_weights
ocp.cost.zu_e = cost_weights
ocp.constraints.lh = np.zeros(CONSTR_DIM)
ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM)
ocp.constraints.idxsh = np.arange(CONSTR_DIM)
ocp.constraints.lh_e = np.zeros(CONSTR_DIM)
ocp.constraints.uh_e = 1e4*np.ones(CONSTR_DIM)
ocp.constraints.idxsh_e = np.arange(CONSTR_DIM)
# The HPIPM solver can give decent solutions even when it is stopped early
# Which is critical for our purpose where compute time is strictly bounded
@@ -503,7 +511,7 @@ class LongitudinalMpc:
# Set L2 slack cost on lower bound constraints
Zl = np.array(constraint_cost_weights)
for i in range(N):
for i in range(N+1):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True,
selfdrive/controls/tests/test_longitudinal_planner.py
+24
View File
@@ -125,6 +125,30 @@ def test_mpc_duplicate_lead_filters_do_not_cross_contaminate_tracks():
assert mpc.duplicate_lead_v_filters[1].x == pytest.approx(28.0)
def test_mpc_recovers_reverse_terminal_cruise_plan():
mpc = LongitudinalMpc()
mpc.set_cur_state(20.65, 1.05)
positions = [0., 1.436, 5.776, 13.108, 23.554, 37.173, 53.812, 72.856, 92.651, 110.775, 125.258, 134.177, 127.994]
speeds = [20.65, 20.72, 20.94, 21.29, 21.66, 21.86, 21.62, 20.35, 17.42, 13.29, 8.67, 3.56, -15.95]
accels = [1.05, 1.05, 1.05, .95, .59, .034, -.67, -2.13, -3.5, -3.5, -3.5, -3.5, -20.93]
for i, state in enumerate(zip(positions, speeds, accels, strict=True)):
mpc.solver.set(i, 'x', np.asarray(state))
mpc.prev_a = np.interp(T_IDXS_MPC + mpc.dt, T_IDXS_MPC, accels)
radar = log.RadarState.new_message()
for _ in range(20):
mpc.set_weights(250.0, 100.0, 5.5, v_ego=20.65)
mpc.set_accel_limits(-0.5, 1.05)
mpc.set_cur_state(20.65, 1.05)
trajectories = [np.zeros(len(T_IDXS_MPC)) for _ in range(4)]
mpc.update(radar, 55.0 / 3.6, *trajectories, 0.75, 1.6, tracking_lead=False)
assert mpc.solution_status == 0
assert np.min(mpc.v_solution) >= -0.01
assert mpc.params[-1, 0] - 0.01 <= mpc.a_solution[-1] <= mpc.params[-1, 1] + 0.01
assert np.interp(0.2, T_IDXS_MPC, mpc.a_solution) < 0.0
def test_prius_stopped_lead_obstacle_bias_is_small_and_vehicle_specific():
prius = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_PRIUS)
other = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
starpilot/car/ford/lateral.py
+15
View File
@@ -34,6 +34,9 @@ MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - ACCELERATION_DUE_TO_GRAVITY * 0.06
STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL
CURVATURE_LOOKAHEAD_MIN = 0.20
CURVATURE_LOOKAHEAD_MAX = 0.40
MACH_E_HIGH_SPEED_LOOKAHEAD_EXTRA = 0.40
MACH_E_HIGH_SPEED_LOOKAHEAD_START_SPEED = 16.0
MACH_E_HIGH_SPEED_LOOKAHEAD_FULL_SPEED = 23.0
MACH_E_TURN_IN_LOOKAHEAD_EXTRA = 0.80
MACH_E_LOW_SPEED_TURN_IN_LOOKAHEAD_EXTRA = 1.60
MACH_E_LOW_SPEED_TURN_IN_START_SPEED = 2.0
@@ -228,6 +231,13 @@ class FordLateralController:
direction = int(getattr(self.model.meta.laneChangeDirection, "raw", self.model.meta.laneChangeDirection))
return state in (1, 2, 3), direction
def _high_speed_lookahead_extra(self, v_ego: float, steering_pressed: bool, lane_change: bool) -> float:
if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or
steering_pressed or lane_change):
return 0.0
return MACH_E_HIGH_SPEED_LOOKAHEAD_EXTRA * float(np.interp(
v_ego, [MACH_E_HIGH_SPEED_LOOKAHEAD_START_SPEED, MACH_E_HIGH_SPEED_LOOKAHEAD_FULL_SPEED], [0.0, 1.0]))
@staticmethod
def _current_curvature(CS) -> float:
return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
@@ -595,6 +605,7 @@ class FordLateralController:
v_ego = float(CS.out.vEgoRaw)
lookahead = self._curvature_lookahead()
lookahead += self._high_speed_lookahead_extra(v_ego, bool(CS.out.steeringPressed), self._lane_change()[0])
predicted = self._predicted_curvature(v_ego, lookahead)
allow_opposite_preview = False
if self.CP.carFingerprint in FORD_CONSERVATIVE_PREVIEW_CARS:
@@ -677,6 +688,10 @@ class FordLateralController:
if self._lane_change()[0]:
curvature_rate = 0.0
if (self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.flags & FordFlags.CANFD and
command_predicted != predicted and command_predicted * curvature_rate > 0.0):
curvature_rate = 0.0
self.curvature_last = float(np.clip(applied, -0.02, 0.02))
min_curvature_rate = -0.001024
if self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.flags & FordFlags.CANFD:
starpilot/car/ford/tests/test_lateral.py
+81
View File
@@ -917,6 +917,87 @@ def test_curvature_strategy_uses_learned_lookahead(controller, monkeypatch):
assert lookaheads == [pytest.approx(0.38)]
@pytest.mark.parametrize("speed,expected", ((0.0, 0.0), (15.0, 0.0), (16.0, 0.0), (19.5, 0.2),
(23.0, 0.4), (30.0, 0.4), (40.0, 0.4)))
def test_mach_e_high_speed_preview_ramps_continuously(controller, speed, expected):
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
controller.CP.flags = FordFlags.CANFD
assert controller._high_speed_lookahead_extra(speed, False, False) == pytest.approx(expected)
@pytest.mark.parametrize("fingerprint,flags,driver,lane_change", (
(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, True, False),
(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, True),
(CAR.FORD_MUSTANG_MACH_E_MK1, 0, False, False),
(CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, False, False),
(CAR.FORD_F_150_MK14, FordFlags.CANFD, False, False),
(CAR.FORD_EDGE_MK2, 0, False, False),
))
def test_high_speed_preview_preserves_takeover_lane_changes_and_other_fords(controller, fingerprint, flags, driver, lane_change):
controller.CP.carFingerprint = fingerprint
controller.CP.flags = flags
assert controller._high_speed_lookahead_extra(30.0, driver, lane_change) == 0.0
@pytest.mark.parametrize("sign", (-1, 1))
@pytest.mark.parametrize("speed", (16.0, 19.5, 23.0, 30.0))
def test_mach_e_high_speed_preview_preserves_constant_curve_authority(controller, monkeypatch, sign, speed):
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
controller.CP.flags = FordFlags.CANFD
curvature = sign * 0.001
controller.curvature_last = curvature
controller.desired_curvature_last = curvature
monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: curvature)
result = controller.update(SimpleNamespace(latActive=True), car_state(speed=speed, curvature=curvature),
SimpleNamespace(curvature=curvature))
assert result.curvature == pytest.approx(curvature)
assert result.curvature_rate == 0.0
assert result.path_angle == 0.0
@pytest.mark.parametrize("driver,lane_change", ((False, False), (True, False), (False, True)))
def test_mach_e_high_speed_preview_update_uses_bounded_horizon(controller, monkeypatch, driver, lane_change):
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
controller.CP.flags = FordFlags.CANFD
controller.sm["liveDelay"].lateralDelay = 0.38
monkeypatch.setattr(controller, "_lane_change", lambda: (lane_change, 0))
lookaheads = []
monkeypatch.setattr(controller, "_predicted_curvature", lambda v, t: lookaheads.append(t) or 0.0)
controller.update(SimpleNamespace(latActive=True), car_state(speed=30.0, steering_pressed=driver),
SimpleNamespace(curvature=0.0001))
assert lookaheads[0] == pytest.approx(0.38 if driver or lane_change else 0.78)
assert controller._curvature_lookahead() == pytest.approx(0.38)
@pytest.mark.parametrize("sign", (-1, 1))
@pytest.mark.parametrize("conflicting_rate", (False, True))
@pytest.mark.parametrize("fingerprint,flags,driver,lane_change", (
(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, False),
(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, True, False),
(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, True),
(CAR.FORD_MUSTANG_MACH_E_MK1, 0, False, False),
(CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, False, False),
(CAR.FORD_EDGE_MK2, 0, False, False),
))
def test_mach_e_unwind_rate_does_not_fight_selected_release(controller, monkeypatch, sign, conflicting_rate,
fingerprint, flags, driver, lane_change):
controller.CP.carFingerprint = fingerprint
controller.CP.flags = flags
controller.desired_curvature_last = sign * 0.013
controller.curvature_last = sign * 0.012
predicted = sign * 0.011
rate = sign * 0.0002 * (1 if conflicting_rate else -1)
controller.curvature_samples.append(predicted - rate * STEER_DT * 8.0)
monkeypatch.setattr(controller, "_predicted_curvature", lambda v, t: predicted if t < 0.5 else sign * 0.006)
monkeypatch.setattr(controller, "_lane_change", lambda: (lane_change, 0))
result = controller.update(SimpleNamespace(latActive=True),
car_state(speed=8.0, curvature=sign * 0.012, steering_pressed=driver),
SimpleNamespace(curvature=sign * 0.012))
suppress = fingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and flags & FordFlags.CANFD and not driver and not lane_change
assert result.curvature_rate == pytest.approx(0.0 if lane_change or suppress and conflicting_rate else rate)
assert result.path_angle == 0.0
def test_mach_e_preview_does_not_override_opposite_current_path(controller):
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1