From a0539910bfa42829209560e0bc19390d2bcf3cd2 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 13 Nov 2022 16:34:26 -0800 Subject: [PATCH] controlsd: no speed increment if enabled on button rising edge (#26490) * don't increment speed if we enabled on rising edge * more realistic test old-commit-hash: e46063086f079eff6292a32ae9aeeafa3aa523a3 --- selfdrive/controls/lib/drive_helpers.py | 10 ++++++--- selfdrive/controls/tests/test_cruise_speed.py | 21 ++++++++++++++++++- 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 37dacf5fb2..09550de7bb 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -56,7 +56,7 @@ class VCruiseHelper: # if stock cruise is completely disabled, then we can use our own set speed logic self._update_v_cruise_non_pcm(CS, enabled, is_metric) self.v_cruise_cluster_kph = self.v_cruise_kph - self.update_button_timers(CS) + self.update_button_timers(CS, enabled) else: self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH @@ -97,6 +97,10 @@ class VCruiseHelper: if button_type == ButtonType.accelCruise and cruise_standstill: return + # Don't adjust speed if we've enabled since the button was depressed (some ports enable on rising edge) + if not self.button_change_states[button_type]["enabled"]: + return + v_cruise_delta = v_cruise_delta * (5 if long_press else 1) if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta @@ -109,7 +113,7 @@ class VCruiseHelper: self.v_cruise_kph = clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX) - def update_button_timers(self, CS): + def update_button_timers(self, CS, enabled): # increment timer for buttons still pressed for k in self.button_timers: if self.button_timers[k] > 0: @@ -119,7 +123,7 @@ class VCruiseHelper: if b.type.raw in self.button_timers: # Start/end timer and store current state on change of button pressed self.button_timers[b.type.raw] = 1 if b.pressed else 0 - self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill} + self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} def initialize_v_cruise(self, CS): # initializing is handled by the PCM diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py index 396ff2b46f..3d6f55931e 100755 --- a/selfdrive/controls/tests/test_cruise_speed.py +++ b/selfdrive/controls/tests/test_cruise_speed.py @@ -74,6 +74,25 @@ class TestVCruiseHelper(unittest.TestCase): self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) self.assertEqual(pressed, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) + def test_rising_edge_enable(self): + """ + Some car interfaces may enable on rising edge of a button, + ensure we don't adjust speed if enabled changes mid-press. + """ + + # NOTE: enabled is always one frame behind the result from button press in controlsd + for enabled, pressed in ((False, False), + (False, True), + (True, False)): + CS = car.CarState(cruiseState={"available": True}) + 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) + + # 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) + def test_resume_in_standstill(self): """ Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill. @@ -93,7 +112,7 @@ class TestVCruiseHelper(unittest.TestCase): def test_initialize_v_cruise(self): """ - Asserts allowed cruise speeds on enabling with SET + Asserts allowed cruise speeds on enabling with SET. """ for v_ego in np.linspace(0, 100, 101):