Vision Turn Speed Controller

Added toggles for "Vision Turn Speed Control" along with aggressiveness for the speed and sensitivity for the curve itself.

Credit goes to Pfeiferj!

https: //github.com/pfeiferj
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>

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 {

common/params.cc

@@ -251,6 +251,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CrosstrekTorque", PERSISTENT},
     {"CurrentHolidayTheme", PERSISTENT},
     {"CurrentRandomEvent", PERSISTENT},
+    {"CurveSensitivity", PERSISTENT},
     {"CustomAlerts", PERSISTENT},
     {"CustomColors", PERSISTENT},
     {"CustomCruise", PERSISTENT},
@@ -270,6 +271,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"DisableMTSCSmoothing", PERSISTENT},
     {"DisableOnroadUploads", PERSISTENT},
     {"DisableOpenpilotLongitudinal", PERSISTENT},
+    {"DisableVTSCSmoothing", PERSISTENT},
     {"DisengageVolume", PERSISTENT},
     {"DistanceLongPressed", PERSISTENT},
     {"DragonPilotTune", PERSISTENT},
@@ -438,12 +440,14 @@ std::unordered_map<std::string, uint32_t> 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},

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

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<int, QString>(), this, false, "%");
+
     } else {
       toggle = new ParamControl(param, title, desc, icon, this);
     }
@@ -1006,7 +1023,7 @@ void FrogPilotControlsPanel::hideToggles() {
   trafficProfile->setVisible(false);
 
   std::set<QString> longitudinalKeys = {"ConditionalExperimental", "CustomPersonalities", "ExperimentalModeActivation",
-                                        "LongitudinalTune", "MTSCEnabled", "SpeedLimitController"};
+                                        "LongitudinalTune", "MTSCEnabled", "SpeedLimitController", "VisionTurnControl"};
 
   for (auto &[key, toggle] : toggles) {
     toggle->setVisible(false);

selfdrive/frogpilot/ui/qt/offroad/control_settings.h

@@ -54,7 +54,7 @@ private:
   std::set<QString> speedLimitControllerControlsKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority"};
   std::set<QString> speedLimitControllerQOLKeys = {"ForceMPHDashboard", "SetSpeedLimit", "SLCConfirmation", "SLCLookaheadHigher", "SLCLookaheadLower"};
   std::set<QString> speedLimitControllerVisualsKeys = {"ShowSLCOffset", "SpeedLimitChangedAlert", "UseVienna"};
-  std::set<QString> visionTurnControlKeys = {};
+  std::set<QString> visionTurnControlKeys = {"CurveSensitivity", "DisableVTSCSmoothing", "TurnAggressiveness"};
 
   std::map<std::string, ParamControl*> toggles;
 

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;
 

selfdrive/ui/qt/onroad.h

@@ -237,6 +237,7 @@ private:
   bool turnSignalLeft;
   bool turnSignalRight;
   bool useViennaSLCSign;
+  bool vtscControllingCurve;
 
   float cruiseAdjustment;
   float distanceConversion;

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;

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;