diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 66aa1b71f..c4b546402 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -25,6 +25,26 @@ struct ModularAssistiveDrivingSystem { } } +struct IntelligentCruiseButtonManagement { + state @0 :IntelligentCruiseButtonManagementState; + sendButton @1 :SendButtonState; + vTarget @2 :Float32; + + enum IntelligentCruiseButtonManagementState { + inactive @0; # No button press or default state + preActive @1; # Pre-active state before transitioning to increasing or decreasing + increasing @2; # Increasing speed + decreasing @3; # Decreasing speed + holding @4; # Holding steady speed + } + + enum SendButtonState { + none @0; + increase @1; + decrease @2; + } +} + # Same struct as Log.RadarState.LeadData struct LeadData { dRel @0 :Float32; @@ -48,6 +68,7 @@ struct LeadData { struct SelfdriveStateSP @0x81c2f05a394cf4af { mads @0 :ModularAssistiveDrivingSystem; + intelligentCruiseButtonManagement @1 :IntelligentCruiseButtonManagement; } struct ModelManagerSP @0xaedffd8f31e7b55d { @@ -210,6 +231,8 @@ struct OnroadEventSP @0xda96579883444c35 { struct CarParamsSP @0x80ae746ee2596b11 { flags @0 :UInt32; # flags for car specific quirks in sunnypilot safetyParam @1 : Int16; # flags for sunnypilot's custom safety flags + pcmCruiseSpeed @3 :Bool; + intelligentCruiseButtonManagementAvailable @4 :Bool; neuralNetworkLateralControl @2 :NeuralNetworkLateralControl; @@ -229,6 +252,7 @@ struct CarControlSP @0xa5cd762cd951a455 { params @1 :List(Param); leadOne @2 :LeadData; leadTwo @3 :LeadData; + intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement; struct Param { key @0 :Text; @@ -237,7 +261,7 @@ struct CarControlSP @0xa5cd762cd951a455 { valueDEPRECATED @1 :Text; # The data type change may cause issues with backwards compatibility. } - + enum ParamType { string @0; bool @1; diff --git a/common/params_keys.h b/common/params_keys.h index f0511bf9c..80d12c6b9 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -149,6 +149,7 @@ inline static std::unordered_map keys = { {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}}, {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}}, {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}}, + {"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}}, {"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}}, {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}}, {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}}, diff --git a/opendbc_repo b/opendbc_repo index 189dc3f78..52d50f984 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 189dc3f78c3b2eeb0402e69760602aa7149d05c9 +Subproject commit 52d50f984191d63cf6a79398b21df2d2e3b9236e diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 820c68e89..2e52c0082 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -179,7 +179,7 @@ class Car: self.params.put_nonblocking("CarParamsSPPersistent", cp_sp_bytes) self.mock_carstate = MockCarState() - self.v_cruise_helper = VCruiseHelper(self.CP) + self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP) self.is_metric = self.params.get_bool("IsMetric") self.experimental_mode = self.params.get_bool("ExperimentalMode") diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 5ffb43f9e..c82287d2b 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -30,8 +30,8 @@ CRUISE_INTERVAL_SIGN = { class VCruiseHelper(VCruiseHelperSP): - def __init__(self, CP): - VCruiseHelperSP.__init__(self) + def __init__(self, CP, CP_SP): + VCruiseHelperSP.__init__(self, CP, CP_SP) self.CP = CP self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET @@ -46,10 +46,13 @@ class VCruiseHelper(VCruiseHelperSP): def update_v_cruise(self, CS, enabled, is_metric): self.v_cruise_kph_last = self.v_cruise_kph + self.get_minimum_set_speed(is_metric) + if CS.cruiseState.available: - if not self.CP.pcmCruise: + _enabled = self.update_enabled_state(CS, enabled) + if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled): # 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._update_v_cruise_non_pcm(CS, _enabled, is_metric) self.v_cruise_cluster_kph = self.v_cruise_kph self.update_button_timers(CS, enabled) else: @@ -111,7 +114,7 @@ class VCruiseHelper(VCruiseHelperSP): if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH) - self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX) + self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), self.v_cruise_min, V_CRUISE_MAX) def update_button_timers(self, CS, enabled): # increment timer for buttons still pressed diff --git a/selfdrive/car/helpers.py b/selfdrive/car/helpers.py index 275c84479..a7abc1976 100644 --- a/selfdrive/car/helpers.py +++ b/selfdrive/car/helpers.py @@ -60,5 +60,8 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct struct_dataclass.params = [structs.CarControlSP.Param(**remove_deprecated(p)) for p in struct_dict.get('params', [])] struct_dataclass.leadOne = structs.LeadData(**remove_deprecated(struct_dict.get('leadOne', {}))) struct_dataclass.leadTwo = structs.LeadData(**remove_deprecated(struct_dict.get('leadTwo', {}))) + struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement( + **remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {})) + ) return struct_dataclass diff --git a/selfdrive/car/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py index 4f9444d4b..61bae6893 100644 --- a/selfdrive/car/tests/test_cruise_speed.py +++ b/selfdrive/car/tests/test_cruise_speed.py @@ -5,7 +5,7 @@ import numpy as np from parameterized import parameterized_class from cereal import log from openpilot.selfdrive.car.cruise import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT -from cereal import car +from cereal import car, custom from openpilot.common.constants import CV from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -44,12 +44,13 @@ class TestCruiseSpeed: assert simulation_steady_state == pytest.approx(cruise_speed, abs=.01), f'Did not reach {self.speed} m/s' -# TODO: test pcmCruise -@parameterized_class(('pcm_cruise',), [(False,)]) +# TODO: test pcmCruise and pcmCruiseSpeed +@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)]) class TestVCruiseHelper: def setup_method(self): self.CP = car.CarParams(pcmCruise=self.pcm_cruise) - self.v_cruise_helper = VCruiseHelper(self.CP) + self.CP_SP = custom.CarParamsSP(pcmCruiseSpeed=self.pcm_cruise_speed) + self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP) self.reset_cruise_speed_state() def reset_cruise_speed_state(self): diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9e88a3769..3751efc87 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -116,7 +116,8 @@ class Controls(ControlsExt, ModelStateBase): CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ (not standstill or self.CP.steerAtStandstill) - CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl + CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and \ + (self.CP.openpilotLongitudinalControl or not self.CP_SP.pcmCruiseSpeed) actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state @@ -168,7 +169,7 @@ class Controls(ControlsExt, ModelStateBase): CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist() CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist() - CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl + CC.cruiseControl.override = CC.enabled and not CC.longActive and (self.CP.openpilotLongitudinalControl or not self.CP_SP.pcmCruiseSpeed) CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise) CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and not self.sm['longitudinalPlan'].shouldStop diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 749378f3a..69dc51360 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -26,6 +26,7 @@ from openpilot.system.version import get_build_metadata from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper +from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP REPLAY = "REPLAY" in os.environ @@ -156,6 +157,7 @@ class SelfdriveD(CruiseHelper): self.events_sp_prev = [] self.mads = ModularAssistiveDrivingSystem(self) + self.icbm = IntelligentCruiseButtonManagement(self.CP, self.CP_SP) self.car_events_sp = CarSpecificEventsSP(self.CP, self.params) @@ -442,6 +444,8 @@ class SelfdriveD(CruiseHelper): self.events.add(EventName.personalityChanged) self.experimental_mode_switched = False + self.icbm.run(CS, self.sm['carControl'], self.is_metric) + def data_sample(self): _car_state = messaging.recv_one(self.car_state_sock) CS = _car_state.carState if _car_state else self.CS_prev @@ -546,6 +550,11 @@ class SelfdriveD(CruiseHelper): mads.active = self.mads.active mads.available = self.mads.enabled_toggle + icbm = ss_sp.intelligentCruiseButtonManagement + icbm.state = self.icbm.state + icbm.sendButton = self.icbm.cruise_button + icbm.vTarget = self.icbm.v_target + self.pm.send('selfdriveStateSP', ss_sp_msg) # onroadEventsSP - logged every second or on change diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc index ac7b57e29..df4a6077b 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc @@ -18,6 +18,16 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) { cruisePanelScroller = new ScrollViewSP(list, this); vlayout->addWidget(cruisePanelScroller); + intelligentCruiseButtonManagement = new ParamControlSP( + "IntelligentCruiseButtonManagement", + tr("Intelligent Cruise Button Management (ICBM) (Alpha)"), + tr("When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control."), + "", + this + ); + intelligentCruiseButtonManagement->setConfirmation(true, false); + list->addItem(intelligentCruiseButtonManagement); + SmartCruiseControlVision = new ParamControl( "SmartCruiseControlVision", tr("Smart Cruise Control - Vision"), @@ -42,16 +52,22 @@ void LongitudinalPanel::showEvent(QShowEvent *event) { void LongitudinalPanel::refresh(bool _offroad) { auto cp_bytes = params.get("CarParamsPersistent"); - if (!cp_bytes.empty()) { + auto cp_sp_bytes = params.get("CarParamsSPPersistent"); + if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { AlignedBuffer aligned_buf; + AlignedBuffer aligned_buf_sp; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); + cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot(); has_longitudinal_control = hasLongitudinalControl(CP); is_pcm_cruise = CP.getPcmCruise(); + intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable(); } else { has_longitudinal_control = false; is_pcm_cruise = false; + intelligent_cruise_button_management_available = false; } QString accEnabledDescription = tr("Enable custom Short & Long press increments for cruise speed increase/decrease."); @@ -63,7 +79,7 @@ void LongitudinalPanel::refresh(bool _offroad) { customAccIncrement->setDescription(onroadOnlyDescription); customAccIncrement->showDescription(); } else { - if (has_longitudinal_control) { + if (has_longitudinal_control || intelligent_cruise_button_management_available) { if (is_pcm_cruise) { customAccIncrement->setDescription(accPcmCruiseDisabledDescription); customAccIncrement->showDescription(); @@ -75,14 +91,20 @@ void LongitudinalPanel::refresh(bool _offroad) { customAccIncrement->toggleFlipped(false); customAccIncrement->setDescription(accNoLongDescription); customAccIncrement->showDescription(); + params.remove("IntelligentCruiseButtonManagement"); + intelligentCruiseButtonManagement->toggleFlipped(false); } } + bool icbm_allowed = intelligent_cruise_button_management_available && !has_longitudinal_control; + intelligentCruiseButtonManagement->setEnabled(icbm_allowed && offroad); + // enable toggle when long is available and is not PCM cruise - customAccIncrement->setEnabled(has_longitudinal_control && !is_pcm_cruise && !offroad); + bool cai_allowed = (has_longitudinal_control && !is_pcm_cruise) || icbm_allowed; + customAccIncrement->setEnabled(cai_allowed && !offroad); customAccIncrement->refresh(); - SmartCruiseControlVision->setEnabled(has_longitudinal_control); + SmartCruiseControlVision->setEnabled(has_longitudinal_control || icbm_allowed); offroad = _offroad; } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h index 36c35720e..af8982259 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h @@ -23,6 +23,7 @@ private: Params params; bool has_longitudinal_control = false; bool is_pcm_cruise = false; + bool intelligent_cruise_button_management_available = false;; bool offroad = false; QStackedLayout *main_layout = nullptr; @@ -30,4 +31,5 @@ private: QWidget *cruisePanelScreen = nullptr; CustomAccIncrement *customAccIncrement = nullptr; ParamControl *SmartCruiseControlVision; + ParamControl *intelligentCruiseButtonManagement = nullptr; }; diff --git a/sunnypilot/selfdrive/car/cruise_ext.py b/sunnypilot/selfdrive/car/cruise_ext.py index f443aeee0..716e3e1c9 100644 --- a/sunnypilot/selfdrive/car/cruise_ext.py +++ b/sunnypilot/selfdrive/car/cruise_ext.py @@ -7,19 +7,46 @@ See the LICENSE.md file in the root directory for more details. import numpy as np from cereal import car +from opendbc.car import structs from openpilot.common.params import Params +from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed ButtonType = car.CarState.ButtonEvent.Type +CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, + ButtonType.setCruise: 0, ButtonType.resumeCruise: 0, + ButtonType.cancel: 0, ButtonType.mainCruise: 0} + +V_CRUISE_MIN = 8 + + +def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarState.ButtonEvent.Type, int]) -> None: + # increment timer for buttons still pressed + for k in button_timers: + if button_timers[k] > 0: + button_timers[k] += 1 + + for b in CS.buttonEvents: + if b.type.raw in button_timers: + # Start/end timer and store current state on change of button pressed + button_timers[b.type.raw] = 1 if b.pressed else 0 + + class VCruiseHelperSP: - def __init__(self) -> None: + def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> None: + self.CP = CP + self.CP_SP = CP_SP self.params = Params() + self.v_cruise_min = 0 + self.enabled_prev = False self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled") self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True) self.long_increment = self.params.get("CustomAccLongPressIncrement", return_default=True) + self.enable_button_timers = CRUISE_BUTTON_TIMER + def read_custom_set_speed_params(self) -> None: self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled") self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True) @@ -39,3 +66,26 @@ class VCruiseHelperSP: v_cruise_delta = v_cruise_delta * actual_increment return round_to_nearest, v_cruise_delta + + def get_minimum_set_speed(self, is_metric: bool) -> None: + if self.CP_SP.pcmCruiseSpeed: + self.v_cruise_min = V_CRUISE_MIN + return + + self.v_cruise_min = get_minimum_set_speed(is_metric) + + def update_enabled_state(self, CS: car.CarState, enabled: bool) -> bool: + # special enabled state for non pcmCruiseSpeed, unchanged for non pcmCruise + if not self.CP_SP.pcmCruiseSpeed: + update_manual_button_timers(CS, self.enable_button_timers) + button_pressed = any(self.enable_button_timers[k] > 0 for k in self.enable_button_timers) + + if enabled and not self.enabled_prev: + self.enabled_prev = not button_pressed + enabled = False + elif not enabled: + self.enabled_prev = enabled + + return enabled and self.enabled_prev + + return enabled diff --git a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/__init__.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py new file mode 100644 index 000000000..0b0957b82 --- /dev/null +++ b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py @@ -0,0 +1,137 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from cereal import car, custom +from opendbc.car import structs, apply_hysteresis +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_CTRL +from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed +from openpilot.sunnypilot.selfdrive.car.cruise_ext import CRUISE_BUTTON_TIMER, update_manual_button_timers + +LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource +State = custom.IntelligentCruiseButtonManagement.IntelligentCruiseButtonManagementState +SendButtonState = custom.IntelligentCruiseButtonManagement.SendButtonState + +ALLOWED_SPEED_THRESHOLD = 1.8 # m/s, ~4 MPH +HYST_GAP = 0.75 +INACTIVE_TIMER = 0.4 + + +SEND_BUTTONS = { + State.increasing: SendButtonState.increase, + State.decreasing: SendButtonState.decrease, +} + + +class IntelligentCruiseButtonManagement: + def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP): + self.CP = CP + self.CP_SP = CP_SP + + self.v_target = 0 + self.v_cruise_cluster = 0 + self.v_cruise_min = 0 + self.cruise_button = SendButtonState.none + self.state = State.inactive + self.pre_active_timer = 0 + + self.is_ready = False + self.is_ready_prev = False + self.v_target_ms_last = 0.0 + self.is_metric = False + + self.cruise_button_timers = CRUISE_BUTTON_TIMER + + @property + def v_cruise_equal(self) -> bool: + return self.v_target == self.v_cruise_cluster + + def update_calculations(self, CS: car.CarState) -> None: + speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH + ms_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS + v_cruise_ms = CS.vCruise * CV.KPH_TO_MS + + # all targets in m/s + v_targets = { + LongitudinalPlanSource.cruise: v_cruise_ms + } + source = min(v_targets, key=lambda k: v_targets[k]) + v_target_ms = v_targets[source] + + self.v_target_ms_last = apply_hysteresis(v_target_ms, self.v_target_ms_last, HYST_GAP * ms_conv) + + self.v_target = round(self.v_target_ms_last * speed_conv) + self.v_cruise_min = get_minimum_set_speed(self.is_metric) + self.v_cruise_cluster = round(CS.cruiseState.speedCluster * speed_conv) + + def update_state_machine(self) -> custom.IntelligentCruiseButtonManagement.SendButtonState: + self.pre_active_timer = max(0, self.pre_active_timer - 1) + + # HOLDING, ACCELERATING, DECELERATING, PRE_ACTIVE + if self.state != State.inactive: + if not self.is_ready: + self.state = State.inactive + + else: + # PRE_ACTIVE + if self.state == State.preActive: + if self.pre_active_timer <= 0: + if self.v_cruise_equal: + self.state = State.holding + + elif self.v_target > self.v_cruise_cluster: + self.state = State.increasing + + elif self.v_target < self.v_cruise_cluster and self.v_cruise_cluster > self.v_cruise_min: + self.state = State.decreasing + + # HOLDING + elif self.state == State.holding: + if not self.v_cruise_equal: + self.state = State.preActive + + # ACCELERATING + elif self.state == State.increasing: + if self.v_target <= self.v_cruise_cluster: + self.state = State.holding + + # DECELERATING + elif self.state == State.decreasing: + if self.v_target >= self.v_cruise_cluster or self.v_cruise_cluster <= self.v_cruise_min: + self.state = State.holding + + # INACTIVE + elif self.state == State.inactive: + if self.is_ready and not self.is_ready_prev: + self.pre_active_timer = int(INACTIVE_TIMER / DT_CTRL) + self.state = State.preActive + + send_button = SEND_BUTTONS.get(self.state, SendButtonState.none) + + return send_button + + def update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None: + update_manual_button_timers(CS, self.cruise_button_timers) + + allowed_speed = CS.vEgo > ALLOWED_SPEED_THRESHOLD + ready = CS.cruiseState.enabled and allowed_speed and not CC.cruiseControl.override and not CC.cruiseControl.cancel and \ + not CC.cruiseControl.resume + button_pressed = any(self.cruise_button_timers[k] > 0 for k in self.cruise_button_timers) + + self.is_ready = ready and not button_pressed + + def run(self, CS: car.CarState, CC: car.CarControl, is_metric: bool) -> None: + if self.CP_SP.pcmCruiseSpeed: + return + + self.is_metric = is_metric + + self.update_calculations(CS) + self.update_readiness(CS, CC) + + self.cruise_button = self.update_state_machine() + + self.is_ready_prev = self.is_ready diff --git a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/helpers.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/helpers.py new file mode 100644 index 000000000..eb4bbdecb --- /dev/null +++ b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/helpers.py @@ -0,0 +1,8 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +def get_minimum_set_speed(is_metric: bool) -> int: + return 30 if is_metric else 20 diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index cd7a24b63..55244caa5 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -43,11 +43,21 @@ def _initialize_neural_network_lateral_control(CI: CarInterfaceBase, CP: structs CP_SP.neuralNetworkLateralControl.fuzzyFingerprint = not exact_match +def _initialize_intelligent_cruise_button_management(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params = None) -> None: + if params is None: + params = Params() + + icbm_enabled = params.get_bool("IntelligentCruiseButtonManagement") + if icbm_enabled and CP_SP.intelligentCruiseButtonManagementAvailable and not CP.openpilotLongitudinalControl: + CP_SP.pcmCruiseSpeed = False + + def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None: CP = CI.CP CP_SP = CI.CP_SP _initialize_neural_network_lateral_control(CI, CP, CP_SP, params) + _initialize_intelligent_cruise_button_management(CP, CP_SP, params) def initialize_params(params) -> list[dict[str, Any]]: diff --git a/sunnypilot/selfdrive/car/tests/test_custom_cruise.py b/sunnypilot/selfdrive/car/tests/test_custom_cruise.py index 3bed8534e..a1f7f8f02 100644 --- a/sunnypilot/selfdrive/car/tests/test_custom_cruise.py +++ b/sunnypilot/selfdrive/car/tests/test_custom_cruise.py @@ -11,7 +11,8 @@ ButtonEvent = car.CarState.ButtonEvent ButtonType = car.CarState.ButtonEvent.Type -@parameterized_class(('pcm_cruise',), [(False,)]) +# TODO: test pcmCruise and pcmCruiseSpeed +@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)]) class TestCustomAccIncrements(TestVCruiseHelper): def setup_method(self): TestVCruiseHelper.setup_method(self) diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index a096b7dc8..e0f9326bf 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -75,6 +75,8 @@ class ControlsExt: CC_SP.params = self.param_store.param_list + CC_SP.intelligentCruiseButtonManagement = sm['selfdriveStateSP'].intelligentCruiseButtonManagement + return CC_SP @staticmethod