give 10 frames of tolerance to reset -- WARNING: hacks inside

old-commit-hash: d72ac9e53d211074b059f049f8ebc89b9cff1c58
pull/33449/head
Shane Smiskol 11 months ago
parent fa892a9755
commit a1754bd4fd
  1. 2
      selfdrive/car/cruise.py
  2. 8
      selfdrive/controls/lib/longitudinal_planner.py

@ -44,7 +44,7 @@ class VCruiseHelper:
def update_v_cruise(self, CS, enabled, is_metric): def update_v_cruise(self, CS, enabled, is_metric):
self.v_cruise_kph_last = self.v_cruise_kph 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 not self.CP.pcmCruise:
# if stock cruise is completely disabled, then we can use our own set speed logic # 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) self._update_v_cruise_non_pcm(CS, enabled, is_metric)

@ -69,6 +69,7 @@ class LongitudinalPlanner:
self.mpc = LongitudinalMpc(dt=dt) self.mpc = LongitudinalMpc(dt=dt)
self.fcw = False self.fcw = False
self.dt = dt self.dt = dt
self.enable_frames = 0
self.a_desired = init_a self.a_desired = init_a
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) 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 long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
force_slow_decel = sm['controlsState'].forceDecel 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 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 # No change cost when user is controlling the speed, or when standstill
prev_accel_constraint = not (reset_state or sm['carState'].standstill) prev_accel_constraint = not (reset_state or sm['carState'].standstill)

Loading…
Cancel
Save