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

@@ -29,6 +29,8 @@ A_CRUISE_MAX_VALS_ECO =        [1.4, 1.2, 1.0, 0.8, 0.6, 0.4, 0.2]
 A_CRUISE_MAX_VALS_SPORT =      [3.0, 2.5, 2.0, 1.0, 0.9, 0.8, 0.6]
 A_CRUISE_MAX_VALS_SPORT_PLUS = [4.0, 3.5, 3.0, 1.0, 0.9, 0.8, 0.6]
 
+TARGET_LAT_A = 1.9
+
 TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
 
 def get_max_accel_eco(v_ego):
@@ -65,6 +67,7 @@ class FrogPilotPlanner:
     self.speed_jerk = 0
     self.tracked_model_length = 0
     self.v_cruise = 0
+    self.vtsc_target = 0
 
     self.tracking_lead_mac = MovingAverageCalculator()
 
@@ -145,7 +148,7 @@ class FrogPilotPlanner:
 
     if controlsState.experimentalMode:
       self.min_accel = ACCEL_MIN
-    elif self.mtsc_target < v_cruise:
+    elif min(self.mtsc_target, self.vtsc_target) < v_cruise:
       self.min_accel = A_CRUISE_MIN
     elif frogpilot_toggles.map_deceleration and (eco_gear or sport_gear):
       if eco_gear:
@@ -264,6 +267,16 @@ class FrogPilotPlanner:
     else:
       self.slc_target = 0
 
+    # 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 = clip(self.vtsc_target, CRUISING_SPEED, v_cruise)
+    else:
+      self.vtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
+
     if frogpilot_toggles.force_standstill and carState.standstill and not self.override_force_stop and controlsState.enabled:
       self.forcing_stop = True
       self.v_cruise = -1
@@ -283,7 +296,7 @@ class FrogPilotPlanner:
       self.forcing_stop = False
       self.tracked_model_length = 0
 
-      targets = [self.mtsc_target, self.slc_target - v_ego_diff]
+      targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff, self.vtsc_target]
       self.v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets]))
 
   def publish(self, sm, pm, frogpilot_toggles):
@@ -298,7 +311,8 @@ class FrogPilotPlanner:
     frogpilotPlan.speedJerkStock = float(J_EGO_COST * 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.vtscControllingCurve = bool(self.mtsc_target > self.vtsc_target)
 
     frogpilotPlan.conditionalExperimentalActive = self.cem.experimental_mode
 

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -118,7 +118,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
   if (is_cruise_set && cruiseAdjustment != 0) {
     float transition = qBound(0.0f, 5.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()),
@@ -551,8 +551,9 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce
   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/annotated_camera.h

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

selfdrive/ui/ui.cc

@@ -243,6 +243,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();
@@ -294,6 +295,7 @@ void ui_update_frogpilot_params(UIState *s, Params &params) {
   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

@@ -125,6 +125,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;
@@ -149,6 +150,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;