From f5be78fce3a54468e27d2bb7aaa06e283fd0a09f Mon Sep 17 00:00:00 2001 From: SloPOS <144983089+SloPOS@users.noreply.github.com> Date: Wed, 8 Apr 2026 15:51:53 +0000 Subject: [PATCH] Fix longitudinal support: add precision weights to MPC based on plan source --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 4 +++- selfdrive/controls/lib/longitudinal_planner.py | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index efdef9dd71bb71..76e4a29901a8df 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -267,8 +267,10 @@ def set_cost_weights(self, cost_weights, constraint_cost_weights): for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): + def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, precision=False): jerk_factor = get_jerk_factor(personality) + if precision: + jerk_factor *= 0.5 a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index e02b02d2e0c499..80771e4f691c60 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -136,7 +136,8 @@ def update(self, sm): if force_slow_decel: v_cruise = 0.0 - self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) + precision = self.source != LongitudinalPlanSource.cruise + self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality, precision=precision) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)