diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 2aab4b71af..49eb5988e2 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -253,7 +253,8 @@ class LongitudinalMpc: cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] elif self.mode == 'blended': - cost_weights = [0., 0.1, 0.2, 5.0, 0.0, 1.0] + a_change_cost = 40.0 if prev_accel_constraint else 0 + cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0] else: raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set')