From 415152ade909835a9f503c9c3dbd087acacc1e71 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 11 Nov 2022 23:11:49 -0800
Subject: [PATCH] controlsd: add tests around cruise speed (#26478)

* start to add some tests

* test !pcmCruise

* test !pcmCruise

* better test

* fix pylint

* new test for making sure we adjust on falling edge of buttons
old-commit-hash: a638afb98e2b9d7844c04b1ec55c79d3274bc9b1
---
 selfdrive/controls/tests/test_cruise_speed.py | 64 ++++++++++++++++++-
 1 file changed, 61 insertions(+), 3 deletions(-)
 mode change 100644 => 100755 selfdrive/controls/tests/test_cruise_speed.py

diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py
old mode 100644
new mode 100755
index ca070f1c3f..72b7dddc20
--- a/selfdrive/controls/tests/test_cruise_speed.py
+++ b/selfdrive/controls/tests/test_cruise_speed.py
@@ -1,10 +1,16 @@
 #!/usr/bin/env python3
-import unittest
 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 cereal import car
 from common.params import Params
+from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
+ButtonEvent = car.CarState.ButtonEvent
+ButtonType = car.CarState.ButtonEvent.Type
 
-from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
 def run_cruise_simulation(cruise, t_end=20.):
   man = Maneuver(
@@ -19,7 +25,7 @@ def run_cruise_simulation(cruise, t_end=20.):
   )
   valid, output = man.evaluate()
   assert valid
-  return output[-1,3]
+  return output[-1, 3]
 
 
 class TestCruiseSpeed(unittest.TestCase):
@@ -35,5 +41,57 @@ class TestCruiseSpeed(unittest.TestCase):
         self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {speed} m/s')
 
 
+# TODO: test pcmCruise
+@parameterized_class(('pcm_cruise',), [(False,)])
+class TestVCruiseHelper(unittest.TestCase):
+  def setUp(self):
+    self.CP = car.CarParams(pcmCruise=self.pcm_cruise)  # pylint: disable=E1101
+    self.v_cruise_helper = VCruiseHelper(self.CP)
+
+  def test_adjust_speed(self):
+    """
+    Asserts speed changes on falling edges of buttons.
+    """
+
+    self.v_cruise_helper.initialize_v_cruise(car.CarState())
+
+    for btn in (ButtonType.accelCruise, ButtonType.decelCruise):
+      initial_v_cruise = self.v_cruise_helper.v_cruise_kph
+      for pressed in (True, False):
+        CS = car.CarState(cruiseState={"available": True})
+        CS.buttonEvents = [ButtonEvent(type=btn, pressed=pressed)]
+
+        self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+        self.assertEqual(pressed, (initial_v_cruise == self.v_cruise_helper.v_cruise_kph))
+
+  def test_resume_in_standstill(self):
+    """
+    Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill.
+    """
+
+    self.v_cruise_helper.initialize_v_cruise(car.CarState())
+    initial_v_cruise = self.v_cruise_helper.v_cruise_kph
+
+    for standstill in (True, False):
+      for pressed in (True, False):
+        CS = car.CarState(cruiseState={"available": True, "standstill": standstill})
+        CS.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=pressed)]
+
+        self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+        # speed should only update if not at standstill and button falling edge
+        should_equal = standstill or pressed
+        self.assertEqual(should_equal, (initial_v_cruise == self.v_cruise_helper.v_cruise_kph))
+
+  def test_initialize_v_cruise(self):
+    """
+    Asserts allowed cruise speeds on enabling with SET
+    """
+
+    for v_ego in np.linspace(0, 100, 101):
+      self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=float(v_ego)))
+      self.assertTrue(V_CRUISE_ENABLE_MIN <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX)
+      self.assertTrue(self.v_cruise_helper.v_cruise_initialized)
+
+
 if __name__ == "__main__":
   unittest.main()