diff --git a/cereal/custom.capnp b/cereal/custom.capnp
index 9a2a576e99..37346c8a2a 100644
--- a/cereal/custom.capnp
+++ b/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;
diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py
index f4f71f4e92..a13c873fbe 100755
--- a/selfdrive/selfdrived/selfdrived.py
+++ b/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)
diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc
index e19760f2e1..ca58282a3d 100644
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc
+++ b/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.")) + "
" + tr("It")+ " " + tr("does not") + " " + tr("affect driving in any way.") + "",
- "../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,
},
};
diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc
index 71bdd92cab..991710bbfa 100644
--- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc
+++ b/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(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);
+}
diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h
index d76eb06060..073780bc10 100644
--- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h
+++ b/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;
};
diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc
index 7b582a8341..9acf844088 100644
--- a/selfdrive/ui/sunnypilot/ui.cc
+++ b/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) {
diff --git a/selfdrive/ui/sunnypilot/ui_scene.h b/selfdrive/ui/sunnypilot/ui_scene.h
index 69cebd6d79..768cc5d7a1 100644
--- a/selfdrive/ui/sunnypilot/ui_scene.h
+++ b/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;
diff --git a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py
index 7ee7f3ea64..468e6f55b5 100644
--- a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py
+++ b/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()
diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
index b4ebf1bd56..14cb4f21d0 100644
--- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
+++ b/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