accel/decel button short/long press 1mph/5mph (#22013)

old-commit-hash: 92895f1e68

selfdrive/controls/controlsd.py

@@ -48,6 +48,7 @@ Desire = log.LateralPlan.Desire
 LaneChangeState = log.LateralPlan.LaneChangeState
 LaneChangeDirection = log.LateralPlan.LaneChangeDirection
 EventName = car.CarEvent.EventName
+ButtonEvent = car.CarState.ButtonEvent
 
 
 class Controls:
@@ -149,6 +150,7 @@ class Controls:
     self.events_prev = []
     self.current_alert_types = [ET.PERMANENT]
     self.logged_comm_issue = False
+    self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0}
 
     # TODO: no longer necessary, aside from process replay
     self.sm['liveParameters'].valid = True
@@ -367,7 +369,7 @@ class Controls:
 
     # if stock cruise is completely disabled, then we can use our own set speed logic
     if not self.CP.pcmCruise:
-      self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.enabled)
+      self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.button_timers, self.enabled)
     elif self.CP.pcmCruise and CS.cruiseState.enabled:
       self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
 
@@ -520,6 +522,16 @@ class Controls:
 
     return actuators, lac_log
 
+  def update_button_timers(self, buttonEvents):
+    # increment timer for buttons still pressed
+    for k in self.button_timers.keys():
+      if self.button_timers[k] > 0:
+        self.button_timers[k] += 1
+
+    for b in buttonEvents:
+      if b.type.raw in self.button_timers:
+        self.button_timers[b.type.raw] = 1 if b.pressed else 0
+
   def publish_logs(self, CS, start_time, actuators, lac_log):
     """Send actuators and hud commands to the car, send controlsstate and MPC logging"""
 
@@ -673,6 +685,8 @@ class Controls:
     self.publish_logs(CS, start_time, actuators, lac_log)
     self.prof.checkpoint("Sent")
 
+    self.update_button_timers(CS.buttonEvents)
+
   def controlsd_thread(self):
     while True:
       self.step()

selfdrive/controls/lib/drive_helpers.py

@@ -1,3 +1,4 @@
+import math
 from cereal import car
 from common.numpy_fast import clip, interp
 from common.realtime import DT_MDL
@@ -8,7 +9,7 @@ from selfdrive.modeld.constants import T_IDXS
 # kph
 V_CRUISE_MAX = 135
 V_CRUISE_MIN = 8
-V_CRUISE_DELTA = 8
+V_CRUISE_DELTA = 1.6
 V_CRUISE_ENABLE_MIN = 40
 LAT_MPC_N = 16
 LON_MPC_N = 32
@@ -19,6 +20,17 @@ CAR_ROTATION_RADIUS = 0.0
 MAX_CURVATURE_RATES = [0.03762194918267951, 0.003441203371932992]
 MAX_CURVATURE_RATE_SPEEDS = [0, 35]
 
+CRUISE_LONG_PRESS = 50
+CRUISE_NEAREST_FUNC = {
+  car.CarState.ButtonEvent.Type.accelCruise: math.ceil,
+  car.CarState.ButtonEvent.Type.decelCruise: math.floor,
+}
+CRUISE_INTERVAL_SIGN = {
+  car.CarState.ButtonEvent.Type.accelCruise: +1,
+  car.CarState.ButtonEvent.Type.decelCruise: -1,
+}
+
+
 class MPC_COST_LAT:
   PATH = 1.0
   HEADING = 1.0
@@ -40,16 +52,35 @@ def get_steer_max(CP, v_ego):
   return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
 
 
-def update_v_cruise(v_cruise_kph, buttonEvents, enabled):
+def update_v_cruise(v_cruise_kph, buttonEvents, button_timers, enabled):
   # handle button presses. TODO: this should be in state_control, but a decelCruise press
   # would have the effect of both enabling and changing speed is checked after the state transition
+  if not enabled:
+    return v_cruise_kph
+
+  long_press = False
+  button_type = None
+
   for b in buttonEvents:
-    if enabled and not b.pressed:
+    if b.type.raw in button_timers and not b.pressed:
-      if b.type == car.CarState.ButtonEvent.Type.accelCruise:
+      if button_timers[b.type.raw] > CRUISE_LONG_PRESS:
-        v_cruise_kph += V_CRUISE_DELTA - (v_cruise_kph % V_CRUISE_DELTA)
+        return v_cruise_kph # end long press
-      elif b.type == car.CarState.ButtonEvent.Type.decelCruise:
+      button_type = b.type.raw
-        v_cruise_kph -= V_CRUISE_DELTA - ((V_CRUISE_DELTA - v_cruise_kph) % V_CRUISE_DELTA)
+      break
-      v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
+  else:
+    for k in button_timers.keys():
+      if button_timers[k] and button_timers[k] % CRUISE_LONG_PRESS == 0:
+        button_type = k
+        long_press = True
+        break
+
+  if button_type:
+    v_cruise_delta = V_CRUISE_DELTA * (5 if long_press else 1)
+    if long_press and v_cruise_kph % v_cruise_delta != 0: # partial interval
+      v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](v_cruise_kph / v_cruise_delta) * v_cruise_delta
+    else:
+      v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
+    v_cruise_kph = clip(round(v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
 
   return v_cruise_kph