Merge remote-tracking branch 'sunnypilot/sunnypilot/master' into feature/slc

# Conflicts:
#	cereal/custom.capnp
#	selfdrive/selfdrived/selfdrived.py
#	sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

cereal/custom.capnp

@@ -146,7 +146,9 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   longitudinalPlanSource @1 :LongitudinalPlanSource;
   smartCruiseControl @2 :SmartCruiseControl;
   speedLimit @3 :SpeedLimit;
-  events @4 :List(OnroadEventSP.Event);
+  vTarget @4 :Float32;
+  aTarget @5 :Float32;
+  events @6 :List(OnroadEventSP.Event);
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;

selfdrive/selfdrived/selfdrived.py

@@ -95,8 +95,9 @@ class SelfdriveD(CruiseHelper):
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
-                                   'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback'] + \
-                                   self.camera_packets + self.sensor_packets + self.gps_packets + ['modelDataV2SP', 'longitudinalPlanSP'],
+                                   'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
+                                   'modelDataV2SP', 'longitudinalPlanSP'] + \
+                                   self.camera_packets + self.sensor_packets + self.gps_packets,
                                   ignore_alive=ignore, ignore_avg_freq=ignore,
                                   ignore_valid=ignore, frequency=int(1/DT_CTRL))
 
@@ -444,7 +445,7 @@ class SelfdriveD(CruiseHelper):
           self.events.add(EventName.personalityChanged)
         self.experimental_mode_switched = False
 
-    self.icbm.run(CS, self.sm['carControl'], self.is_metric)
+    self.icbm.run(CS, self.sm['carControl'], self.sm['longitudinalPlanSP'], self.is_metric)
 
   def data_sample(self):
     _car_state = messaging.recv_one(self.car_state_sock)

selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc

@@ -25,21 +25,28 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
       "BlindSpot",
       tr("Show Blind Spot Warnings"),
       tr("Enabling this will display warnings when a vehicle is detected in your blind spot as long as your car has BSM supported."),
-      "../assets/offroad/icon_monitoring.png",
+      "",
       false,
     },
     {
       "RainbowMode",
       tr("Enable Tesla Rainbow Mode"),
       RainbowizeWords(tr("A beautiful rainbow effect on the path the model wants to take.")) + "<br/><i>" + tr("It")+ " <b>" + tr("does not") + "</b> " + tr("affect driving in any way.") + "</i>",
-      "../assets/offroad/icon_monitoring.png",
+      "",
       false,
     },
     {
       "StandstillTimer",
       tr("Enable Standstill Timer"),
       tr("Show a timer on the HUD when the car is at a standstill."),
-      "../assets/offroad/icon_monitoring.png",
+      "",
+      false,
+    },
+    {
+      "RoadName",
+      tr("Display Road Name"),
+      tr("Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name."),
+      "",
       false,
     },
   };

selfdrive/ui/sunnypilot/qt/onroad/hud.cc

@@ -32,7 +32,9 @@ void HudRendererSP::updateState(const UIState &s) {
   speedLimit = lp_sp.getSpeedLimit().getResolver().getSpeedLimit() * speedConv;
   speedLimitOffset = lp_sp.getSpeedLimit().getResolver().getSpeedLimitOffset() * speedConv;
   speedLimitMode = static_cast<SpeedLimitMode>(s.scene.speed_limit_mode);
+  roadName = s.scene.road_name;
   if (sm.updated("liveMapDataSP")) {
+    roadNameStr = QString::fromStdString(lmd.getRoadName());
     speedLimitAheadValid = lmd.getSpeedLimitAheadValid();
     speedLimitAhead = lmd.getSpeedLimitAhead() * speedConv;
     speedLimitAheadDistance = lmd.getSpeedLimitAheadDistance();
@@ -138,6 +140,9 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
       drawSpeedLimitSigns(p);
       drawUpcomingSpeedLimit(p);
     }
+
+    // Road Name
+    drawRoadName(p, surface_rect);
   }
 }
 
@@ -516,3 +521,33 @@ void HudRendererSP::drawUpcomingSpeedLimit(QPainter &p) {
   p.setPen(QColor(180, 180, 180, 255));
   p.drawText(ahead_rect.adjusted(0, 110, 0, 0), Qt::AlignTop | Qt::AlignHCenter, distanceStr);
 }
+
+void HudRendererSP::drawRoadName(QPainter &p, const QRect &surface_rect) {
+  if (!roadName || roadNameStr.isEmpty()) return;
+
+  // Measure text to size container
+  p.setFont(InterFont(40, QFont::Normal));
+  QFontMetrics fm(p.font());
+
+  int text_width = fm.horizontalAdvance(roadNameStr);
+  int padding = 40;
+  int rect_width = text_width + padding;
+
+  // Constrain to reasonable bounds
+  int min_width = 200;
+  int max_width = surface_rect.width() - 40;
+  rect_width = std::max(min_width, std::min(rect_width, max_width));
+
+  // Center at top of screen
+  QRect road_rect(surface_rect.width() / 2 - rect_width / 2, -6, rect_width, 60);
+
+  p.setPen(Qt::NoPen);
+  p.setBrush(QColor(0, 0, 0, 120));
+  p.drawRoundedRect(road_rect, 12, 12);
+
+  p.setPen(QColor(255, 255, 255, 200));
+
+  // Truncate if still too long
+  QString truncated = fm.elidedText(roadNameStr, Qt::ElideRight, road_rect.width() - 20);
+  p.drawText(road_rect, Qt::AlignCenter, truncated);
+}

selfdrive/ui/sunnypilot/qt/onroad/hud.h

@@ -31,6 +31,7 @@ private:
   void drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name);
   void drawSpeedLimitSigns(QPainter &p);
   void drawUpcomingSpeedLimit(QPainter &p);
+  void drawRoadName(QPainter &p, const QRect &surface_rect);
 
   bool lead_status;
   float lead_d_rel;
@@ -74,4 +75,6 @@ private:
   float speedLimitAheadDistancePrev;
   int speedLimitAheadValidFrame;
   SpeedLimitMode speedLimitMode = SpeedLimitMode::OFF;
+  bool roadName;
+  QString roadNameStr;
 };

selfdrive/ui/sunnypilot/ui.cc

@@ -54,6 +54,7 @@ void ui_update_params_sp(UIStateSP *s) {
   s->scene.dev_ui_info = std::atoi(params.get("DevUIInfo").c_str());
   s->scene.standstill_timer = params.getBool("StandstillTimer");
   s->scene.speed_limit_mode = std::atoi(params.get("SpeedLimitMode").c_str());
+  s->scene.road_name = params.getBool("RoadName");
 }
 
 DeviceSP::DeviceSP(QObject *parent) : Device(parent) {

selfdrive/ui/sunnypilot/ui_scene.h

@@ -11,4 +11,5 @@ typedef struct UISceneSP : UIScene {
   int dev_ui_info = 0;
   bool standstill_timer = false;
   int speed_limit_mode = 0;
+  bool road_name = false;
 } UISceneSP;

sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py

@@ -49,19 +49,11 @@ class IntelligentCruiseButtonManagement:
   def v_cruise_equal(self) -> bool:
     return self.v_target == self.v_cruise_cluster
 
-  def update_calculations(self, CS: car.CarState) -> None:
+  def update_calculations(self, CS: car.CarState, LP_SP: custom.LongitudinalPlanSP) -> None:
     speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
     ms_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
-    v_cruise_ms = CS.vCruise * CV.KPH_TO_MS
 
-    # all targets in m/s
-    v_targets = {
-      LongitudinalPlanSource.cruise: v_cruise_ms
-    }
-    source = min(v_targets, key=lambda k: v_targets[k])
-    v_target_ms = v_targets[source]
-
-    self.v_target_ms_last = apply_hysteresis(v_target_ms, self.v_target_ms_last, HYST_GAP * ms_conv)
+    self.v_target_ms_last = apply_hysteresis(LP_SP.vTarget, self.v_target_ms_last, HYST_GAP * ms_conv)
 
     self.v_target = round(self.v_target_ms_last * speed_conv)
     self.v_cruise_min = get_minimum_set_speed(self.is_metric)
@@ -123,13 +115,13 @@ class IntelligentCruiseButtonManagement:
 
     self.is_ready = ready and not button_pressed
 
-  def run(self, CS: car.CarState, CC: car.CarControl, is_metric: bool) -> None:
+  def run(self, CS: car.CarState, CC: car.CarControl, LP_SP: custom.LongitudinalPlanSP, is_metric: bool) -> None:
     if self.CP_SP.pcmCruiseSpeed:
       return
 
     self.is_metric = is_metric
 
-    self.update_calculations(CS)
+    self.update_calculations(CS, LP_SP)
     self.update_readiness(CS, CC)
 
     self.cruise_button = self.update_state_machine()

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -31,6 +31,9 @@ class LongitudinalPlannerSP:
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
     self.source = LongitudinalPlanSource.cruise
 
+    self.output_v_target = 0.
+    self.output_a_target = 0.
+
   @property
   def mlsim(self) -> bool:
     # If we don't have a generation set, we assume it's default model. Which as of today are mlsim.
@@ -68,9 +71,8 @@ class LongitudinalPlannerSP:
     }
 
     self.source = min(targets, key=lambda k: targets[k][0])
-    v_target, a_target = targets[self.source]
-
-    return v_target, a_target
+    self.output_v_target, self.output_a_target = targets[self.source]
+    return self.output_v_target, self.output_a_target
 
   def update(self, sm: messaging.SubMaster) -> None:
     self.dec.update(sm)
@@ -82,6 +84,8 @@ class LongitudinalPlannerSP:
 
     longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
     longitudinalPlanSP.longitudinalPlanSource = self.source
+    longitudinalPlanSP.vTarget = float(self.output_v_target)
+    longitudinalPlanSP.aTarget = float(self.output_a_target)
     longitudinalPlanSP.events = self.events_sp.to_msg()
 
     # Dynamic Experimental Control