Map Turn Speed Control

Added toggle for "Map Turn Speed Control".

Credit goes to Pfeiferj!

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

selfdrive/ui/qt/onroad.cc

@@ -475,7 +475,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
 
   QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
   QString speedStr = QString::number(std::nearbyint(speed));
-  QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed)) : "–";
+  QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
 
   if (!showDriverCamera) {
     // Draw outer box + border to contain set speed and speed limit
@@ -495,7 +495,16 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
     int bottom_radius = has_eu_speed_limit ? 100 : 32;
 
     QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size);
-    if (scene.reverse_cruise) {
+    if (is_cruise_set && cruiseAdjustment) {
+      float transition = qBound(0.0f, 4.0f * (cruiseAdjustment / setSpeed), 1.0f);
+      QColor min = whiteColor(75), max = greenColor(75);
+
+      p.setPen(QPen(QColor::fromRgbF(
+        min.redF()   + transition * (max.redF()   - min.redF()),
+        min.greenF() + transition * (max.greenF() - min.greenF()),
+        min.blueF()  + transition * (max.blueF()  - min.blueF())
+      ), 6));
+    } else if (scene.reverse_cruise) {
       p.setPen(QPen(QColor(0, 150, 255), 6));
     } else {
       p.setPen(QPen(whiteColor(75), 6));
@@ -1106,6 +1115,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;
+  cruiseAdjustment = disableSmoothing ? fmax(setSpeed - scene.adjusted_cruise, 0) : fmax(0.25 * (setSpeed - scene.adjusted_cruise) + 0.75 * cruiseAdjustment - 1, 0);
+
   customColors = scene.custom_colors;
 
   experimentalMode = scene.experimental_mode;

selfdrive/ui/qt/onroad.h

@@ -136,6 +136,7 @@ private:
   bool turnSignalLeft;
   bool turnSignalRight;
 
+  float cruiseAdjustment;
   float distanceConversion;
   float laneWidthLeft;
   float laneWidthRight;
@@ -166,6 +167,8 @@ private:
 
   QTimer *animationTimer;
 
+  inline QColor greenColor(int alpha = 242) { return QColor(23, 134, 68, alpha); }
+
 protected:
   void paintGL() override;
   void initializeGL() override;

selfdrive/ui/ui.cc

@@ -255,6 +255,7 @@ static void update_state(UIState *s) {
       scene.obstacle_distance_stock = frogpilotPlan.getSafeObstacleDistanceStock();
       scene.stopped_equivalence = frogpilotPlan.getStoppedEquivalenceFactor();
     }
+    scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise();
   }
   if (sm.updated("liveLocationKalman")) {
     auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();
@@ -307,6 +308,7 @@ void ui_update_frogpilot_params(UIState *s) {
   scene.custom_signals = custom_theme ? params.getInt("CustomSignals") : 0;
   scene.holiday_themes = custom_theme && params.getBool("HolidayThemes");
 
+  scene.disable_smoothing_mtsc = params.getBool("DisableMTSCSmoothing");
   scene.driver_camera = params.getBool("DriverCamera");
   scene.experimental_mode_via_screen = params.getBool("ExperimentalModeViaScreen") && params.getBool("ExperimentalModeActivation");
 

selfdrive/ui/ui.h

@@ -178,6 +178,7 @@ typedef struct UIScene {
   bool blind_spot_path;
   bool blind_spot_right;
   bool conditional_experimental;
+  bool disable_smoothing_mtsc;
   bool driver_camera;
   bool dynamic_path_width;
   bool enabled;
@@ -199,6 +200,7 @@ typedef struct UIScene {
   bool unlimited_road_ui_length;
   bool use_si;
 
+  float adjusted_cruise;
   float lane_line_width;
   float lane_width_left;
   float lane_width_right;