diff --git a/common/params.cc b/common/params.cc index 0c19b6dc76..78055f35fd 100644 --- a/common/params.cc +++ b/common/params.cc @@ -228,6 +228,7 @@ std::unordered_map keys = { {"CustomMapboxTokenSk", PERSISTENT}, {"CustomOffsets", PERSISTENT}, {"CustomStockLong", PERSISTENT}, + {"CustomStockLongPlanner", PERSISTENT}, {"CustomTorqueLateral", PERSISTENT}, {"DevUI", PERSISTENT}, {"DevUIInfo", PERSISTENT}, diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index e2d1532189..939739d530 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -63,7 +63,7 @@ class CarController: self.lat_disengage_init = False self.lat_active_last = False - self.sm = messaging.SubMaster(['longitudinalPlanSP']) + self.sm = messaging.SubMaster(['longitudinalPlan', 'longitudinalPlanSP']) self.param_s = Params() self.is_metric = self.param_s.get_bool("IsMetric") self.speed_limit_control_enabled = False @@ -92,12 +92,19 @@ class CarController: self.v_tsc = 0 self.m_tsc = 0 self.steady_speed = 0 + self.speeds = 0 + self.v_target_plan = 0 self.hkg_can_smooth_stop = self.param_s.get_bool("HkgSmoothStop") + self.custom_planner_speed = self.param_s.get_bool("CustomStockLongPlanner") def update(self, CC, CS, now_nanos): if not self.CP.pcmCruiseSpeed: self.sm.update(0) + if self.sm.updated['longitudinalPlan']: + _speeds = self.sm['longitudinalPlan'].speeds + self.speeds = _speeds[-1] if len(_speeds) else 0 + if self.sm.updated['longitudinalPlanSP']: self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState @@ -110,8 +117,10 @@ class CarController: if self.frame % 200 == 0: self.speed_limit_control_enabled = self.param_s.get_bool("SpeedLimitControl") self.is_metric = self.param_s.get_bool("IsMetric") + self.custom_planner_speed = self.param_s.get_bool("CustomStockLongPlanner") self.last_speed_limit_sign_tap = self.param_s.get_bool("LastSpeedLimitSignTap") self.v_cruise_min = HYUNDAI_V_CRUISE_MIN[self.is_metric] * (CV.KPH_TO_MPH if not self.is_metric else 1) + self.v_target_plan = min(CC.vCruise * CV.KPH_TO_MS, self.speeds) actuators = CC.actuators hud_control = CC.hudControl @@ -428,6 +437,8 @@ class CarController: target_speed_kph = set_speed_kph else: target_speed_kph = min(v_cruise_kph_prev, set_speed_kph) + if self.custom_planner_speed: + target_speed_kph = self.curve_speed_hysteresis(self.v_target_plan) if self.v_tsc_state != 0 or self.m_tsc_state > 1: self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev) else: diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index a3e375f355..77ec997c19 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -52,6 +52,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { tr("When enabled, sunnypilot will attempt to control stock longitudinal control with ACC button presses.\nThis feature must be used along with SLC, and/or V-TSC, and/or M-TSC."), "../assets/offroad/icon_speed_limit.png", }, + { + "CustomStockLongPlanner", + tr("Use Planner Speed"), + "", + "../assets/offroad/icon_blank.png", + }, { "ExperimentalMode", tr("Experimental Mode"), @@ -129,6 +135,9 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { connect(toggles["ExperimentalLongitudinalEnabled"], &ToggleControl::toggleFlipped, [=]() { updateToggles(); }); + connect(toggles["CustomStockLong"], &ToggleControl::toggleFlipped, [=]() { + updateToggles(); + }); param_watcher = new ParamWatcher(this); @@ -150,6 +159,7 @@ void TogglesPanel::updateToggles() { auto experimental_mode_toggle = toggles["ExperimentalMode"]; auto op_long_toggle = toggles["ExperimentalLongitudinalEnabled"]; auto custom_stock_long_toggle = toggles["CustomStockLong"]; + auto custom_stock_long_planner = toggles["CustomStockLongPlanner"]; const QString e2e_description = QString("%1
" "

%2


" "%3
" @@ -194,6 +204,12 @@ void TogglesPanel::updateToggles() { long_personality_setting->setEnabled(true); custom_stock_long_toggle->setEnabled(false); params.remove("CustomStockLong"); + } else if (custom_stock_long_toggle->isToggled()) { + experimental_mode_toggle->setEnabled(true); + experimental_mode_toggle->setDescription(e2e_description); + long_personality_setting->setEnabled(false); + op_long_toggle->setEnabled(false); + custom_stock_long_planner->setEnabled(true); } else { // no long for now experimental_mode_toggle->setEnabled(false); @@ -214,14 +230,19 @@ void TogglesPanel::updateToggles() { experimental_mode_toggle->setDescription("" + long_desc + "

" + e2e_description); custom_stock_long_toggle->setEnabled(CP.getCustomStockLongAvailable()); + custom_stock_long_planner->setEnabled(false); + op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release); + op_long_toggle->setEnabled(CP.getExperimentalLongitudinalAvailable() && !is_release); } experimental_mode_toggle->refresh(); custom_stock_long_toggle->refresh(); + op_long_toggle->refresh(); } else { experimental_mode_toggle->setDescription(e2e_description); op_long_toggle->setVisible(false); custom_stock_long_toggle->setVisible(false); + custom_stock_long_planner->setVisible(false); } } diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index d9973c663a..9a0e5e4eab 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -365,7 +365,8 @@ ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(fals void ExperimentalButton::changeMode() { const auto cp = (*uiState()->sm)["carParams"].getCarParams(); - bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed"); + bool can_change = (hasLongitudinalControl(cp) || (cp.getCustomStockLongAvailable() && params.getBool("CustomStockLongPlanner"))) + && params.getBool("ExperimentalModeConfirmed"); if (can_change) { params.putBool("ExperimentalMode", !experimental_mode); }