From 3cab2a57f78e9b9b835431c1e25ec3d2e7770186 Mon Sep 17 00:00:00 2001 From: Jason Wen <47793918+sunnyhaibin@users.noreply.github.com> Date: Thu, 16 Feb 2023 21:17:05 -0500 Subject: [PATCH] Longitudinal: Distance button to toggle experimental mode (#34) * Distance button on steering wheel to toggle experimental mode * HKG and Honda * Longitudinal: parse distance button from steering wheel * missing gm signal * GM, VW (MQB & PQ), Toyota * try this logic out --- selfdrive/car/gm/interface.py | 2 +- selfdrive/car/honda/interface.py | 2 +- selfdrive/car/hyundai/interface.py | 2 +- selfdrive/car/interfaces.py | 21 ++++++++++++++++++++- selfdrive/car/toyota/interface.py | 2 +- selfdrive/car/volkswagen/interface.py | 2 +- 6 files changed, 25 insertions(+), 6 deletions(-) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 68c9b2231106ae..98b73c78e7f48e 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -284,7 +284,7 @@ def _update(self, c): self.CS.accEnabled = False self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS) + ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.gap_dist_button)) # MADS BUTTON if self.CS.out.madsEnabled != self.CS.madsEnabled: diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index dcf50b43f37064..4f28de93da4ff4 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -345,7 +345,7 @@ def _update(self, c): self.CS.accEnabled = False self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS) + ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_setting == 3)) ret.buttonEvents = buttonEvents diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 5a1ab761bace80..849c7045ec065e 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -343,7 +343,7 @@ def _update(self, c): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) ret.cruiseState.enabled = False if self.CP.pcmCruise else self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS) + ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_buttons[-1] == 3)) # MADS BUTTON if self.CS.out.madsEnabled != self.CS.madsEnabled: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index d28460f5b8eedb..5a3f61818ddc29 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -96,6 +96,8 @@ def __init__(self, CP, CarController, CarState): self.below_speed_pause = self.param_s.get_bool("BelowSpeedPause") self.prev_acc_mads_combo = False self.mads_event_lock = True + self.gap_button_counter = 0 + self.experimental_mode_hold = False @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): @@ -364,7 +366,7 @@ def get_sp_cancel_cruise_state(self, mads_enabled, acc_enabled=False): def get_sp_pedal_disengage(self, brake_pressed, standstill): return brake_pressed and (not self.CS.out.brakePressed or not standstill) - def get_sp_common_state(self, cs_out, CS, gear_allowed=True): + def get_sp_common_state(self, cs_out, CS, gear_allowed=True, gap_button=False): if self.CP.pcmCruise: if not cs_out.cruiseState.enabled and CS.out.cruiseState.enabled: CS.madsEnabled, cs_out.cruiseState.enabled = self.get_sp_cancel_cruise_state(CS.madsEnabled) @@ -376,6 +378,8 @@ def get_sp_common_state(self, cs_out, CS, gear_allowed=True): elif not cs_out.cruiseState.enabled and CS.out.cruiseState.enabled: CS.madsEnabled = False + self.toggle_exp_mode(gap_button) + cs_out.belowLaneChangeSpeed = cs_out.vEgo < LANE_CHANGE_SPEED_MIN and self.below_speed_pause if cs_out.gearShifter in [GearShifter.park, GearShifter.reverse] or cs_out.doorOpen or \ @@ -403,6 +407,21 @@ def get_sp_common_state(self, cs_out, CS, gear_allowed=True): return cs_out, CS + def toggle_exp_mode(self, gap_pressed): + if not self.CP.openpilotLongitudinalControl: + return None + experimental_mode = self.param_s.get_bool("ExperimentalMode") + if gap_pressed: + if not self.experimental_mode_hold: + self.gap_button_counter += 1 + if self.gap_button_counter > 50: + self.gap_button_counter = 0 + self.experimental_mode_hold = True + self.param_s.put_bool("ExperimentalMode", not experimental_mode) + else: + self.gap_button_counter = 0 + self.experimental_mode_hold = False + def create_sp_events(self, CS, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False, enable_from_brake=False, enable_pressed_long=False, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)): diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index db81304cb0553f..4f616dd07a89ac 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -270,7 +270,7 @@ def _update(self, c): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) ret.cruiseState.enabled = False if self.CP.pcmCruise else self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS) + ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.gap_dist_button)) # CANCEL if self.CS.out.cruiseState.enabled and not ret.cruiseState.enabled: diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 18e7b62751d6f9..9d19e2f74e225c 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -240,7 +240,7 @@ def _update(self, c): self.CS.accEnabled = False self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS) + ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.gap_dist_button == 3)) # MADS BUTTON if self.CS.out.madsEnabled != self.CS.madsEnabled: