diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index d3d36fac20..31738cb475 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -248,6 +248,7 @@ def gen_long_ocp(): class LongitudinalMpc: def __init__(self, mode='acc', dt=DT_MDL): + self.braking_offset = 1 self.mode = mode self.dt = dt self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) @@ -303,6 +304,7 @@ class LongitudinalMpc: def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard): jerk_factor = get_jerk_factor(personality) + jerk_factor /= np.mean(self.braking_offset) if self.mode == 'acc': 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] @@ -369,6 +371,21 @@ class LongitudinalMpc: lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_1 = self.process_lead(radarstate.leadTwo) + lead = radarstate.leadOne + + self.smoother_braking = True if ( + self.mode == 'acc' and + np.any(v_ego < 16) and + np.any(lead_xv_0[:,0] < 40) and + not np.any(lead.dRel < (v_ego - 1) * t_follow) + ) else False + + if self.smoother_braking: + distance_factor = np.maximum(1, lead_xv_0[:,0] - (lead_xv_0[:,1] * t_follow)) + self.braking_offset = np.clip((v_ego - lead_xv_0[:,1]) - COMFORT_BRAKE, 1, distance_factor) + t_follow = t_follow / self.braking_offset + else: + self.braking_offset = 1 # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance