Controls - Vision Turn Speed Controller

Slow down for detected curves in the road.

Credit goes to Pfeiferj!

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

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -36,6 +36,8 @@ A_CRUISE_MAX_VALS_SPORT = [3.5, 3.5, 3.3, 2.8, 1.5, 1.0, 0.75, 0.65, 0.6]
 
 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)
 
@@ -66,6 +68,7 @@ class FrogPilotPlanner:
     self.road_curvature = 0
     self.slc_target = 0
     self.speed_jerk = 0
+    self.vtsc_target = 0
 
   def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, radarState, frogpilot_toggles):
     if frogpilot_toggles.radarless_model:
@@ -80,7 +83,7 @@ class FrogPilotPlanner:
 
     v_cruise_kph = min(controlsState.vCruise, V_CRUISE_UNSET)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
-    v_cruise_changed = self.mtsc_target < v_cruise
+    v_cruise_changed = (self.mtsc_target or self.vtsc_target) < v_cruise
 
     v_ego = max(carState.vEgo, 0)
     v_lead = self.lead_one.vLead
@@ -233,7 +236,17 @@ class FrogPilotPlanner:
     else:
       self.slc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
 
-    targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff]
+    # Pfeiferj's Vision Turn Controller
+    if frogpilot_toggles.vision_turn_controller and v_ego > CRUISING_SPEED and controlsState.enabled:
+      adjusted_road_curvature = self.road_curvature * frogpilot_toggles.curve_sensitivity
+      adjusted_target_lat_a = TARGET_LAT_A * frogpilot_toggles.turn_aggressiveness
+
+      self.vtsc_target = (adjusted_target_lat_a / adjusted_road_curvature)**0.5
+      self.vtsc_target = np.clip(self.vtsc_target, CRUISING_SPEED, v_cruise)
+    else:
+      self.vtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
+
+    targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff, self.vtsc_target]
     filtered_targets = [target if target > CRUISING_SPEED else v_cruise for target in targets]
 
     return min(filtered_targets)
@@ -249,7 +262,7 @@ class FrogPilotPlanner:
     frogpilotPlan.speedJerkStock = J_EGO_COST * float(self.base_speed_jerk)
     frogpilotPlan.tFollow = float(self.t_follow)
 
-    frogpilotPlan.adjustedCruise = float(self.mtsc_target * (CV.MS_TO_KPH if frogpilot_toggles.is_metric else CV.MS_TO_MPH))
+    frogpilotPlan.adjustedCruise = float(min(self.mtsc_target, self.vtsc_target) * (CV.MS_TO_KPH if frogpilot_toggles.is_metric else CV.MS_TO_MPH))
 
     frogpilotPlan.conditionalExperimental = self.cem.experimental_mode
 
@@ -264,4 +277,6 @@ class FrogPilotPlanner:
 
     frogpilotPlan.vCruise = float(self.v_cruise)
 
+    frogpilotPlan.vtscControllingCurve = bool(self.mtsc_target > self.vtsc_target)
+
     pm.send('frogpilotPlan', frogpilot_plan_send)

selfdrive/ui/qt/onroad.cc

@@ -443,7 +443,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()),
@@ -865,8 +865,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;
 
   experimentalMode = scene.experimental_mode;
 

selfdrive/ui/qt/onroad.h

@@ -158,6 +158,7 @@ private:
   bool speedLimitController;
   bool trafficModeActive;
   bool useViennaSLCSign;
+  bool vtscControllingCurve;
 
   float accelerationConversion;
   float cruiseAdjustment;

selfdrive/ui/ui.cc

@@ -244,6 +244,7 @@ static void update_state(UIState *s) {
     scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden();
     scene.speed_limit_overridden_speed = frogpilotPlan.getSlcOverriddenSpeed();
     scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit();
+    scene.vtsc_controlling_curve = frogpilotPlan.getVtscControllingCurve();
   }
   if (sm.updated("liveLocationKalman")) {
     auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();
@@ -284,6 +285,7 @@ void ui_update_frogpilot_params(UIState *s) {
   scene.show_cem_status_bar = scene.conditional_experimental && !params.getBool("HideCEMStatusBar");
 
   scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing");
+  scene.disable_smoothing_vtsc = params.getBool("VisionTurnControl") && params.getBool("DisableVTSCSmoothing");
 
   bool driving_personalities = scene.longitudinal_control && params.getBool("DrivingPersonalities");
   scene.onroad_distance_button = driving_personalities && params.getBool("OnroadDistanceButton");

selfdrive/ui/ui.h

@@ -189,6 +189,7 @@ typedef struct UIScene {
   bool always_on_lateral_active;
   bool conditional_experimental;
   bool disable_smoothing_mtsc;
+  bool disable_smoothing_vtsc;
   bool enabled;
   bool experimental_mode;
   bool experimental_mode_via_screen;
@@ -211,6 +212,7 @@ typedef struct UIScene {
   bool traffic_mode_active;
   bool use_kaofui_icons;
   bool use_vienna_slc_sign;
+  bool vtsc_controlling_curve;
 
   float adjusted_cruise;
   float lead_detection_threshold;