diff --git a/cereal/custom.capnp b/cereal/custom.capnp index c10cbe587..2ce1378a7 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -56,6 +56,7 @@ struct FrogPilotPlan @0x80ae746ee2596b11 { tFollow @20 :Float32; unconfirmedSlcSpeedLimit @21 :Float64; vCruise @22 :Float32; + vtscControllingCurve @23 :Bool; } struct CustomReserved5 @0xa5cd762cd951a455 { diff --git a/common/params.cc b/common/params.cc index 6fa7bce0d..ea6ff9527 100644 --- a/common/params.cc +++ b/common/params.cc @@ -251,6 +251,7 @@ std::unordered_map keys = { {"CrosstrekTorque", PERSISTENT}, {"CurrentHolidayTheme", PERSISTENT}, {"CurrentRandomEvent", PERSISTENT}, + {"CurveSensitivity", PERSISTENT}, {"CustomAlerts", PERSISTENT}, {"CustomColors", PERSISTENT}, {"CustomCruise", PERSISTENT}, @@ -270,6 +271,7 @@ std::unordered_map keys = { {"DisableMTSCSmoothing", PERSISTENT}, {"DisableOnroadUploads", PERSISTENT}, {"DisableOpenpilotLongitudinal", PERSISTENT}, + {"DisableVTSCSmoothing", PERSISTENT}, {"DisengageVolume", PERSISTENT}, {"DistanceLongPressed", PERSISTENT}, {"DragonPilotTune", PERSISTENT}, @@ -438,12 +440,14 @@ std::unordered_map keys = { {"TrafficJerk", PERSISTENT}, {"TrafficMode", PERSISTENT}, {"TrafficModeActive", CLEAR_ON_OFFROAD_TRANSITION}, + {"TurnAggressiveness", PERSISTENT}, {"TurnDesires", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"UnlockDoors", PERSISTENT}, {"Updated", PERSISTENT}, {"UseSI", PERSISTENT}, {"UseVienna", PERSISTENT}, + {"VisionTurnControl", PERSISTENT}, {"WarningImmediateVolume", PERSISTENT}, {"WarningSoftVolume", PERSISTENT}, {"WheelIcon", PERSISTENT}, diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_vtc.png b/selfdrive/frogpilot/assets/toggle_icons/icon_vtc.png new file mode 100644 index 000000000..8218b456c Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_vtc.png differ diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 71de5fed7..4e826def4 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -37,6 +37,8 @@ A_CRUISE_MAX_VALS_SPORT = [3.5, 3.5, 3.3, 2.8, 1.5, 1.0, .75, .6, .38, .2] TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT] +TARGET_LAT_A = 1.9 # m/s^2 + def get_min_accel_eco(v_ego): return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_ECO) @@ -71,6 +73,7 @@ class FrogPilotPlanner: self.mtsc_target = 0 self.slc_target = 0 self.t_follow = 0 + self.vtsc_target = 0 def update(self, carState, controlsState, frogpilotCarControl, frogpilotNavigation, liveLocationKalman, modelData, radarState): v_cruise_kph = min(controlsState.vCruise, V_CRUISE_MAX) @@ -87,7 +90,7 @@ class FrogPilotPlanner: else: self.max_accel = ACCEL_MAX - v_cruise_changed = self.mtsc_target < v_cruise + v_cruise_changed = (self.mtsc_target or self.vtsc_target) < v_cruise if self.deceleration_profile == 1 and not v_cruise_changed: self.min_accel = get_min_accel_eco(v_ego) @@ -248,7 +251,7 @@ class FrogPilotPlanner: frogpilotPlan.accelerationJerk = A_CHANGE_COST * (float(self.jerk) if self.lead_one.status else 1) frogpilotPlan.accelerationJerkStock = A_CHANGE_COST - frogpilotPlan.adjustedCruise = float(self.mtsc_target * (CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH)) + frogpilotPlan.adjustedCruise = float(min(self.mtsc_target, self.vtsc_target) * (CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH)) frogpilotPlan.conditionalExperimental = self.cem.experimental_mode frogpilotPlan.desiredFollowDistance = self.safe_obstacle_distance - self.stopped_equivalence_factor frogpilotPlan.egoJerk = J_EGO_COST * (float(self.jerk) if self.lead_one.status else 1) @@ -272,6 +275,8 @@ class FrogPilotPlanner: frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit + frogpilotPlan.vtscControllingCurve = bool(self.mtsc_target > self.vtsc_target) + pm.send('frogpilotPlan', frogpilot_plan_send) def update_frogpilot_params(self): @@ -319,3 +324,7 @@ class FrogPilotPlanner: self.speed_limit_controller = self.CP.openpilotLongitudinalControl and self.params.get_bool("SpeedLimitController") self.speed_limit_confirmation = self.speed_limit_controller and self.params.get_bool("SLCConfirmation") self.speed_limit_controller_override = self.speed_limit_controller and self.params.get_int("SLCOverride") + + self.vision_turn_controller = self.CP.openpilotLongitudinalControl and self.params.get_bool("VisionTurnControl") + self.curve_sensitivity = self.params.get_int("CurveSensitivity") / 100 if self.vision_turn_controller else 1 + self.turn_aggressiveness = self.params.get_int("TurnAggressiveness") / 100 if self.vision_turn_controller else 1 diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index f19495224..c36367a4b 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -133,6 +133,11 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"ShowSLCOffset", tr("Show Speed Limit Offset"), tr("Show the speed limit offset separated from the speed limit in the onroad UI when using 'Speed Limit Controller'."), ""}, {"SpeedLimitChangedAlert", tr("Speed Limit Changed Alert"), tr("Trigger an alert whenever the speed limit changes."), ""}, {"UseVienna", tr("Use Vienna Speed Limit Signs"), tr("Use the Vienna (EU) speed limit style signs as opposed to MUTCD (US)."), ""}, + + {"VisionTurnControl", tr("Vision Turn Speed Controller"), tr("Slow down for detected curves in the road."), "../frogpilot/assets/toggle_icons/icon_vtc.png"}, + {"DisableVTSCSmoothing", tr("Disable VTSC UI Smoothing"), tr("Disables the smoothing for the requested speed in the onroad UI."), ""}, + {"CurveSensitivity", tr("Curve Detection Sensitivity"), tr("Set curve detection sensitivity. Higher values prompt earlier responses, lower values lead to smoother but later reactions."), ""}, + {"TurnAggressiveness", tr("Turn Speed Aggressiveness"), tr("Set turn speed aggressiveness. Higher values result in faster turns, lower values yield gentler turns."), ""}, }; for (const auto &[param, title, desc, icon] : controlToggles) { @@ -753,6 +758,18 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil slcPriorityButton->setValue(initialPriorities.join(", ")); addItem(slcPriorityButton); + } else if (param == "VisionTurnControl") { + FrogPilotParamManageControl *visionTurnControlToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(visionTurnControlToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + openParentToggle(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(visionTurnControlKeys.find(key.c_str()) != visionTurnControlKeys.end()); + } + }); + toggle = visionTurnControlToggle; + } else if (param == "CurveSensitivity" || param == "TurnAggressiveness") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 1, 200, std::map(), this, false, "%"); + } else { toggle = new ParamControl(param, title, desc, icon, this); } @@ -1006,7 +1023,7 @@ void FrogPilotControlsPanel::hideToggles() { trafficProfile->setVisible(false); std::set longitudinalKeys = {"ConditionalExperimental", "CustomPersonalities", "ExperimentalModeActivation", - "LongitudinalTune", "MTSCEnabled", "SpeedLimitController"}; + "LongitudinalTune", "MTSCEnabled", "SpeedLimitController", "VisionTurnControl"}; for (auto &[key, toggle] : toggles) { toggle->setVisible(false); diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index 2effde49c..6a1d3180f 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -54,7 +54,7 @@ private: std::set speedLimitControllerControlsKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority"}; std::set speedLimitControllerQOLKeys = {"ForceMPHDashboard", "SetSpeedLimit", "SLCConfirmation", "SLCLookaheadHigher", "SLCLookaheadLower"}; std::set speedLimitControllerVisualsKeys = {"ShowSLCOffset", "SpeedLimitChangedAlert", "UseVienna"}; - std::set visionTurnControlKeys = {}; + std::set visionTurnControlKeys = {"CurveSensitivity", "DisableVTSCSmoothing", "TurnAggressiveness"}; std::map toggles; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 589d373dd..e51084c0d 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -668,7 +668,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { if (is_cruise_set && cruiseAdjustment != 0) { float transition = qBound(0.0f, 4.0f * (cruiseAdjustment / setSpeed), 1.0f); QColor min = whiteColor(75); - QColor max = greenColor(); + QColor max = vtscControllingCurve ? redColor() : greenColor(); p.setPen(QPen(QColor::fromRgbF( min.redF() + transition * (max.redF() - min.redF()), @@ -1322,8 +1322,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { conditionalStatus = scene.conditional_status; showConditionalExperimentalStatusBar = scene.show_cem_status_bar; - bool disableSmoothing = scene.disable_smoothing_mtsc; + bool disableSmoothing = vtscControllingCurve ? scene.disable_smoothing_vtsc : scene.disable_smoothing_mtsc; cruiseAdjustment = disableSmoothing || !is_cruise_set ? fmax(setSpeed - scene.adjusted_cruise, 0) : fmax(0.25 * (setSpeed - scene.adjusted_cruise) + 0.75 * cruiseAdjustment - 1, 0); + vtscControllingCurve = scene.vtsc_controlling_curve; customColors = scene.custom_colors; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 8324542ca..963b22d77 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -237,6 +237,7 @@ private: bool turnSignalLeft; bool turnSignalRight; bool useViennaSLCSign; + bool vtscControllingCurve; float cruiseAdjustment; float distanceConversion; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 171674064..d5688c425 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -273,6 +273,7 @@ static void update_state(UIState *s) { scene.speed_limit_overridden_speed = frogpilotPlan.getSlcOverriddenSpeed(); scene.stopped_equivalence = frogpilotPlan.getStoppedEquivalenceFactor(); scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit(); + scene.vtsc_controlling_curve = frogpilotPlan.getVtscControllingCurve(); } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); @@ -348,6 +349,7 @@ void ui_update_frogpilot_params(UIState *s) { scene.random_events = custom_theme && params.getBool("RandomEvents"); scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing"); + scene.disable_smoothing_vtsc = params.getBool("VisionTurnControl") && params.getBool("DisableVTSCSmoothing"); scene.experimental_mode_via_screen = scene.longitudinal_control && params.getBool("ExperimentalModeActivation") && params.getBool("ExperimentalModeViaTap"); bool lane_detection = params.getBool("NudgelessLaneChange") && params.getInt("LaneDetectionWidth") != 0; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 879f71291..9f0bdcb1f 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -190,6 +190,7 @@ typedef struct UIScene { bool compass; bool conditional_experimental; bool disable_smoothing_mtsc; + bool disable_smoothing_vtsc; bool driver_camera; bool dynamic_path_width; bool enabled; @@ -243,6 +244,7 @@ typedef struct UIScene { bool use_kaofui_icons; bool use_si; bool use_vienna_slc_sign; + bool vtsc_controlling_curve; bool wake_up_screen; bool wheel_speed;