From 92895f1e68f76f1092d2fcea8e672eef7309aaf2 Mon Sep 17 00:00:00 2001
From: Greg Hogan <gregjhogan@gmail.com>
Date: Fri, 1 Oct 2021 02:09:56 -0700
Subject: [PATCH] accel/decel button short/long press 1mph/5mph (#22013)

---
 selfdrive/controls/controlsd.py         | 16 ++++++++-
 selfdrive/controls/lib/drive_helpers.py | 47 ++++++++++++++++++++-----
 2 files changed, 54 insertions(+), 9 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index b6a02bc923..fa18bfd9e2 100755
--- a/selfdrive/controls/controlsd.py
+++ b/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()
diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py
index 9f554c88b4..e06c105f03 100644
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/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 == car.CarState.ButtonEvent.Type.accelCruise:
-        v_cruise_kph += V_CRUISE_DELTA - (v_cruise_kph % V_CRUISE_DELTA)
-      elif b.type == car.CarState.ButtonEvent.Type.decelCruise:
-        v_cruise_kph -= V_CRUISE_DELTA - ((V_CRUISE_DELTA - v_cruise_kph) % V_CRUISE_DELTA)
-      v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
+    if b.type.raw in button_timers and not b.pressed:
+      if button_timers[b.type.raw] > CRUISE_LONG_PRESS:
+        return v_cruise_kph # end long press
+      button_type = b.type.raw
+      break
+  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