From e5f1f86ac2d19550dcd6238635811a0a2efa09b7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 13 Oct 2025 19:37:58 -0400 Subject: [PATCH 1/7] params: helper to clamp out-of-range int params (#1373) * params: helpers to clamp out-of-range values * lint * inline * fix access * actually fix the param * inherit them * more lint --- selfdrive/selfdrived/selfdrived.py | 8 ++++++- sunnypilot/__init__.py | 21 +++++++++++++++++++ .../controls/lib/speed_limit/common.py | 9 ++++---- .../lib/speed_limit/speed_limit_resolver.py | 15 +++++++++++-- 4 files changed, 46 insertions(+), 7 deletions(-) diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 4bc75021f7..0a4ea749e4 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -24,6 +24,7 @@ from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroa from openpilot.system.version import get_build_metadata from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem +from openpilot.sunnypilot import get_sanitize_int_param from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement @@ -130,7 +131,12 @@ class SelfdriveD(CruiseHelper): self.logged_comm_issue = None self.not_running_prev = None self.experimental_mode = False - self.personality = self.params.get("LongitudinalPersonality", return_default=True) + self.personality = get_sanitize_int_param( + "LongitudinalPersonality", + min(log.LongitudinalPersonality.schema.enumerants.values()), + max(log.LongitudinalPersonality.schema.enumerants.values()), + self.params + ) self.recalibrating_seen = False self.state_machine = StateMachine() self.rk = Ratekeeper(100, print_delay_threshold=None) diff --git a/sunnypilot/__init__.py b/sunnypilot/__init__.py index dd9870597d..ccacd0be0a 100644 --- a/sunnypilot/__init__.py +++ b/sunnypilot/__init__.py @@ -5,6 +5,7 @@ This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ +from enum import IntEnum import hashlib PARAMS_UPDATE_PERIOD = 3 # seconds @@ -16,3 +17,23 @@ def get_file_hash(path: str) -> str: for byte_block in iter(lambda: f.read(4096), b""): sha256_hash.update(byte_block) return sha256_hash.hexdigest() + + +class IntEnumBase(IntEnum): + @classmethod + def min(cls): + return min(cls) + + @classmethod + def max(cls): + return max(cls) + + +def get_sanitize_int_param(key: str, min_val: int, max_val: int, params) -> int: + val: int = params.get(key, return_default=True) + clipped_val = max(min_val, min(max_val, val)) + + if clipped_val != val: + params.put(key, clipped_val) + + return clipped_val diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/common.py b/sunnypilot/selfdrive/controls/lib/speed_limit/common.py index baf9328032..c46768464e 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/common.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/common.py @@ -4,10 +4,11 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ -from enum import IntEnum + +from openpilot.sunnypilot import IntEnumBase -class Policy(IntEnum): +class Policy(IntEnumBase): car_state_only = 0 map_data_only = 1 car_state_priority = 2 @@ -15,13 +16,13 @@ class Policy(IntEnum): combined = 4 -class OffsetType(IntEnum): +class OffsetType(IntEnumBase): off = 0 fixed = 1 percentage = 2 -class Mode(IntEnum): +class Mode(IntEnumBase): off = 0 information = 1 warning = 2 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py index 51fb8f6d64..459334d156 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py @@ -12,7 +12,7 @@ from openpilot.common.constants import CV from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL -from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD, get_sanitize_int_param from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy, OffsetType @@ -42,6 +42,12 @@ class SpeedLimitResolver: self.distance_solutions = {} # Store for distance to current speed limit start for different sources self.policy = self.params.get("SpeedLimitPolicy", return_default=True) + self.policy = get_sanitize_int_param( + "SpeedLimitPolicy", + Policy.min().value, + Policy.max().value, + self.params + ) self._policy_to_sources_map = { Policy.car_state_only: [SpeedLimitSource.car], Policy.map_data_only: [SpeedLimitSource.map], @@ -54,7 +60,12 @@ class SpeedLimitResolver: self._reset_limit_sources(source) self.is_metric = self.params.get_bool("IsMetric") - self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True) + self.offset_type = get_sanitize_int_param( + "SpeedLimitOffsetType", + OffsetType.min().value, + OffsetType.max().value, + self.params + ) self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True) self.speed_limit = 0. From 285fd9760698673d99a51cf407d480d2bc4e0531 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 13 Oct 2025 20:36:24 -0400 Subject: [PATCH 2/7] ui: only draw speedCluster speed over "MAX" when ICBM is enabled (#1374) --- selfdrive/ui/sunnypilot/qt/onroad/hud.cc | 4 +++- selfdrive/ui/sunnypilot/qt/onroad/hud.h | 1 + selfdrive/ui/sunnypilot/ui.cc | 2 +- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index 3ed8feb993..8c913cec17 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -35,6 +35,7 @@ void HudRendererSP::updateState(const UIState &s) { const auto gpsLocation = is_gps_location_external ? sm["gpsLocationExternal"].getGpsLocationExternal() : sm["gpsLocation"].getGpsLocation(); const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters(); const auto car_params = sm["carParams"].getCarParams(); + const auto car_params_sp = sm["carParamsSP"].getCarParamsSP(); const auto lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP(); const auto lmd = sm["liveMapDataSP"].getLiveMapDataSP(); @@ -130,6 +131,7 @@ void HudRendererSP::updateState(const UIState &s) { carControlEnabled = car_control.getEnabled(); speedCluster = car_state.getCruiseState().getSpeedCluster() * speedConv; + pcmCruiseSpeed = car_params_sp.getPcmCruiseSpeed(); } void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { @@ -689,7 +691,7 @@ void HudRendererSP::drawSetSpeedSP(QPainter &p, const QRect &surface_rect) { } // Draw "MAX" or carState.cruiseState.speedCluster (when ICBM is active) text - if (carControlEnabled) { + if (!pcmCruiseSpeed && carControlEnabled) { if (std::nearbyint(set_speed) != std::nearbyint(speedCluster)) { icbm_active_counter = 3 * UI_FREQ; } else if (icbm_active_counter > 0) { diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index 554c9c5d0d..bc7ee81de3 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -120,4 +120,5 @@ private: bool carControlEnabled; float speedCluster = 0; int icbm_active_counter = 0; + bool pcmCruiseSpeed; }; diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc index ab1ca0e6a6..df7b3ce5ca 100644 --- a/selfdrive/ui/sunnypilot/ui.cc +++ b/selfdrive/ui/sunnypilot/ui.cc @@ -29,7 +29,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) { "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan", "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP", "carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters", - "carStateSP", "liveParameters", "liveMapDataSP" + "carStateSP", "liveParameters", "liveMapDataSP", "carParamsSP" }); // update timer From 39e73cc46ea666787ff49bb1926d9d5625c1ae29 Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Mon, 13 Oct 2025 18:45:59 -0700 Subject: [PATCH 3/7] ui: add ModelRendererSP::draw (#1372) * ModelRendererSP::draw * match * less * huh? * unused --------- Co-authored-by: Jason Wen --- selfdrive/ui/qt/onroad/model.cc | 3 +- selfdrive/ui/qt/onroad/model.h | 4 - selfdrive/ui/sunnypilot/qt/onroad/model.cc | 143 ++++++++++++--------- selfdrive/ui/sunnypilot/qt/onroad/model.h | 14 +- 4 files changed, 88 insertions(+), 76 deletions(-) diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index a4165be216..176af56613 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/selfdrive/ui/qt/onroad/model.cc @@ -22,7 +22,7 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { update_model(model, lead_one); drawLaneLines(painter); - drawPath(painter, model, surface_rect); + drawPath(painter, model, surface_rect.height()); if (longitudinal_control && sm.alive("radarState")) { update_leads(radar_state, model.getPosition()); @@ -173,7 +173,6 @@ QColor ModelRenderer::blendColors(const QColor &start, const QColor &end, float (1 - t) * start.alphaF() + t * end.alphaF()); } - void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, const QRect &surface_rect) { const float speedBuff = 10.; diff --git a/selfdrive/ui/qt/onroad/model.h b/selfdrive/ui/qt/onroad/model.h index 3ef8ba5320..0a58d345c4 100644 --- a/selfdrive/ui/qt/onroad/model.h +++ b/selfdrive/ui/qt/onroad/model.h @@ -39,9 +39,6 @@ protected: virtual void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead); void drawLaneLines(QPainter &painter); void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height); - virtual void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, const QRect &surface_rect) {; - drawPath(painter, model, surface_rect.height()); - } void updatePathGradient(QLinearGradient &bg); QColor blendColors(const QColor &start, const QColor &end, float t); @@ -58,5 +55,4 @@ protected: QPointF lead_vertices[2] = {}; Eigen::Matrix3f car_space_transform = Eigen::Matrix3f::Zero(); QRectF clip_region; - }; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/model.cc b/selfdrive/ui/sunnypilot/qt/onroad/model.cc index 14943b1d48..086b703e10 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/model.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/model.cc @@ -21,74 +21,63 @@ void ModelRendererSP::update_model(const cereal::ModelDataV2::Reader &model, con mapLineToPolygon(model.getLaneLines()[2], 0.2, -0.05, &right_blindspot_vertices, max_idx_barrier); } -void ModelRendererSP::drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, const QRect &surface_rect) { +void ModelRendererSP::draw(QPainter &painter, const QRect &surface_rect) { + ModelRenderer::draw(painter, surface_rect); auto *s = uiState(); auto &sm = *(s->sm); - bool blindspot = s->scene.blindspot_ui; - if (blindspot) { - bool left_blindspot = sm["carState"].getCarState().getLeftBlindspot(); - bool right_blindspot = sm["carState"].getCarState().getRightBlindspot(); - - //painter.setBrush(QColor::fromRgbF(1.0, 0.0, 0.0, 0.4)); // Red with alpha for blind spot - - if (left_blindspot && !left_blindspot_vertices.isEmpty()) { - QLinearGradient gradient(0, 0, surface_rect.width(), 0); // Horizontal gradient from left to right - gradient.setColorAt(0.0, QColor(255, 165, 0, 102)); // Orange with alpha - gradient.setColorAt(1.0, QColor(255, 255, 0, 102)); // Yellow with alpha - painter.setBrush(gradient); - painter.drawPolygon(left_blindspot_vertices); - } - - if (right_blindspot && !right_blindspot_vertices.isEmpty()) { - QLinearGradient gradient(surface_rect.width(), 0, 0, 0); // Horizontal gradient from right to left - gradient.setColorAt(0.0, QColor(255, 165, 0, 102)); // Orange with alpha - gradient.setColorAt(1.0, QColor(255, 255, 0, 102)); // Yellow with alpha - painter.setBrush(gradient); - painter.drawPolygon(right_blindspot_vertices); - } + if (sm.rcv_frame("liveCalibration") < s->scene.started_frame || + sm.rcv_frame("modelV2") < s->scene.started_frame) { + return; } + painter.save(); + + const auto &model = sm["modelV2"].getModelV2(); + const auto &radar_state = sm["radarState"].getRadarState(); + const auto &lead_one = radar_state.getLeadOne(); + const auto &car_state = sm["carState"].getCarState(); + + update_model(model, lead_one); + drawLaneLines(painter); + + bool blindspot = s->scene.blindspot_ui; bool rainbow = s->scene.rainbow_mode; - //float v_ego = sm["carState"].getCarState().getVEgo(); + + bool left_blindspot = car_state.getLeftBlindspot(); + bool right_blindspot = car_state.getRightBlindspot(); + + if (blindspot) { + drawBlindspot(painter, surface_rect, left_blindspot, right_blindspot); + } if (rainbow) { - // Simple time-based animation - float time_offset = std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch()).count() / 1000.0f; - - // simple linear gradient from bottom to top - QLinearGradient bg(0, surface_rect.height(), 0, 0); - - // evenly spaced colors across the spectrum - // The animation shifts the entire spectrum smoothly - float animation_speed = 40.0f; // speed vroom vroom - float hue_offset = fmod(time_offset * animation_speed, 360.0f); - - // 6-8 color stops for smooth transitions more color makes it laggy - const int num_stops = 7; - for (int i = 0; i < num_stops; i++) { - float position = static_cast(i) / (num_stops - 1); - - float hue = fmod(hue_offset + position * 360.0f, 360.0f); - float saturation = 0.9f; - float lightness = 0.6f; - - // Alpha fades out towards the far end of the path - float alpha = 0.8f * (1.0f - position * 0.3f); - - QColor color = QColor::fromHslF(hue / 360.0f, saturation, lightness, alpha); - bg.setColorAt(position, color); - } - - painter.setBrush(bg); - painter.drawPolygon(track_vertices); + drawRainbowPath(painter, surface_rect); } else { - // Normal path rendering ModelRenderer::drawPath(painter, model, surface_rect.height()); } drawLeadStatus(painter, surface_rect.height(), surface_rect.width()); + + painter.restore(); +} + +void ModelRendererSP::drawBlindspot(QPainter &painter, const QRect &surface_rect, bool left_blindspot, bool right_blindspot) { + if (left_blindspot && !left_blindspot_vertices.isEmpty()) { + QLinearGradient gradient(0, 0, surface_rect.width(), 0); // Horizontal gradient from left to right + gradient.setColorAt(0.0, QColor(255, 165, 0, 102)); // Orange with alpha + gradient.setColorAt(1.0, QColor(255, 255, 0, 102)); // Yellow with alpha + painter.setBrush(gradient); + painter.drawPolygon(left_blindspot_vertices); + } + + if (right_blindspot && !right_blindspot_vertices.isEmpty()) { + QLinearGradient gradient(surface_rect.width(), 0, 0, 0); // Horizontal gradient from right to left + gradient.setColorAt(0.0, QColor(255, 165, 0, 102)); // Orange with alpha + gradient.setColorAt(1.0, QColor(255, 255, 0, 102)); // Yellow with alpha + painter.setBrush(gradient); + painter.drawPolygon(right_blindspot_vertices); + } } void ModelRendererSP::drawLeadStatus(QPainter &painter, int height, int width) { @@ -121,19 +110,16 @@ void ModelRendererSP::drawLeadStatus(QPainter &painter, int height, int width) { } if (has_lead_one) { - drawLeadStatusAtPosition(painter, lead_one, lead_vertices[0], height, width, "L1"); + drawLeadStatusPosition(painter, lead_one, lead_vertices[0], height, width); } if (has_lead_two && std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0) { - drawLeadStatusAtPosition(painter, lead_two, lead_vertices[1], height, width, "L2"); + drawLeadStatusPosition(painter, lead_two, lead_vertices[1], height, width); } } -void ModelRendererSP::drawLeadStatusAtPosition(QPainter &painter, - const cereal::RadarState::LeadData::Reader &lead_data, - const QPointF &chevron_pos, - int height, int width, - const QString &label) { +void ModelRendererSP::drawLeadStatusPosition(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, + const QPointF &chevron_pos, int height, int width) { float d_rel = lead_data.getDRel(); float v_rel = lead_data.getVRel(); auto *s = uiState(); @@ -223,3 +209,36 @@ void ModelRendererSP::drawLeadStatusAtPosition(QPainter &painter, painter.setPen(Qt::NoPen); } + +void ModelRendererSP::drawRainbowPath(QPainter &painter, const QRect &surface_rect) { + // Simple time-based animation + float time_offset = std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()).count() / 1000.0f; + + // simple linear gradient from bottom to top + QLinearGradient bg(0, surface_rect.height(), 0, 0); + + // evenly spaced colors across the spectrum + // The animation shifts the entire spectrum smoothly + float animation_speed = 40.0f; // speed vroom vroom + float hue_offset = fmod(time_offset * animation_speed, 360.0f); + + // 6-8 color stops for smooth transitions more color makes it laggy + const int num_stops = 7; + for (int i = 0; i < num_stops; i++) { + float position = static_cast(i) / (num_stops - 1); + + float hue = fmod(hue_offset + position * 360.0f, 360.0f); + float saturation = 0.9f; + float lightness = 0.6f; + + // Alpha fades out towards the far end of the path + float alpha = 0.8f * (1.0f - position * 0.3f); + + QColor color = QColor::fromHslF(hue / 360.0f, saturation, lightness, alpha); + bg.setColorAt(position, color); + } + + painter.setBrush(bg); + painter.drawPolygon(track_vertices); +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/model.h b/selfdrive/ui/sunnypilot/qt/onroad/model.h index 73999f0059..68068f2068 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/model.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/model.h @@ -13,17 +13,15 @@ class ModelRendererSP : public ModelRenderer { public: ModelRendererSP() = default; + void draw(QPainter &painter, const QRect &surface_rect); + private: void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead) override; - void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, const QRect &rect) override; - - // Lead status display methods void drawLeadStatus(QPainter &painter, int height, int width); - void drawLeadStatusAtPosition(QPainter &painter, - const cereal::RadarState::LeadData::Reader &lead_data, - const QPointF &chevron_pos, - int height, int width, - const QString &label); + void drawLeadStatusPosition(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, + const QPointF &chevron_pos, int height, int width); + void drawBlindspot(QPainter &painter, const QRect &surface_rect, bool left_blindspot, bool right_blindspot); + void drawRainbowPath(QPainter &painter, const QRect &surface_rect); QPolygonF left_blindspot_vertices; QPolygonF right_blindspot_vertices; From 7229c7541eac0fc321252d4c53a98534ec1fbb07 Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Mon, 13 Oct 2025 19:02:53 -0700 Subject: [PATCH 4/7] capnp: consolidate TurnDirection enum (#1370) Co-authored-by: Jason Wen --- cereal/custom.capnp | 10 +++++----- selfdrive/controls/lib/desire_helper.py | 10 +++++----- sunnypilot/selfdrive/controls/lib/lane_turn_desire.py | 10 +++++----- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 833845d36d..20f0984620 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -404,12 +404,12 @@ struct LiveMapDataSP @0xf416ec09499d9d19 { struct ModelDataV2SP @0xa1680744031fdb2d { laneTurnDirection @0 :TurnDirection; -} -enum TurnDirection { - none @0; - turnLeft @1; - turnRight @2; + enum TurnDirection { + none @0; + turnLeft @1; + turnRight @2; + } } struct CustomReserved10 @0xcb9fd56c7057593a { diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index e72d464d06..bf24fe2602 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -32,9 +32,9 @@ DESIRES = { } TURN_DESIRES = { - custom.TurnDirection.none: log.Desire.none, - custom.TurnDirection.turnLeft: log.Desire.turnLeft, - custom.TurnDirection.turnRight: log.Desire.turnRight, + custom.ModelDataV2SP.TurnDirection.none: log.Desire.none, + custom.ModelDataV2SP.TurnDirection.turnLeft: log.Desire.turnLeft, + custom.ModelDataV2SP.TurnDirection.turnRight: log.Desire.turnRight, } @@ -49,7 +49,7 @@ class DesireHelper: self.desire = log.Desire.none self.alc = AutoLaneChangeController(self) self.lane_turn_controller = LaneTurnController(self) - self.lane_turn_direction = custom.TurnDirection.none + self.lane_turn_direction = custom.ModelDataV2SP.TurnDirection.none @staticmethod def get_lane_change_direction(CS): @@ -126,7 +126,7 @@ class DesireHelper: self.prev_one_blinker = one_blinker - if self.lane_turn_direction != custom.TurnDirection.none: + if self.lane_turn_direction != custom.ModelDataV2SP.TurnDirection.none: self.desire = TURN_DESIRES[self.lane_turn_direction] else: self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] diff --git a/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py index 00ce026abb..7767fdae74 100644 --- a/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py +++ b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py @@ -15,7 +15,7 @@ LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS class LaneTurnController: def __init__(self, desire_helper): self.DH = desire_helper - self.turn_direction = custom.TurnDirection.none + self.turn_direction = custom.ModelDataV2SP.TurnDirection.none self.params = Params() self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS self.param_read_counter = 0 @@ -33,13 +33,13 @@ class LaneTurnController: def update_lane_turn(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float) -> None: if left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left: - self.turn_direction = custom.TurnDirection.turnLeft + self.turn_direction = custom.ModelDataV2SP.TurnDirection.turnLeft elif right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right: - self.turn_direction = custom.TurnDirection.turnRight + self.turn_direction = custom.ModelDataV2SP.TurnDirection.turnRight else: - self.turn_direction = custom.TurnDirection.none + self.turn_direction = custom.ModelDataV2SP.TurnDirection.none def get_turn_direction(self): if not self.enabled: - return custom.TurnDirection.none + return custom.ModelDataV2SP.TurnDirection.none return self.turn_direction From 59c64acc2901e7156fddadc9194e99e3fa44ebd5 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 13 Oct 2025 22:26:47 -0400 Subject: [PATCH 5/7] Subaru: Stop and Go support (beta) (#1375) * Subaru: Stop and Go auto-resume support * bump * bump * fix * bump * fix init * wat * use just standstill for now * Revert "use just standstill for now" This reverts commit f72cce68921ffe15b8914ce42d213758a1807619. * bump * bump * fix it * only send at 10 * bump * fix type * forget about planner resume, it sucks * try to send off_accel * still need it * always send * disable safety checks for now * same * more * all the time for both * don't need i guess * bump * try 15 frames per try * all should have it * try 3 for all * use throttle for all preglobal? * bump * bump * separate thresholds between preglobal and global * longer wait before sending * shorter time but immediately resend * quick * new timeout * about to cry * same thing but another try * no need * round 3 * try 1.4 * lower! * 1.2 * last try * beta asf * bump --- common/params_keys.h | 2 + opendbc_repo | 2 +- .../settings/vehicle/subaru_settings.cc | 45 +++++++++++++++++++ .../settings/vehicle/subaru_settings.h | 31 +++++++++++++ sunnypilot/selfdrive/car/interfaces.py | 6 +++ 5 files changed, 85 insertions(+), 1 deletion(-) diff --git a/common/params_keys.h b/common/params_keys.h index 3b5d02a429..ecb73a9456 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -205,6 +205,8 @@ inline static std::unordered_map keys = { // sunnypilot car specific params {"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}}, + {"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}}, {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}}, {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/opendbc_repo b/opendbc_repo index b592ecdd3b..b8a00bddda 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit b592ecdd3b571a1acee0c04726117a137cec5832 +Subproject commit b8a00bddda562f981b24e099a3850209579e890a diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle/subaru_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle/subaru_settings.cc index 47c4057f44..302740a94a 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle/subaru_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle/subaru_settings.cc @@ -8,7 +8,52 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle/subaru_settings.h" SubaruSettings::SubaruSettings(QWidget *parent) : BrandSettingsInterface(parent) { + stopAndGoToggle = new ParamControl("SubaruStopAndGo", tr("Stop and Go (Beta)"), "", ""); + stopAndGoToggle->setConfirmation(true, false); + list->addItem(stopAndGoToggle); + + stopAndGoManualParkingBrakeToggle = new ParamControl( + "SubaruStopAndGoManualParkingBrake", + tr("Stop and Go for Manual Parking Brake (Beta)"), + "", + "" + ); + stopAndGoManualParkingBrakeToggle->setConfirmation(true, false); + list->addItem(stopAndGoManualParkingBrakeToggle); } void SubaruSettings::updateSettings() { + auto cp_bytes = params.get("CarParamsPersistent"); + if (!cp_bytes.empty()) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + + is_subaru = CP.getBrand() == "subaru"; + + if (is_subaru) { + if (!(CP.getFlags() & (SUBARU_FLAG_GLOBAL_GEN2 | SUBARU_FLAG_HYBRID))) { + has_stop_and_go = true; + } + } + } else { + is_subaru = false; + has_stop_and_go = false; + } + + bool stop_and_go_disabled = !offroad || !has_stop_and_go; + QString stop_and_go_desc = stopAndGoDescriptionBuilder(stopAndGoDesc); + QString stop_and_go_manual_parking_brake_desc = stopAndGoDescriptionBuilder(stopAndGoManualParkingBrakeDesc); + if (stop_and_go_disabled) { + stop_and_go_desc = stopAndGoDescriptionBuilder(stopAndGoDesc, stopAndGoDisabledMsg()); + stop_and_go_manual_parking_brake_desc = stopAndGoDescriptionBuilder(stopAndGoManualParkingBrakeDesc, stopAndGoDisabledMsg()); + } + + stopAndGoToggle->setEnabled(has_stop_and_go); + stopAndGoToggle->setDescription(stop_and_go_desc); + stopAndGoToggle->showDescription(); + + stopAndGoManualParkingBrakeToggle->setEnabled(has_stop_and_go); + stopAndGoManualParkingBrakeToggle->setDescription(stop_and_go_manual_parking_brake_desc); + stopAndGoManualParkingBrakeToggle->showDescription(); } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle/subaru_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle/subaru_settings.h index a715951ad9..2bb160beba 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle/subaru_settings.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle/subaru_settings.h @@ -14,6 +14,9 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" +const int SUBARU_FLAG_GLOBAL_GEN2 = 4; +const int SUBARU_FLAG_HYBRID = 32; + class SubaruSettings : public BrandSettingsInterface { Q_OBJECT @@ -23,4 +26,32 @@ public: private: bool offroad = false; + bool is_subaru; + bool has_stop_and_go; + + ParamControl* stopAndGoToggle; + ParamControl* stopAndGoManualParkingBrakeToggle; + + QString stopAndGoDesc = tr("Experimental feature to enable auto-resume during stop-and-go for certain supported Subaru platforms."); + QString stopAndGoManualParkingBrakeDesc = tr("Experimental feature to enable stop and go for Subaru Global models with manual handbrake. Models with electric parking brake should keep this disabled. Thanks to martinl for this implementation!"); + + QString stopAndGoDisabledMsg() const { + if (is_subaru && !has_stop_and_go) { + return tr("This feature is currently not available on this platform."); + } + + if (!is_subaru) { + return tr("Start the car to check car compatibility."); + } + + if (!offroad) { + return tr("Enable \"Always Offroad\" in Device panel, or turn vehicle off to toggle."); + } + + return QString(); + } + + static QString stopAndGoDescriptionBuilder(const QString &base_description, const QString &custom_description = "") { + return "" + custom_description + "

" + base_description; + } }; diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index ed0c11fb19..3072cde8cd 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -84,4 +84,10 @@ def initialize_params(params) -> list[dict[str, Any]]: "HyundaiLongitudinalTuning" ]) + # subaru + keys.extend([ + "SubaruStopAndGo", + "SubaruStopAndGoManualParkingBrake", + ]) + return [{k: params.get(k, return_default=True)} for k in keys] From 339bc0b8b3c4a192aa4e982039bc2ac28246c080 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 14 Oct 2025 00:19:21 -0400 Subject: [PATCH 6/7] Revert "capnp: consolidate TurnDirection enum" (#1376) Revert "capnp: consolidate TurnDirection enum (#1370)" This reverts commit 7229c7541eac0fc321252d4c53a98534ec1fbb07. --- cereal/custom.capnp | 10 +++++----- selfdrive/controls/lib/desire_helper.py | 10 +++++----- sunnypilot/selfdrive/controls/lib/lane_turn_desire.py | 10 +++++----- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 20f0984620..833845d36d 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -404,12 +404,12 @@ struct LiveMapDataSP @0xf416ec09499d9d19 { struct ModelDataV2SP @0xa1680744031fdb2d { laneTurnDirection @0 :TurnDirection; +} - enum TurnDirection { - none @0; - turnLeft @1; - turnRight @2; - } +enum TurnDirection { + none @0; + turnLeft @1; + turnRight @2; } struct CustomReserved10 @0xcb9fd56c7057593a { diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index bf24fe2602..e72d464d06 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -32,9 +32,9 @@ DESIRES = { } TURN_DESIRES = { - custom.ModelDataV2SP.TurnDirection.none: log.Desire.none, - custom.ModelDataV2SP.TurnDirection.turnLeft: log.Desire.turnLeft, - custom.ModelDataV2SP.TurnDirection.turnRight: log.Desire.turnRight, + custom.TurnDirection.none: log.Desire.none, + custom.TurnDirection.turnLeft: log.Desire.turnLeft, + custom.TurnDirection.turnRight: log.Desire.turnRight, } @@ -49,7 +49,7 @@ class DesireHelper: self.desire = log.Desire.none self.alc = AutoLaneChangeController(self) self.lane_turn_controller = LaneTurnController(self) - self.lane_turn_direction = custom.ModelDataV2SP.TurnDirection.none + self.lane_turn_direction = custom.TurnDirection.none @staticmethod def get_lane_change_direction(CS): @@ -126,7 +126,7 @@ class DesireHelper: self.prev_one_blinker = one_blinker - if self.lane_turn_direction != custom.ModelDataV2SP.TurnDirection.none: + if self.lane_turn_direction != custom.TurnDirection.none: self.desire = TURN_DESIRES[self.lane_turn_direction] else: self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] diff --git a/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py index 7767fdae74..00ce026abb 100644 --- a/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py +++ b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py @@ -15,7 +15,7 @@ LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS class LaneTurnController: def __init__(self, desire_helper): self.DH = desire_helper - self.turn_direction = custom.ModelDataV2SP.TurnDirection.none + self.turn_direction = custom.TurnDirection.none self.params = Params() self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS self.param_read_counter = 0 @@ -33,13 +33,13 @@ class LaneTurnController: def update_lane_turn(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float) -> None: if left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left: - self.turn_direction = custom.ModelDataV2SP.TurnDirection.turnLeft + self.turn_direction = custom.TurnDirection.turnLeft elif right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right: - self.turn_direction = custom.ModelDataV2SP.TurnDirection.turnRight + self.turn_direction = custom.TurnDirection.turnRight else: - self.turn_direction = custom.ModelDataV2SP.TurnDirection.none + self.turn_direction = custom.TurnDirection.none def get_turn_direction(self): if not self.enabled: - return custom.ModelDataV2SP.TurnDirection.none + return custom.TurnDirection.none return self.turn_direction From 7f5342f3784c841b275fcbf213ef5e339f1fe991 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 14 Oct 2025 01:13:20 -0400 Subject: [PATCH 7/7] soundd: custom audible alerts (#1377) * Revert "capnp: consolidate TurnDirection enum (#1370)" This reverts commit 7229c7541eac0fc321252d4c53a98534ec1fbb07. * soundd: custom audible alerts * comment --- cereal/custom.capnp | 39 +++++++++++++++++++++++++++++++++++++++ selfdrive/ui/soundd.py | 9 ++++++++- 2 files changed, 47 insertions(+), 1 deletion(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 833845d36d..5243380d31 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -69,6 +69,45 @@ struct LeadData { struct SelfdriveStateSP @0x81c2f05a394cf4af { mads @0 :ModularAssistiveDrivingSystem; intelligentCruiseButtonManagement @1 :IntelligentCruiseButtonManagement; + + enum AudibleAlert { + none @0; + + engage @1; + disengage @2; + refuse @3; + + warningSoft @4; + warningImmediate @5; + + prompt @6; + promptRepeat @7; + promptDistracted @8; + + # unused, these are reserved for upstream events so we don't collide + reserved9 @9; + reserved10 @10; + reserved11 @11; + reserved12 @12; + reserved13 @13; + reserved14 @14; + reserved15 @15; + reserved16 @16; + reserved17 @17; + reserved18 @18; + reserved19 @19; + reserved20 @20; + reserved21 @21; + reserved22 @22; + reserved23 @23; + reserved24 @24; + reserved25 @25; + reserved26 @26; + reserved27 @27; + reserved28 @28; + reserved29 @29; + reserved30 @30; + } } struct ModelManagerSP @0xaedffd8f31e7b55d { diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index a94456efe0..485c406266 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -4,7 +4,7 @@ import time import wave -from cereal import car, messaging +from cereal import car, messaging, custom from openpilot.common.basedir import BASEDIR from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import Ratekeeper @@ -26,8 +26,13 @@ AMBIENT_DB = 30 # DB where MIN_VOLUME is applied DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied AudibleAlert = car.CarControl.HUDControl.AudibleAlert +AudibleAlertSP = custom.SelfdriveStateSP.AudibleAlert +sound_list_sp: dict[int, tuple[str, int | None, float]] = { + # AudibleAlertSP, file name, play count (none for infinite) +} + sound_list: dict[int, tuple[str, int | None, float]] = { # AudibleAlert, file name, play count (none for infinite) AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), @@ -40,6 +45,8 @@ sound_list: dict[int, tuple[str, int | None, float]] = { AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME), AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME), + + **sound_list_sp, } def check_selfdrive_timeout_alert(sm):