Skip to content

Commit

Permalink
Longitudinal: Distance button to toggle experimental mode (commaai#34)
Browse files Browse the repository at this point in the history
* 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
  • Loading branch information
sunnyhaibin authored Feb 17, 2023
1 parent c0893c4 commit 3cab2a5
Show file tree
Hide file tree
Showing 6 changed files with 25 additions and 6 deletions.
2 changes: 1 addition & 1 deletion selfdrive/car/gm/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/honda/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/hyundai/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
21 changes: 20 additions & 1 deletion selfdrive/car/interfaces.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -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)
Expand All @@ -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 \
Expand Down Expand Up @@ -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)):
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/toyota/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/car/volkswagen/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down

0 comments on commit 3cab2a5

Please sign in to comment.