mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-24 01:12:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'exp-hold-fix' into master-dev-c3

Browse Source
This commit is contained in:
Jason Wen
2024-08-12 18:48:40 -04:00
parent cba4c552f3 853fed447d
commit b1cd16f1e0
12 changed files with 39 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/chrysler/interface.py
+1 -1
View File
@@ -135,7 +135,7 @@ class CarInterface(CarInterfaceBase):
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret = self.get_sp_common_state(ret, gap_button=bool(self.CS.distance_button))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
selfdrive/car/ford/interface.py
+1 -1
View File
@@ -115,7 +115,7 @@ class CarInterface(CarInterfaceBase):
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret = self.get_sp_common_state(ret, gap_button=bool(self.CS.distance_button))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
selfdrive/car/gm/interface.py
+1 -4
View File
@@ -205,8 +205,6 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
distance_button = 0
# Don't add event if transitioning from INIT, unless it's to an actual button
if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT:
self.CS.button_events = [
@@ -215,7 +213,6 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button,
{1: ButtonType.gapAdjustCruise})
]
distance_button = self.CS.distance_button
self.CS.button_events = [
*self.CS.button_events,
@@ -252,7 +249,7 @@ class CarInterface(CarInterfaceBase):
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret = self.get_sp_common_state(ret, gap_button=bool(distance_button))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
selfdrive/car/honda/interface.py
+1 -1
View File
@@ -300,7 +300,7 @@ class CarInterface(CarInterfaceBase):
elif not ret.cruiseState.enabled:
self.CS.accEnabled = False
ret = self.get_sp_common_state(ret, gap_button=(self.CS.cruise_setting == 3))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
selfdrive/car/hyundai/interface.py
+1 -1
View File
@@ -266,7 +266,7 @@ class CarInterface(CarInterfaceBase):
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
ret = self.get_sp_common_state(ret, gap_button=(self.CS.cruise_buttons[-1] == 3))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
selfdrive/car/interfaces.py
+1 -17
View File
@@ -616,7 +616,7 @@ class CarInterfaceBase(ABC):
else:
return self.CS.madsEnabled
def get_sp_common_state(self, cs_out, gear_allowed=True, gap_button=False):
def get_sp_common_state(self, cs_out, gear_allowed=True):
cs_out.cruiseState.enabled = self.CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed else cs_out.cruiseState.enabled
if not self.enable_mads:
@@ -625,9 +625,6 @@ class CarInterfaceBase(ABC):
elif not cs_out.cruiseState.enabled and self.CS.out.cruiseState.enabled:
self.CS.madsEnabled = False
if self.CP.openpilotLongitudinalControl:
self.toggle_exp_mode(gap_button) # TODO-SP: use buttonEvents to handle this, then remove gap_button
lane_change_speed_min = get_min_lateral_speed(self.CS.params_list.pause_lateral_speed, self.CS.params_list.is_metric)
cs_out.belowLaneChangeSpeed = cs_out.vEgo < lane_change_speed_min and self.CS.params_list.below_speed_pause
@@ -655,19 +652,6 @@ class CarInterfaceBase(ABC):
return cs_out
# TODO: SP: use upstream's buttonEvents counter checks from controlsd
def toggle_exp_mode(self, gap_pressed):
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_nonblocking("ExperimentalMode", not self.CS.params_list.experimental_mode)
else:
self.gap_button_counter = 0
self.experimental_mode_hold = False
def create_sp_events(self, 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)):
selfdrive/car/mazda/interface.py
+1 -1
View File
@@ -71,7 +71,7 @@ class CarInterface(CarInterfaceBase):
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret = self.get_sp_common_state(ret, gap_button=bool(self.CS.distance_button))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
selfdrive/car/nissan/interface.py
+1 -1
View File
@@ -50,7 +50,7 @@ class CarInterface(CarInterfaceBase):
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
ret = self.get_sp_common_state(ret, gap_button=bool(self.CS.distance_button))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
selfdrive/car/toyota/interface.py
+1 -1
View File
@@ -238,7 +238,7 @@ class CarInterface(CarInterfaceBase):
if not self.CP.pcmCruise:
ret.cruiseState.enabled = self.CS.accEnabled
ret = self.get_sp_common_state(ret, gap_button=bool(distance_button))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
selfdrive/car/volkswagen/interface.py
+1 -1
View File
@@ -140,7 +140,7 @@ class CarInterface(CarInterfaceBase):
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret = self.get_sp_common_state(ret, gap_button=any(b.type == ButtonType.gapAdjustCruise and b.pressed for b in self.CS.button_events))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
selfdrive/controls/controlsd.py
+10 -2
View File
@@ -20,7 +20,7 @@ from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.car.car_helpers import get_car_interface, get_startup_event
from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_lag_adjusted_curvature
from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_lag_adjusted_curvature, CRUISE_LONG_PRESS
from openpilot.selfdrive.controls.lib.events import Events, ET
from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
@@ -731,11 +731,19 @@ class Controls:
cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
setattr(actuators, p, 0.0)
# toggle experimental mode once on distance button hold
if self.CP.openpilotLongitudinalControl:
if self.v_cruise_helper.experimental_mode_update and self.v_cruise_helper.button_timers[ButtonType.gapAdjustCruise] == CRUISE_LONG_PRESS:
self.experimental_mode = not self.experimental_mode
self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode)
# decrement personality on distance button press
if self.CP.openpilotLongitudinalControl:
if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents) and \
not self.v_cruise_helper.experimental_mode_update:
self.personality = (self.personality - 1) % 3
self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
self.v_cruise_helper.experimental_mode_update = False
return CC, lac_log
selfdrive/controls/lib/drive_helpers.py
+19 -3
View File
@@ -79,7 +79,7 @@ class VCruiseHelper:
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_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.gapAdjustCruise: 0}
self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
self.is_metric_prev = None
@@ -93,6 +93,8 @@ class VCruiseHelper:
self.sp_override_cruise_speed_last = V_CRUISE_UNSET
self.sp_override_enabled_last = False
self.experimental_mode_update = False
@property
def v_cruise_initialized(self):
return self.v_cruise_kph != V_CRUISE_UNSET
@@ -105,12 +107,14 @@ class VCruiseHelper:
self._update_v_cruise_min(is_metric)
if CS.cruiseState.available:
if self.CP.openpilotLongitudinalControl:
self._update_experimental_mode(CS)
if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed:
# 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, reverse_acc)
self._update_v_cruise_slc(long_plan_sp)
self.v_cruise_cluster_kph = self.v_cruise_kph
self.update_button_timers(CS, enabled)
else:
if enabled and sp_override_speed and CS.cruiseState.speed * CV.MS_TO_KPH < sp_override_speed:
if self.sp_override_v_cruise_kph == V_CRUISE_UNSET:
@@ -132,10 +136,12 @@ class VCruiseHelper:
self.sp_override_cruise_speed_last = CS.cruiseState.speed
self.sp_override_enabled_last = enabled
self.update_button_timers(CS, enabled)
else:
self.sp_override_v_cruise_kph = V_CRUISE_UNSET
self.v_cruise_kph = V_CRUISE_UNSET
self.v_cruise_cluster_kph = V_CRUISE_UNSET
self.experimental_mode_update = False
def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, reverse_acc):
# handle button presses. TODO: this should be in state_control, but a decelCruise press
@@ -165,7 +171,7 @@ class VCruiseHelper:
long_press = True
break
if button_type is None:
if button_type is None or button_type == ButtonType.gapAdjustCruise:
return
resume_button = ButtonType.accelCruise
@@ -268,6 +274,16 @@ class VCruiseHelper:
self.v_cruise_min = GM_V_CRUISE_MIN[is_metric]
self.is_metric_prev = is_metric
def _update_experimental_mode(self, CS):
for b in CS.buttonEvents:
if b.type == ButtonType.gapAdjustCruise and not b.pressed:
if self.button_timers[ButtonType.gapAdjustCruise] > CRUISE_LONG_PRESS:
return # end long press
break
else:
if self.button_timers[ButtonType.gapAdjustCruise] and self.button_timers[ButtonType.gapAdjustCruise] == CRUISE_LONG_PRESS:
self.experimental_mode_update = True
def clip_curvature(v_ego, prev_curvature, new_curvature):
v_ego = max(MIN_SPEED, v_ego)