diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index eba8019117166b..181916aa0a670d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -83,6 +83,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): @@ -105,7 +106,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]) @@ -128,7 +129,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) @@ -200,6 +201,12 @@ def publish(self, sm, pm): action_t = self.CP.longitudinalActuatorDelay + DT_MDL a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels, action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + if self.mode == 'blended': + a_target_e2e, should_stop_e2e = get_accel_from_plan(list(sm['modelV2'].velocity.x)[:CONTROL_N], + list(sm['modelV2'].acceleration.x)[:CONTROL_N], + action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + 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 diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 777aa8aa7e4d35..ce864db9134219 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:72d3d6f8d3c98f5431ec86be77b6350d7d4f43c25075c0106f1d1e7ec7c77668 +oid sha256:d9615aa78a4cab296dfe3a994597caf909c814246dee1e3ac57653f7eb7acbe4 size 49096168