controlsd: add test around cruise speed gas pressed behavior (#26486)

* test

* test

* debug

* test

* test

* test

* clean up

* clean up

* add test

* stash

* clean up

* clean up

* clean up

* assert equal

selfdrive/controls/lib/drive_helpers.py

@@ -12,6 +12,7 @@ V_CRUISE_MAX = 145  # kph
 V_CRUISE_MIN = 8  # kph
 V_CRUISE_ENABLE_MIN = 40  # kph
 V_CRUISE_INITIAL = 255  # kph
+IMPERIAL_INCREMENT = 1.6  # should be CV.MPH_TO_KPH, but this causes rounding errors
 
 MIN_SPEED = 1.0
 LAT_MPC_N = 16
@@ -73,8 +74,7 @@ class VCruiseHelper:
     long_press = False
     button_type = None
 
-    # should be CV.MPH_TO_KPH, but this causes rounding errors
+    v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT
-    v_cruise_delta = 1. if is_metric else 1.6
 
     for b in CS.buttonEvents:
       if b.type.raw in self.button_timers and not b.pressed:

selfdrive/controls/tests/test_cruise_speed.py

@@ -3,7 +3,7 @@ import numpy as np
 from parameterized import parameterized_class
 import unittest
 
-from selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MAX, V_CRUISE_ENABLE_MIN
+from selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_ENABLE_MIN, IMPERIAL_INCREMENT
 from cereal import car
 from common.conversions import Conversions as CV
 from common.params import Params
@@ -110,6 +110,30 @@ class TestVCruiseHelper(unittest.TestCase):
         should_equal = standstill or pressed
         self.assertEqual(should_equal, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
 
+  def test_set_gas_pressed(self):
+    """
+    Asserts pressing set while enabled with gas pressed sets
+    the speed to the maximum of vEgo and current cruise speed.
+    """
+
+    for v_ego in np.linspace(0, 100, 101):
+      self.reset_cruise_speed_state()
+      self.enable(V_CRUISE_ENABLE_MIN * CV.KPH_TO_MS)
+
+      # first decrement speed, then perform gas pressed logic
+      expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT
+      expected_v_cruise_kph = max(expected_v_cruise_kph, v_ego * CV.MS_TO_KPH)  # clip to min of vEgo
+      expected_v_cruise_kph = float(np.clip(round(expected_v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX))
+
+      CS = car.CarState(vEgo=float(v_ego), gasPressed=True, cruiseState={"available": True})
+      CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)]
+      self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+      # TODO: fix skipping first run due to enabled on rising edge exception
+      if v_ego == 0.0:
+        continue
+      self.assertEqual(expected_v_cruise_kph, self.v_cruise_helper.v_cruise_kph)
+
   def test_initialize_v_cruise(self):
     """
     Asserts allowed cruise speeds on enabling with SET.