diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript index 164b965142..5c316ed8e2 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript @@ -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 + \ diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so index f50a948719..036bc6a785 100755 Binary files a/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so and b/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so differ diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 216dac2563..332093a4b2 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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, diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index e5a3c41903..d397ccfa8a 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -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) diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 5215e1abf4..7a01112457 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -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: diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index 13f54812d3..35f08e8d27 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -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