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

@@ -493,7 +493,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)) : "–";
 
   // Draw outer box + border to contain set speed and speed limit
   const int sign_margin = 12;
@@ -512,7 +512,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 (reverseCruise) {
+  if (is_cruise_set && cruiseAdjustment) {
+    float transition = qBound(0.0f, 4.0f * (cruiseAdjustment / setSpeed), 1.0f);
+    QColor min = whiteColor(75), max = redColor(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 (reverseCruise) {
     p.setPen(QPen(QColor(0, 150, 255), 6));
   } else {
     p.setPen(QPen(whiteColor(75), 6));
@@ -1085,6 +1094,7 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
   conditionalSpeed = scene.conditional_speed;
   conditionalSpeedLead = scene.conditional_speed_lead;
   conditionalStatus = scene.conditional_status;
+  cruiseAdjustment = fmax((0.1 * fmax(setSpeed - scene.adjusted_cruise, 0) + 0.9 * cruiseAdjustment) - 1, 0);
   customColors = scene.custom_colors;
   desiredFollow = scene.desired_follow;
   experimentalMode = scene.experimental_mode;

selfdrive/ui/qt/onroad.h

@@ -143,6 +143,7 @@ private:
   bool turnSignalRight;
   bool useSI;
   double maxAcceleration;
+  float cruiseAdjustment;
   float laneWidthLeft;
   float laneWidthRight;
   int cameraView;

selfdrive/ui/ui.cc

@@ -252,6 +252,7 @@ static void update_state(UIState *s) {
       scene.obstacle_distance_stock = frogpilotLongitudinalPlan.getSafeObstacleDistanceStock();
       scene.stopped_equivalence = frogpilotLongitudinalPlan.getStoppedEquivalenceFactor();
     }
+    scene.adjusted_cruise = frogpilotLongitudinalPlan.getAdjustedCruise();
   }
   if (sm.updated("liveLocationKalman")) {
     auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();

selfdrive/ui/ui.h

@@ -193,6 +193,7 @@ typedef struct UIScene {
   bool turn_signal_right;
   bool unlimited_road_ui_length;
   bool use_si;
+  float adjusted_cruise;
   float lane_line_width;
   float lane_width_left;
   float lane_width_right;