diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index e2d83033d6..69eeb4f77f 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -44,7 +44,7 @@ class VCruiseHelper: def update_v_cruise(self, CS, enabled, is_metric): self.v_cruise_kph_last = self.v_cruise_kph - if CS.cruiseState.available: + if CS.cruiseState.available and CS.cruiseState.speed > 1: if not self.CP.pcmCruise: # if stock cruise is completely disabled, then we can use our own set speed logic self._update_v_cruise_non_pcm(CS, enabled, is_metric) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 103d3f7047..d805d6e252 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -69,6 +69,7 @@ class LongitudinalPlanner: self.mpc = LongitudinalMpc(dt=dt) self.fcw = False self.dt = dt + self.enable_frames = 0 self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) @@ -105,8 +106,13 @@ class LongitudinalPlanner: long_control_off = sm['controlsState'].longControlState == LongCtrlState.off force_slow_decel = sm['controlsState'].forceDecel + if not long_control_off: + self.enable_frames += 1 + else: + self.enable_frames = 0 + # Reset current state when not engaged, or user is controlling the speed - reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled + reset_state = (long_control_off or self.enable_frames < 10) if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill)