diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index a2547f99b..f413903aa 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -63,26 +63,6 @@ class LongitudinalPlanner: self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) - @staticmethod - def parse_model(model_msg): - if (len(model_msg.position.x) == ModelConstants.IDX_N and - len(model_msg.velocity.x) == ModelConstants.IDX_N and - len(model_msg.acceleration.x) == ModelConstants.IDX_N): - x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) - j = np.zeros(len(T_IDXS_MPC)) - else: - x = np.zeros(len(T_IDXS_MPC)) - v = np.zeros(len(T_IDXS_MPC)) - a = np.zeros(len(T_IDXS_MPC)) - j = np.zeros(len(T_IDXS_MPC)) - if len(model_msg.meta.disengagePredictions.gasPressProbs) > 1: - throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1] - else: - throttle_prob = 1.0 - return x, v, a, j, throttle_prob - def update(self, sm): if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) @@ -116,7 +96,8 @@ class LongitudinalPlanner: # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) - _, _, _, _, throttle_prob = self.parse_model(sm['modelV2']) + throttle_probs = sm['modelV2'].meta.disengagePredictions.gasPressProbs + throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0 # Don't clip at low speeds since throttle_prob doesn't account for creep self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED