diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index bdbdb7023a..f0dc2e9467 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/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 1.6 + v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT for b in CS.buttonEvents: if b.type.raw in self.button_timers and not b.pressed: diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py index 3d6f55931e..a635198ceb 100755 --- a/selfdrive/controls/tests/test_cruise_speed.py +++ b/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.