2d7e10e9-d22c-447c-9d2b-664daaca669b/400

selfdrive/controls/lib/longitudinal_planner.py

@@ -87,6 +87,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
     self.a_desired_trajectory = np.zeros(CONTROL_N)
     self.j_desired_trajectory = np.zeros(CONTROL_N)
     self.solverExecutionTime = 0.0
+    self.mode = 'acc'
 
   @staticmethod
   def parse_model(model_msg, model_error):
@@ -109,7 +110,7 @@ def parse_model(model_msg, model_error):
     return x, v, a, j, throttle_prob
 
   def update(self, sm):
-    self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
+    self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
 
     if len(sm['carControl'].orientationNED) == 3:
       accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -132,7 +133,7 @@ def update(self, sm):
     # No change cost when user is controlling the speed, or when standstill
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
-    if self.mpc.mode == 'acc':
+    if self.mode == 'acc':
       accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
       steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
       accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP)
@@ -202,6 +203,10 @@ def publish(self, sm, pm):
     longitudinalPlan.fcw = self.fcw
 
     a_target, should_stop = get_accel_from_plan(self.CP, longitudinalPlan.speeds, longitudinalPlan.accels)
+    if self.mode == 'blended':
+      a_target_e2e, should_stop_e2e = get_accel_from_plan(self.CP, list(sm['modelV2'].velocity.x)[:CONTROL_N], list(sm['modelV2'].acceleration.x)[:CONTROL_N])
+      a_target = min(a_target, a_target_e2e)
+      should_stop = should_stop or should_stop_e2e
     longitudinalPlan.aTarget = a_target
     longitudinalPlan.shouldStop = should_stop
     longitudinalPlan.allowBrake = True