diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 104c7c9d49..4219e80f33 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -52,6 +52,7 @@ class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) + self.mpc.mode = 'acc' self.fcw = False self.dt = dt self.allow_throttle = True @@ -89,7 +90,6 @@ class LongitudinalPlanner: return x, v, a, j, throttle_prob def update(self, sm): - self.mpc.mode = 'acc' self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' if len(sm['carControl'].orientationNED) == 3: @@ -97,7 +97,7 @@ class LongitudinalPlanner: else: accel_coast = ACCEL_MAX - v_ego = sm['modelV2'].velocity.x[0] + v_ego = sm['carState'].vEgo v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET @@ -129,7 +129,7 @@ class LongitudinalPlanner: self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) # Compute model v_ego error self.v_model_error = get_speed_error(sm['modelV2'], v_ego) - x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], 0) + x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error) # 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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 5f89d7802d..4b3ef33407 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -0800e73b5d9c801f161b9559557c0559b0682fec \ No newline at end of file +fcc771b9ceb487a61035885acbd84e592fb316bf \ No newline at end of file