diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4e78d8350..9b51b732a 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -124,7 +124,6 @@ class Controls(ControlsExt, ModelStateBase): self.LaC.extension.update_model_v2(self.sm['modelV2']) - self.lat_delay = get_lat_delay(self.params, self.sm["liveDelay"].lateralDelay) self.LaC.extension.update_lateral_lag(self.lat_delay) long_plan = self.sm['longitudinalPlan'] @@ -276,6 +275,9 @@ class Controls(ControlsExt, ModelStateBase): while not evt.is_set(): self.get_params_sp() + if self.CP.lateralTuning.which() == 'torque': + self.lat_delay = get_lat_delay(self.params, self.sm["liveDelay"].lateralDelay) + time.sleep(0.1) def run(self): diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc index 5c3b03d2a..1696c7f63 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc @@ -7,6 +7,8 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h" +#include "selfdrive/ui/sunnypilot/qt/util.h" + SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) { subPanelFrame = new QFrame(); QVBoxLayout *subPanelLayout = new QVBoxLayout(subPanelFrame); @@ -109,7 +111,7 @@ void SpeedLimitSettings::refresh() { QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset")); bool has_longitudinal_control; - bool intelligent_cruise_button_management_available; + bool has_icbm; auto cp_bytes = params.get("CarParamsPersistent"); auto cp_sp_bytes = params.get("CarParamsSPPersistent"); if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { @@ -121,16 +123,16 @@ void SpeedLimitSettings::refresh() { cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot(); has_longitudinal_control = hasLongitudinalControl(CP); - intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable(); + has_icbm = hasIntelligentCruiseButtonManagement(CP_SP); - if (!has_longitudinal_control && CP_SP.getPcmCruiseSpeed()) { + if (!has_longitudinal_control && !has_icbm) { if (speed_limit_mode_param == SpeedLimitMode::ASSIST) { params.put("SpeedLimitMode", std::to_string(static_cast(SpeedLimitMode::WARNING))); } } } else { has_longitudinal_control = false; - intelligent_cruise_button_management_available = false; + has_icbm = false; } speed_limit_mode_settings->setDescription(modeDescription(speed_limit_mode_param)); @@ -150,13 +152,14 @@ void SpeedLimitSettings::refresh() { speed_limit_offset->showDescription(); } - if (has_longitudinal_control || intelligent_cruise_button_management_available) { + if (has_longitudinal_control || has_icbm) { speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues(getSpeedLimitModeValues())); } else { speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues( {SpeedLimitMode::OFF, SpeedLimitMode::INFORMATION, SpeedLimitMode::WARNING})); } + speed_limit_mode_settings->refresh(); speed_limit_mode_settings->showDescription(); speed_limit_offset->showDescription(); } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h index 23fa7b4ec..69e85814c 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h @@ -35,6 +35,7 @@ private: SpeedLimitPolicy *speedLimitPolicyScreen; ButtonParamControlSP *speed_limit_offset_settings; OptionControlSP *speed_limit_offset; + bool icbm_available = false; static QString offsetDescription(SpeedLimitOffsetType type = SpeedLimitOffsetType::NONE) { QString none_str = tr("⦿ None: No Offset"); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc index f2c7ea382..2cc59ea33 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc @@ -7,6 +7,8 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h" +#include "selfdrive/ui/sunnypilot/qt/util.h" + LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) { setStyleSheet(R"( #back_btn { @@ -40,7 +42,9 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) { "", this ); - intelligentCruiseButtonManagement->setConfirmation(true, false); + QObject::connect(intelligentCruiseButtonManagement, &ParamControlSP::toggleFlipped, this, [=](bool) { + refresh(offroad); + }); list->addItem(intelligentCruiseButtonManagement); dynamicExperimentalControl = new ParamControlSP( @@ -112,22 +116,41 @@ void LongitudinalPanel::refresh(bool _offroad) { has_longitudinal_control = hasLongitudinalControl(CP); is_pcm_cruise = CP.getPcmCruise(); - intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable(); + has_icbm = hasIntelligentCruiseButtonManagement(CP_SP); - if (!intelligent_cruise_button_management_available || has_longitudinal_control) { + if (CP_SP.getIntelligentCruiseButtonManagementAvailable() && !has_longitudinal_control) { + intelligentCruiseButtonManagement->setEnabled(offroad); + } else { params.remove("IntelligentCruiseButtonManagement"); + intelligentCruiseButtonManagement->setEnabled(false); } - if (!has_longitudinal_control && CP_SP.getPcmCruiseSpeed()) { + if (has_longitudinal_control || has_icbm) { + // enable Custom ACC Increments when long is available and is not PCM cruise + customAccIncrement->setEnabled(((has_longitudinal_control && !is_pcm_cruise) || has_icbm) && offroad); + dynamicExperimentalControl->setEnabled(has_longitudinal_control); + SmartCruiseControlVision->setEnabled(true); + SmartCruiseControlMap->setEnabled(true); + } else { params.remove("CustomAccIncrementsEnabled"); params.remove("DynamicExperimentalControl"); params.remove("SmartCruiseControlVision"); params.remove("SmartCruiseControlMap"); + customAccIncrement->setEnabled(false); + dynamicExperimentalControl->setEnabled(false); + SmartCruiseControlVision->setEnabled(false); + SmartCruiseControlMap->setEnabled(false); } + + intelligentCruiseButtonManagement->refresh(); + customAccIncrement->refresh(); + dynamicExperimentalControl->refresh(); + SmartCruiseControlVision->refresh(); + SmartCruiseControlMap->refresh(); } else { has_longitudinal_control = false; is_pcm_cruise = false; - intelligent_cruise_button_management_available = false; + has_icbm = false; } QString accEnabledDescription = tr("Enable custom Short & Long press increments for cruise speed increase/decrease."); @@ -139,8 +162,8 @@ void LongitudinalPanel::refresh(bool _offroad) { customAccIncrement->setDescription(onroadOnlyDescription); customAccIncrement->showDescription(); } else { - if (has_longitudinal_control || intelligent_cruise_button_management_available) { - if (is_pcm_cruise) { + if (has_longitudinal_control || has_icbm) { + if (has_longitudinal_control && is_pcm_cruise) { customAccIncrement->setDescription(accPcmCruiseDisabledDescription); customAccIncrement->showDescription(); } else { @@ -150,21 +173,8 @@ void LongitudinalPanel::refresh(bool _offroad) { customAccIncrement->toggleFlipped(false); customAccIncrement->setDescription(accNoLongDescription); customAccIncrement->showDescription(); - 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 - bool cai_allowed = (has_longitudinal_control && !is_pcm_cruise) || icbm_allowed; - customAccIncrement->setEnabled(cai_allowed && !offroad); - customAccIncrement->refresh(); - - dynamicExperimentalControl->setEnabled(has_longitudinal_control); - SmartCruiseControlVision->setEnabled(has_longitudinal_control || icbm_allowed); - SmartCruiseControlMap->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 127b7871e..2b9c0b896 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h @@ -25,7 +25,7 @@ private: Params params; bool has_longitudinal_control = false; bool is_pcm_cruise = false; - bool intelligent_cruise_button_management_available = false;; + bool has_icbm = false; bool offroad = false; QStackedLayout *main_layout = nullptr; diff --git a/selfdrive/ui/sunnypilot/qt/util.cc b/selfdrive/ui/sunnypilot/qt/util.cc index be39b297d..eaa1f4bd1 100644 --- a/selfdrive/ui/sunnypilot/qt/util.cc +++ b/selfdrive/ui/sunnypilot/qt/util.cc @@ -122,3 +122,7 @@ std::optional loadCerealEvent(Params& params, const std:: return std::nullopt; } } + +bool hasIntelligentCruiseButtonManagement(const cereal::CarParamsSP::Reader &car_params_sp) { + return car_params_sp.getIntelligentCruiseButtonManagementAvailable() && Params().getBool("IntelligentCruiseButtonManagement"); +} diff --git a/selfdrive/ui/sunnypilot/qt/util.h b/selfdrive/ui/sunnypilot/qt/util.h index 4b9d615ce..60a73615b 100644 --- a/selfdrive/ui/sunnypilot/qt/util.h +++ b/selfdrive/ui/sunnypilot/qt/util.h @@ -23,3 +23,4 @@ std::optional getParamIgnoringDefault(const std::string ¶m_name, co QMap loadPlatformList(); QStringList searchFromList(const QString &query, const QStringList &list); std::optional loadCerealEvent(Params& params, const std::string& _param); +bool hasIntelligentCruiseButtonManagement(const cereal::CarParamsSP::Reader &car_params_sp); diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py index 302d8cd14..b94845205 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -27,7 +27,11 @@ ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting) ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES) DISABLED_GUARD_PERIOD = 0.5 # secs. -PRE_ACTIVE_GUARD_PERIOD = 15 # secs. Time to wait after activation before considering temp deactivation signal. +# secs. Time to wait after activation before considering temp deactivation signal. +PRE_ACTIVE_GUARD_PERIOD = { + True: 15, + False: 5, +} SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1 # secs. Time to wait after speed limit change before switching to preActive. LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide. @@ -109,6 +113,16 @@ class SpeedLimitAssist: def target_set_speed_confirmed(self) -> bool: return bool(self.v_cruise_cluster_conv == self.target_set_speed_conv) + @property + def v_cruise_cluster_below_confirm_speed_threshold(self) -> bool: + return bool(self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]) + + def update_active_event(self, events_sp: EventsSP) -> None: + if self.v_cruise_cluster_below_confirm_speed_threshold: + events_sp.add(EventNameSP.speedLimitChanged) + else: + events_sp.add(EventNameSP.speedLimitActive) + def get_v_target_from_control(self) -> float: if self._has_speed_limit: if self.pcm_op_long and self.is_enabled: @@ -175,7 +189,7 @@ class SpeedLimitAssist: @property def apply_confirm_speed_threshold(self) -> bool: # below CST: always require user confirmation - if self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]: + if self.v_cruise_cluster_below_confirm_speed_threshold: return True # at/above CST: @@ -231,7 +245,7 @@ class SpeedLimitAssist: self.state = SpeedLimitAssistState.inactive elif self.speed_limit_changed and self.apply_confirm_speed_threshold: self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH: self.state = SpeedLimitAssistState.adapting @@ -241,7 +255,7 @@ class SpeedLimitAssist: self.state = SpeedLimitAssistState.inactive elif self.speed_limit_changed and self.apply_confirm_speed_threshold: self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) elif self.v_offset >= LIMIT_SPEED_OFFSET_TH: self.state = SpeedLimitAssistState.active @@ -251,7 +265,7 @@ class SpeedLimitAssist: self._update_confirmed_state() elif self.speed_limit_changed: self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) # PRE_ACTIVE elif self.state == SpeedLimitAssistState.preActive: @@ -277,7 +291,7 @@ class SpeedLimitAssist: self._update_confirmed_state() elif self._has_speed_limit: self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) else: self.state = SpeedLimitAssistState.pending @@ -303,7 +317,7 @@ class SpeedLimitAssist: elif self.speed_limit_changed and self.apply_confirm_speed_threshold: self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) # PRE_ACTIVE elif self.state == SpeedLimitAssistState.preActive: @@ -317,7 +331,7 @@ class SpeedLimitAssist: elif self.state == SpeedLimitAssistState.inactive: if self.speed_limit_changed: self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) elif self._update_non_pcm_long_confirmed_state(): self.state = SpeedLimitAssistState.active @@ -333,7 +347,7 @@ class SpeedLimitAssist: self.state = SpeedLimitAssistState.active elif self._has_speed_limit: self.state = SpeedLimitAssistState.preActive - self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL) else: self.state = SpeedLimitAssistState.inactive @@ -351,15 +365,15 @@ class SpeedLimitAssist: if self.is_active: if self._state_prev not in ACTIVE_STATES: - events_sp.add(EventNameSP.speedLimitActive) + self.update_active_event(events_sp) # only notify if we acquire a valid speed limit # do not check has_speed_limit here elif self._speed_limit != self.speed_limit_prev: if self.speed_limit_prev <= 0: - events_sp.add(EventNameSP.speedLimitActive) + self.update_active_event(events_sp) elif self.speed_limit_prev > 0 and self._speed_limit > 0: - events_sp.add(EventNameSP.speedLimitChanged) + self.update_active_event(events_sp) def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float, speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None: diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py index d2c7a4716..168c53016 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py @@ -39,7 +39,7 @@ class TestSpeedLimitAssist: self.events_sp = EventsSP() CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2) self.sla = SpeedLimitAssist(CI.CP) - self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL) self.pcm_long_max_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric][1] # use 80 MPH for now self.speed_conv = CV.MS_TO_KPH if self.sla.is_metric else CV.MS_TO_MPH @@ -114,7 +114,7 @@ class TestSpeedLimitAssist: self.sla.state = SpeedLimitAssistState.preActive self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) - for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)): + for _ in range(int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)): self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.inactive diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 5d5424bb1..4edc0bd47 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -4,7 +4,6 @@ from openpilot.common.constants import CV from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \ NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target AlertSize = log.SelfdriveState.AlertSize @@ -34,6 +33,9 @@ def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messag speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv) + alert_1_str = "" + alert_2_str = "" + alert_size = AlertSize.none if CP.openpilotLongitudinalControl and CP.pcmCruise: # PCM long @@ -41,24 +43,15 @@ def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messag pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv) speed_unit = "km/h" if metric else "mph" + + alert_1_str = "Speed Limit Assist: Activation Required" alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate" - else: - # Non PCM long - v_cruise_cluster = CS.vCruiseCluster * CV.KPH_TO_MS - - req_plus, req_minus = compare_cluster_target(v_cruise_cluster, speed_limit_final_last, metric) - arrow_str = "" - if req_plus: - arrow_str = "RES/+" - elif req_minus: - arrow_str = "SET/-" - - alert_2_str = f"Operate the {arrow_str} cruise control button to activate" + alert_size = AlertSize.mid return Alert( - "Speed Limit Assist: Activation Required", + alert_1_str, alert_2_str, - AlertStatus.normal, AlertSize.mid, + AlertStatus.normal, alert_size, Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleLow, .1)