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>

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -17,6 +17,8 @@ from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import Speed
 TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
 TRAFFIC_MODE_T_FOLLOW = [.50, 1.]
 
+TARGET_LAT_A = 1.9  # m/s^2
+
 class FrogPilotPlanner:
   def __init__(self, CP, params, params_memory):
     self.CP = CP
@@ -36,6 +38,7 @@ class FrogPilotPlanner:
     self.slc_target = 0
     self.stop_distance = 0
     self.v_cruise = 0
+    self.vtsc_target = 0
 
     self.accel_limits = [A_CRUISE_MIN, get_max_accel(0)]
 
@@ -44,8 +47,8 @@ class FrogPilotPlanner:
   def update(self, carState, controlsState, modelData, mpc, sm, v_cruise, v_ego):
     enabled = controlsState.enabled
 
-    # Use the stock deceleration profile to handle MTSC more precisely
-    v_cruise_changed = self.mtsc_target < v_cruise
+    # Use the stock deceleration profile to handle MTSC/VTSC more precisely
+    v_cruise_changed = (self.mtsc_target or self.vtsc_target) < v_cruise
 
     # Configure the deceleration profile
     if v_cruise_changed:
@@ -152,7 +155,28 @@ class FrogPilotPlanner:
     else:
       self.slc_target = v_cruise
 
-    targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff]
+    # Pfeiferj's Vision Turn Controller
+    if self.vision_turn_controller and v_ego > CRUISING_SPEED and enabled:
+      # Set the curve sensitivity
+      orientation_rate = np.array(np.abs(modelData.orientationRate.z)) * self.curve_sensitivity
+      velocity = np.array(modelData.velocity.x)
+
+      # Get the maximum lat accel from the model
+      max_pred_lat_acc = np.amax(orientation_rate * velocity)
+
+      # Get the maximum curve based on the current velocity
+      max_curve = max_pred_lat_acc / (v_ego**2)
+
+      # Set the target lateral acceleration
+      adjusted_target_lat_a = TARGET_LAT_A * self.turn_aggressiveness
+
+      # Get the target velocity for the maximum curve
+      self.vtsc_target = (adjusted_target_lat_a / max_curve)**0.5
+      self.vtsc_target = np.clip(self.vtsc_target, CRUISING_SPEED, v_cruise)
+    else:
+      self.vtsc_target = v_cruise
+
+    targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff, self.vtsc_target]
     filtered_targets = [target for target in targets if target > CRUISING_SPEED]
 
     return min(filtered_targets) if filtered_targets else v_cruise
@@ -162,7 +186,7 @@ class FrogPilotPlanner:
     frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
     frogpilotPlan = frogpilot_plan_send.frogpilotPlan
 
-    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 = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor
@@ -181,6 +205,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, params):
@@ -229,3 +255,8 @@ class FrogPilotPlanner:
     if self.speed_limit_controller:
       self.speed_limit_confirmation = params.get_bool("SLCConfirmation")
       self.speed_limit_controller_override = params.get_int("SLCOverride")
+
+    self.vision_turn_controller = params.get_bool("VisionTurnControl")
+    if self.vision_turn_controller:
+      self.curve_sensitivity = params.get_int("CurveSensitivity") / 100
+      self.turn_aggressiveness = params.get_int("TurnAggressiveness") / 100

selfdrive/frogpilot/ui/control_settings.cc

@@ -85,6 +85,11 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"UseVienna", "Use Vienna Speed Limit Signs", "Use the Vienna (EU) speed limit style signs as opposed to MUTCD (US).", ""},
 
     {"TurnDesires", "Use Turn Desires", "Use turn desires for enhanced precision in turns below the minimum lane change speed.", "../assets/navigation/direction_continue_right.png"},
+
+    {"VisionTurnControl", "Vision Turn Speed Controller", "Slow down for detected road curvature for smoother curve handling.", "../frogpilot/assets/toggle_icons/icon_vtc.png"},
+    {"DisableVTSCSmoothing", "Disable VTSC UI Smoothing", "Disables the smoothing for the requested speed in the onroad UI.", ""},
+    {"CurveSensitivity", "Curve Detection Sensitivity", "Set curve detection sensitivity. Higher values prompt earlier responses, lower values lead to smoother but later reactions.", ""},
+    {"TurnAggressiveness", "Turn Speed Aggressiveness", "Set turn speed aggressiveness. Higher values result in faster turns, lower values yield gentler turns.", ""},
   };
 
   for (const auto &[param, title, desc, icon] : controlToggles) {
@@ -451,6 +456,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]() {
+        parentToggleClicked();
+        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);
     }

selfdrive/frogpilot/ui/control_settings.h

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

selfdrive/ui/qt/onroad.cc

@@ -650,7 +650,8 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
     QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size);
     if (is_cruise_set && cruiseAdjustment) {
       float transition = qBound(0.0f, 4.0f * (cruiseAdjustment / setSpeed), 1.0f);
-      QColor min = whiteColor(75), max = greenColor(75);
+      QColor min = whiteColor(75);
+      QColor max = vtscControllingCurve ? redColor(75) : greenColor(75);
 
       p.setPen(QPen(QColor::fromRgbF(
         min.redF()   + transition * (max.redF()   - min.redF()),
@@ -1320,8 +1321,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
   conditionalSpeedLead = scene.conditional_speed_lead;
   conditionalStatus = scene.conditional_status;
 
-  bool disableSmoothing = scene.vtsc_controlling_curve ? scene.disable_smoothing_vtsc : scene.disable_smoothing_mtsc;
+  bool disableSmoothing = vtscControllingCurve ? scene.disable_smoothing_vtsc : scene.disable_smoothing_mtsc;
   cruiseAdjustment = disableSmoothing ? 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

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

selfdrive/ui/ui.cc

@@ -280,6 +280,7 @@ static void update_state(UIState *s) {
       scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit();
     }
     scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise();
+    scene.vtsc_controlling_curve = frogpilotPlan.getVtscControllingCurve();
   }
   if (sm.updated("liveLocationKalman")) {
     auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();
@@ -345,6 +346,7 @@ void ui_update_frogpilot_params(UIState *s) {
   scene.holiday_themes = custom_theme && params.getBool("HolidayThemes");
 
   scene.disable_smoothing_mtsc = params.getBool("DisableMTSCSmoothing");
+  scene.disable_smoothing_vtsc = params.getBool("DisableVTSCSmoothing");
   scene.driver_camera = params.getBool("DriverCamera");
   scene.experimental_mode_via_screen = params.getBool("ExperimentalModeViaScreen") && params.getBool("ExperimentalModeActivation");
   scene.fahrenheit = params.getBool("Fahrenheit");

selfdrive/ui/ui.h

@@ -189,6 +189,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;
@@ -237,6 +238,7 @@ typedef struct UIScene {
   bool unlimited_road_ui_length;
   bool use_si;
   bool use_vienna_slc_sign;
+  bool vtsc_controlling_curve;
   bool wheel_speed;
 
   float acceleration;