diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 63395adf64..2c359ec32a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -183,6 +183,7 @@ class Controls: self.steer_limited = False self.desired_curvature = 0.0 self.desired_curvature_rate = 0.0 + self.experimental_mode = False self.v_cruise_helper = VCruiseHelper(self.CP) # TODO: no longer necessary, aside from process replay @@ -544,7 +545,7 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.v_cruise_helper.initialize_v_cruise(CS) + self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -787,7 +788,7 @@ class Controls: controlsState.startMonoTime = int(start_time * 1e9) controlsState.forceDecel = bool(force_decel) controlsState.canErrorCounter = self.can_rcv_cum_timeout_counter - controlsState.experimentalMode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl + controlsState.experimentalMode = self.experimental_mode lat_tuning = self.CP.lateralTuning.which() if self.joystick_mode: @@ -838,6 +839,7 @@ class Controls: self.prof.checkpoint("Ratekeeper", ignore=True) self.is_metric = self.params.get_bool("IsMetric") + self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl # Sample data from sockets and get a carState CS = self.data_sample() diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 3d5ec8ac1d..a332d06765 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -8,10 +8,12 @@ from selfdrive.modeld.constants import T_IDXS # WARNING: this value was determined based on the model's training distribution, # model predictions above this speed can be unpredictable -V_CRUISE_MAX = 145 # kph -V_CRUISE_MIN = 8 # kph -V_CRUISE_ENABLE_MIN = 40 # kph -V_CRUISE_INITIAL = 255 # kph +# V_CRUISE's are in kph +V_CRUISE_MIN = 8 +V_CRUISE_MAX = 145 +V_CRUISE_UNSET = 255 +V_CRUISE_INITIAL = 40 +V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105 IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors MIN_SPEED = 1.0 @@ -39,15 +41,15 @@ CRUISE_INTERVAL_SIGN = { class VCruiseHelper: def __init__(self, CP): self.CP = CP - self.v_cruise_kph = V_CRUISE_INITIAL - self.v_cruise_cluster_kph = V_CRUISE_INITIAL + self.v_cruise_kph = V_CRUISE_UNSET + self.v_cruise_cluster_kph = V_CRUISE_UNSET self.v_cruise_kph_last = 0 self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} @property def v_cruise_initialized(self): - return self.v_cruise_kph != V_CRUISE_INITIAL + return self.v_cruise_kph != V_CRUISE_UNSET def update_v_cruise(self, CS, enabled, is_metric): self.v_cruise_kph_last = self.v_cruise_kph @@ -62,8 +64,8 @@ class VCruiseHelper: self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH else: - self.v_cruise_kph = V_CRUISE_INITIAL - self.v_cruise_cluster_kph = V_CRUISE_INITIAL + self.v_cruise_kph = V_CRUISE_UNSET + self.v_cruise_cluster_kph = V_CRUISE_UNSET def _update_v_cruise_non_pcm(self, CS, enabled, is_metric): # handle button presses. TODO: this should be in state_control, but a decelCruise press @@ -125,16 +127,18 @@ class VCruiseHelper: self.button_timers[b.type.raw] = 1 if b.pressed else 0 self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - def initialize_v_cruise(self, CS): + def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: # initializing is handled by the PCM if self.CP.pcmCruise: return + initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL + # 250kph or above probably means we never had a set speed if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250: self.v_cruise_kph = self.v_cruise_kph_last else: - self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX))) + self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) self.v_cruise_cluster_kph = self.v_cruise_kph diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py index cd1d31cf07..b83116af45 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_MIN, V_CRUISE_MAX, V_CRUISE_ENABLE_MIN, IMPERIAL_INCREMENT +from selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT from cereal import car from common.conversions import Conversions as CV from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -53,16 +53,16 @@ class TestVCruiseHelper(unittest.TestCase): for _ in range(2): self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False) - def enable(self, v_ego): + def enable(self, v_ego, experimental_mode): # Simulates user pressing set with a current speed - self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego)) + self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode) def test_adjust_speed(self): """ Asserts speed changes on falling edges of buttons. """ - self.enable(V_CRUISE_ENABLE_MIN * CV.KPH_TO_MS) + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) for btn in (ButtonType.accelCruise, ButtonType.decelCruise): for pressed in (True, False): @@ -86,7 +86,7 @@ class TestVCruiseHelper(unittest.TestCase): CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)] self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False) if pressed: - self.enable(V_CRUISE_ENABLE_MIN * CV.KPH_TO_MS) + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) # Expected diff on enabling. Speed should not change on falling edge of pressed self.assertEqual(not pressed, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) @@ -96,7 +96,7 @@ class TestVCruiseHelper(unittest.TestCase): Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill. """ - self.enable(0) + self.enable(0, False) for standstill in (True, False): for pressed in (True, False): @@ -116,7 +116,7 @@ class TestVCruiseHelper(unittest.TestCase): for v_ego in np.linspace(0, 100, 101): self.reset_cruise_speed_state() - self.enable(V_CRUISE_ENABLE_MIN * CV.KPH_TO_MS) + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) # first decrement speed, then perform gas pressed logic expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT @@ -137,13 +137,14 @@ class TestVCruiseHelper(unittest.TestCase): Asserts allowed cruise speeds on enabling with SET. """ - for v_ego in np.linspace(0, 100, 101): - self.reset_cruise_speed_state() - self.assertFalse(self.v_cruise_helper.v_cruise_initialized) + for experimental_mode in (True, False): + for v_ego in np.linspace(0, 100, 101): + self.reset_cruise_speed_state() + self.assertFalse(self.v_cruise_helper.v_cruise_initialized) - self.enable(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) + self.enable(float(v_ego), experimental_mode) + self.assertTrue(V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX) + self.assertTrue(self.v_cruise_helper.v_cruise_initialized) if __name__ == "__main__":