Merge branch 'master' of https://github.com/sunnypilot/sunnypilot

cereal/custom.capnp

@@ -25,6 +25,26 @@ struct ModularAssistiveDrivingSystem {
   }
 }
 
+struct IntelligentCruiseButtonManagement {
+  state @0 :IntelligentCruiseButtonManagementState;
+  sendButton @1 :SendButtonState;
+  vTarget @2 :Float32;
+
+  enum IntelligentCruiseButtonManagementState {
+    inactive @0;      # No button press or default state
+    preActive @1;     # Pre-active state before transitioning to increasing or decreasing
+    increasing @2;    # Increasing speed
+    decreasing @3;    # Decreasing speed
+    holding @4;       # Holding steady speed
+  }
+
+  enum SendButtonState {
+    none @0;
+    increase @1;
+    decrease @2;
+  }
+}
+
 # Same struct as Log.RadarState.LeadData
 struct LeadData {
   dRel @0 :Float32;
@@ -48,6 +68,7 @@ struct LeadData {
 
 struct SelfdriveStateSP @0x81c2f05a394cf4af {
   mads @0 :ModularAssistiveDrivingSystem;
+  intelligentCruiseButtonManagement @1 :IntelligentCruiseButtonManagement;
 }
 
 struct ModelManagerSP @0xaedffd8f31e7b55d {
@@ -122,6 +143,8 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
 
 struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   dec @0 :DynamicExperimentalControl;
+  longitudinalPlanSource @1 :LongitudinalPlanSource;
+  smartCruiseControl @2 :SmartCruiseControl;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -133,6 +156,34 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
       blended @1;
     }
   }
+
+  struct SmartCruiseControl {
+    vision @0 :Vision;
+
+    struct Vision {
+      state @0 :VisionState;
+      vTarget @1 :Float32;
+      aTarget @2 :Float32;
+      currentLateralAccel @3 :Float32;
+      maxPredictedLateralAccel @4 :Float32;
+      enabled @5 :Bool;
+      active @6 :Bool;
+    }
+
+    enum VisionState {
+      disabled @0; # System disabled or inactive.
+      enabled @1; # No predicted substantial turn on vision range.
+      entering @2; # A substantial turn is predicted ahead, adapting speed to turn comfort levels.
+      turning @3; # Actively turning. Managing acceleration to provide a roll on turn feeling.
+      leaving @4; # Road ahead straightens. Start to allow positive acceleration.
+      overriding @5; # System overriding with manual control.
+    }
+  }
+
+  enum LongitudinalPlanSource {
+    cruise @0;
+    sccVision @1;
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {
@@ -180,6 +231,8 @@ struct OnroadEventSP @0xda96579883444c35 {
 struct CarParamsSP @0x80ae746ee2596b11 {
   flags @0 :UInt32;        # flags for car specific quirks in sunnypilot
   safetyParam @1 : Int16;  # flags for sunnypilot's custom safety flags
+  pcmCruiseSpeed @3 :Bool = true;
+  intelligentCruiseButtonManagementAvailable @4 :Bool;
 
   neuralNetworkLateralControl @2 :NeuralNetworkLateralControl;
 
@@ -199,6 +252,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
   params @1 :List(Param);
   leadOne @2 :LeadData;
   leadTwo @3 :LeadData;
+  intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
 
   struct Param {
     key @0 :Text;

common/params_keys.h

@@ -159,6 +159,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
     {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
     {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
+    {"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}},
     {"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}},
     {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
     {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
@@ -169,6 +170,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"SmartCruiseControlVision", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // MADS params
     {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},

selfdrive/car/card.py

@@ -179,7 +179,7 @@ class Car:
     self.params.put_nonblocking("CarParamsSPPersistent", cp_sp_bytes)
 
     self.mock_carstate = MockCarState()
-    self.v_cruise_helper = VCruiseHelper(self.CP)
+    self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP)
 
     self.is_metric = self.params.get_bool("IsMetric")
     self.experimental_mode = self.params.get_bool("ExperimentalMode")

selfdrive/car/cruise.py

@@ -30,8 +30,8 @@ CRUISE_INTERVAL_SIGN = {
 
 
 class VCruiseHelper(VCruiseHelperSP):
-  def __init__(self, CP):
-    VCruiseHelperSP.__init__(self)
+  def __init__(self, CP, CP_SP):
+    VCruiseHelperSP.__init__(self, CP, CP_SP)
     self.CP = CP
     self.v_cruise_kph = V_CRUISE_UNSET
     self.v_cruise_cluster_kph = V_CRUISE_UNSET
@@ -47,12 +47,14 @@ class VCruiseHelper(VCruiseHelperSP):
   def update_v_cruise(self, CS, enabled, is_metric, speed_limit_control=False, speed_limit_predicative=False):
     self.v_cruise_kph_last = self.v_cruise_kph
 
+    self.get_minimum_set_speed(is_metric)
+
     if CS.cruiseState.available:
-      if not self.CP.pcmCruise:
+      _enabled = self.update_enabled_state(CS, enabled)
+      if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled):
         # if stock cruise is completely disabled, then we can use our own set speed logic
-        
-        self._update_v_speed_limit(CS, enabled, speed_limit_control, speed_limit_predicative)
-        self._update_v_cruise_non_pcm(CS, enabled, is_metric)
+        self._update_v_speed_limit(CS, _enabled, speed_limit_control, speed_limit_predicative)
+        self._update_v_cruise_non_pcm(CS, _enabled, is_metric)
         self.v_cruise_cluster_kph = self.v_cruise_kph
         self.update_button_timers(CS, enabled)
       else:
@@ -129,7 +131,7 @@ class VCruiseHelper(VCruiseHelperSP):
     if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
       self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH)
 
-    self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
+    self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
 
   def update_button_timers(self, CS, enabled):
     # increment timer for buttons still pressed

selfdrive/car/helpers.py

@@ -60,5 +60,8 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
   struct_dataclass.params = [structs.CarControlSP.Param(**remove_deprecated(p)) for p in struct_dict.get('params', [])]
   struct_dataclass.leadOne = structs.LeadData(**remove_deprecated(struct_dict.get('leadOne', {})))
   struct_dataclass.leadTwo = structs.LeadData(**remove_deprecated(struct_dict.get('leadTwo', {})))
+  struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement(
+    **remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {}))
+  )
 
   return struct_dataclass

selfdrive/car/tests/test_cruise_speed.py

@@ -5,7 +5,7 @@ import numpy as np
 from parameterized import parameterized_class
 from cereal import log
 from openpilot.selfdrive.car.cruise import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT
-from cereal import car
+from cereal import car, custom
 from openpilot.common.constants import CV
 from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
@@ -49,7 +49,8 @@ class TestCruiseSpeed:
 class TestVCruiseHelper:
   def setup_method(self):
     self.CP = car.CarParams(pcmCruise=self.pcm_cruise)
-    self.v_cruise_helper = VCruiseHelper(self.CP)
+    self.CP_SP = custom.CarParamsSP()
+    self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP)
     self.reset_cruise_speed_state()
 
   def reset_cruise_speed_state(self):

selfdrive/controls/controlsd.py

@@ -139,7 +139,8 @@ class Controls(ControlsExt, ModelStateBase):
 
     CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                    (not standstill or self.CP.steerAtStandstill)
-    CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl
+    CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and \
+                    (self.CP.openpilotLongitudinalControl or not self.CP_SP.pcmCruiseSpeed)
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state
@@ -201,7 +202,7 @@ class Controls(ControlsExt, ModelStateBase):
       CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
       CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
 
-    CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
+    CC.cruiseControl.override = CC.enabled and not CC.longActive and (self.CP.openpilotLongitudinalControl or not self.CP_SP.pcmCruiseSpeed)
     CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise)
     CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and not self.sm['longitudinalPlan'].shouldStop
     CC.cruiseControl.speedLimit = self.enable_speed_limit_control

selfdrive/controls/lib/longitudinal_planner.py

@@ -155,6 +155,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
       accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
 
+    # Get new v_cruise and a_desired from Smart Cruise Control
+    v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
+
     if force_slow_decel:
       v_cruise = 0.0
 

selfdrive/selfdrived/selfdrived.py

@@ -26,6 +26,7 @@ from openpilot.system.version import get_build_metadata
 from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem
 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
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 
 REPLAY = "REPLAY" in os.environ
@@ -156,6 +157,7 @@ class SelfdriveD(CruiseHelper):
     self.events_sp_prev = []
 
     self.mads = ModularAssistiveDrivingSystem(self)
+    self.icbm = IntelligentCruiseButtonManagement(self.CP, self.CP_SP)
 
     self.car_events_sp = CarSpecificEventsSP(self.CP, self.params)
 
@@ -442,6 +444,8 @@ class SelfdriveD(CruiseHelper):
           self.events.add(EventName.personalityChanged)
         self.experimental_mode_switched = False
 
+    self.icbm.run(CS, self.sm['carControl'], self.is_metric)
+
   def data_sample(self):
     _car_state = messaging.recv_one(self.car_state_sock)
     CS = _car_state.carState if _car_state else self.CS_prev
@@ -546,6 +550,11 @@ class SelfdriveD(CruiseHelper):
     mads.active = self.mads.active
     mads.available = self.mads.enabled_toggle
 
+    icbm = ss_sp.intelligentCruiseButtonManagement
+    icbm.state = self.icbm.state
+    icbm.sendButton = self.icbm.cruise_button
+    icbm.vTarget = self.icbm.v_target
+
     self.pm.send('selfdriveStateSP', ss_sp_msg)
 
     # onroadEventsSP - logged every second or on change

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

@@ -18,6 +18,23 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
   cruisePanelScroller = new ScrollViewSP(list, this);
   vlayout->addWidget(cruisePanelScroller);
 
+  intelligentCruiseButtonManagement = new ParamControlSP(
+    "IntelligentCruiseButtonManagement",
+    tr("Intelligent Cruise Button Management (ICBM) (Alpha)"),
+    tr("When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control."),
+    "",
+    this
+  );
+  intelligentCruiseButtonManagement->setConfirmation(true, false);
+  list->addItem(intelligentCruiseButtonManagement);
+
+  SmartCruiseControlVision = new ParamControl(
+    "SmartCruiseControlVision",
+    tr("Smart Cruise Control - Vision"),
+    tr("Use vision path predictions to estimate the appropriate speed to drive through turns ahead."),
+    "");
+  list->addItem(SmartCruiseControlVision);
+
   customAccIncrement = new CustomAccIncrement("CustomAccIncrementsEnabled", tr("Custom ACC Speed Increments"), "", "", this);
   list->addItem(customAccIncrement);
 
@@ -35,16 +52,22 @@ void LongitudinalPanel::showEvent(QShowEvent *event) {
 
 void LongitudinalPanel::refresh(bool _offroad) {
   auto cp_bytes = params.get("CarParamsPersistent");
-  if (!cp_bytes.empty()) {
+  auto cp_sp_bytes = params.get("CarParamsSPPersistent");
+  if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
     AlignedBuffer aligned_buf;
+    AlignedBuffer aligned_buf_sp;
     capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
+    capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size()));
     cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
+    cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot<cereal::CarParamsSP>();
 
     has_longitudinal_control = hasLongitudinalControl(CP);
     is_pcm_cruise = CP.getPcmCruise();
+    intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable();
   } else {
     has_longitudinal_control = false;
     is_pcm_cruise = false;
+    intelligent_cruise_button_management_available = false;
   }
 
   QString accEnabledDescription = tr("Enable custom Short & Long press increments for cruise speed increase/decrease.");
@@ -56,7 +79,7 @@ void LongitudinalPanel::refresh(bool _offroad) {
     customAccIncrement->setDescription(onroadOnlyDescription);
     customAccIncrement->showDescription();
   } else {
-    if (has_longitudinal_control) {
+    if (has_longitudinal_control || intelligent_cruise_button_management_available) {
       if (is_pcm_cruise) {
         customAccIncrement->setDescription(accPcmCruiseDisabledDescription);
         customAccIncrement->showDescription();
@@ -68,12 +91,20 @@ void LongitudinalPanel::refresh(bool _offroad) {
       customAccIncrement->toggleFlipped(false);
       customAccIncrement->setDescription(accNoLongDescription);
       customAccIncrement->showDescription();
+      params.remove("IntelligentCruiseButtonManagement");
+      intelligentCruiseButtonManagement->toggleFlipped(false);
     }
   }
 
+  bool icbm_allowed = intelligent_cruise_button_management_available && !has_longitudinal_control;
+  intelligentCruiseButtonManagement->setEnabled(icbm_allowed && offroad);
+
   // enable toggle when long is available and is not PCM cruise
-  customAccIncrement->setEnabled(has_longitudinal_control && !is_pcm_cruise && !offroad);
+  bool cai_allowed = (has_longitudinal_control && !is_pcm_cruise) || icbm_allowed;
+  customAccIncrement->setEnabled(cai_allowed && !offroad);
   customAccIncrement->refresh();
 
+  SmartCruiseControlVision->setEnabled(has_longitudinal_control || icbm_allowed);
+
   offroad = _offroad;
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h

@@ -23,10 +23,13 @@ private:
   Params params;
   bool has_longitudinal_control = false;
   bool is_pcm_cruise = false;
+  bool intelligent_cruise_button_management_available = false;;
   bool offroad = false;
 
   QStackedLayout *main_layout = nullptr;
   ScrollViewSP *cruisePanelScroller = nullptr;
   QWidget *cruisePanelScreen = nullptr;
   CustomAccIncrement *customAccIncrement = nullptr;
+  ParamControl *SmartCruiseControlVision;
+  ParamControl *intelligentCruiseButtonManagement = nullptr;
 };

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

@@ -35,6 +35,13 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
       "../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,
+    },
   };
 
   // Add regular toggles first

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

@@ -4,6 +4,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.
  */
+#include <QPainterPath>
 
 #include "selfdrive/ui/sunnypilot/qt/onroad/hud.h"
 
@@ -25,6 +26,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 lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
 
   static int reverse_delay = 0;
   bool reverse_allowed = false;
@@ -75,11 +77,33 @@ void HudRendererSP::updateState(const UIState &s) {
   latAccelFactorFiltered = ltp.getLatAccelFactorFiltered();
   frictionCoefficientFiltered = ltp.getFrictionCoefficientFiltered();
   liveValid = ltp.getLiveValid();
+
+  standstillTimer = s.scene.standstill_timer;
+  isStandstill = car_state.getStandstill();
+  longOverride = car_control.getCruiseControl().getOverride();
+  smartCruiseControlVisionEnabled = lp_sp.getSmartCruiseControl().getVision().getEnabled();
+  smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive();
 }
 
 void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
   HudRenderer::draw(p, surface_rect);
   if (!reversing) {
+    // Smart Cruise Control
+    int x_offset = -260;
+    int y1_offset = -80;
+    // int y2_offset = -140;  // reserved for 2 icons
+
+    bool scc_vision_active_pulse = pulseElement(smartCruiseControlVisionFrame);
+    if ((smartCruiseControlVisionEnabled && !smartCruiseControlVisionActive) || (smartCruiseControlVisionActive && scc_vision_active_pulse)) {
+      drawSmartCruiseControlOnroadIcon(p, surface_rect, x_offset, y1_offset, "SCC-V");
+    }
+
+    if (smartCruiseControlVisionActive) {
+      smartCruiseControlVisionFrame++;
+    } else {
+      smartCruiseControlVisionFrame = 0;
+    }
+
     // Bottom Dev UI
     if (devUiInfo == 2) {
       QRect rect_bottom(surface_rect.left(), surface_rect.bottom() - 60, surface_rect.width(), 61);
@@ -94,6 +118,11 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
       QRect rect_right(surface_rect.right() - (UI_BORDER_SIZE * 2), UI_BORDER_SIZE * 1.5, 184, 170);
       drawRightDevUI(p, surface_rect.right() - 184 - UI_BORDER_SIZE * 2, UI_BORDER_SIZE * 2 + rect_right.height());
     }
+
+    // Standstill Timer
+    if (standstillTimer) {
+      drawStandstillTimer(p, surface_rect.right() / 12 * 10, surface_rect.bottom() / 12 * 1.53);
+    }
   }
 }
 
@@ -104,6 +133,48 @@ void HudRendererSP::drawText(QPainter &p, int x, int y, const QString &text, QCo
   p.drawText(real_rect.x(), real_rect.bottom(), text);
 }
 
+bool HudRendererSP::pulseElement(int frame) {
+  if (frame % UI_FREQ < (UI_FREQ / 2.5)) {
+    return false;
+  }
+
+  return true;
+}
+
+void HudRendererSP::drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name) {
+  int x = surface_rect.center().x();
+  int y = surface_rect.height() / 4;
+
+  QString text = QString::fromStdString(name);
+  QFont font = InterFont(36, QFont::Bold);
+  p.setFont(font);
+
+  QFontMetrics fm(font);
+
+  int padding_v = 5;
+  int box_width = 160;
+  int box_height = fm.height() + padding_v * 2;
+
+  QRectF bg_rect(x - (box_width / 2) + x_offset,
+                 y - (box_height / 2) + y_offset,
+                 box_width, box_height);
+
+  QPainterPath boxPath;
+  boxPath.addRoundedRect(bg_rect, 10, 10);
+
+  int text_w = fm.horizontalAdvance(text);
+  qreal baseline_y = bg_rect.top() + padding_v + fm.ascent();
+  qreal text_x = bg_rect.center().x() - (text_w / 2.0);
+
+  QPainterPath textPath;
+  textPath.addText(QPointF(text_x, baseline_y), font, text);
+  boxPath = boxPath.subtracted(textPath);
+
+  p.setPen(Qt::NoPen);
+  p.setBrush(longOverride ? QColor(0x91, 0x9b, 0x95, 0xf1) : QColor(0, 0xff, 0, 0xff));
+  p.drawPath(boxPath);
+}
+
 int HudRendererSP::drawRightDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color) {
 
   p.setFont(InterFont(28, QFont::Bold));
@@ -206,3 +277,37 @@ void HudRendererSP::drawBottomDevUI(QPainter &p, int x, int y) {
   UiElement altitudeElement = DeveloperUi::getAltitude(gpsAccuracy, altitude);
   rw += drawBottomDevUIElement(p, rw, y, altitudeElement.value, altitudeElement.label, altitudeElement.units, altitudeElement.color);
 }
+
+void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) {
+  if (isStandstill) {
+    standstillElapsedTime += 1.0 / UI_FREQ;
+
+    int minute = static_cast<int>(standstillElapsedTime / 60);
+    int second = static_cast<int>(standstillElapsedTime - (minute * 60));
+
+    // stop sign for standstill timer
+    const int size = 190;  // size
+    const float angle = M_PI / 8.0;
+
+    QPolygon octagon;
+    for (int i = 0; i < 8; i++) {
+      float curr_angle = angle + i * M_PI / 4.0;
+      int point_x = x + size / 2 * cos(curr_angle);
+      int point_y = y + size / 2 * sin(curr_angle);
+      octagon << QPoint(point_x, point_y);
+    }
+
+    p.setPen(QPen(Qt::white, 6));
+    p.setBrush(QColor(255, 90, 81, 200)); // red pastel
+    p.drawPolygon(octagon);
+
+    QString time_str = QString("%1:%2").arg(minute, 1, 10, QChar('0')).arg(second, 2, 10, QChar('0'));
+    p.setFont(InterFont(55, QFont::Bold));
+    p.setPen(Qt::white);
+    QRect timerTextRect = p.fontMetrics().boundingRect(QString(time_str));
+    timerTextRect.moveCenter({x, y});
+    p.drawText(timerTextRect, Qt::AlignCenter, QString(time_str));
+  } else {
+    standstillElapsedTime = 0.0;
+  }
+}

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

@@ -25,6 +25,9 @@ private:
   int drawRightDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color);
   int drawBottomDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color);
   void drawBottomDevUI(QPainter &p, int x, int y);
+  void drawStandstillTimer(QPainter &p, int x, int y);
+  bool pulseElement(int frame);
+  void drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name);
 
   bool lead_status;
   float lead_d_rel;
@@ -53,4 +56,11 @@ private:
   bool reversing;
   cereal::CarParams::SteerControlType steerControlType;
   cereal::CarControl::Actuators::Reader actuators;
+  bool standstillTimer;
+  bool isStandstill;
+  float standstillElapsedTime;
+  bool longOverride;
+  bool smartCruiseControlVisionEnabled;
+  bool smartCruiseControlVisionActive;
+  int smartCruiseControlVisionFrame;
 };

selfdrive/ui/sunnypilot/ui.cc

@@ -34,6 +34,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
     ui_update_params_sp(this);
   });
   param_watcher->addParam("DevUIInfo");
+  param_watcher->addParam("StandstillTimer");
 }
 
 // This method overrides completely the update method from the parent class intentionally.
@@ -51,6 +52,7 @@ void UIStateSP::update() {
 void ui_update_params_sp(UIStateSP *s) {
   auto params = Params();
   s->scene.dev_ui_info = std::atoi(params.get("DevUIInfo").c_str());
+  s->scene.standstill_timer = params.getBool("StandstillTimer");
 }
 
 DeviceSP::DeviceSP(QObject *parent) : Device(parent) {

selfdrive/ui/sunnypilot/ui_scene.h

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

sunnypilot/__init__.py

@@ -0,0 +1,7 @@
+"""
+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.
+"""
+PARAMS_UPDATE_PERIOD = 3  # seconds

sunnypilot/selfdrive/car/cruise_ext.py

@@ -7,19 +7,46 @@ See the LICENSE.md file in the root directory for more details.
 import numpy as np
 
 from cereal import car
+from opendbc.car import structs
 from openpilot.common.params import Params
+from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
 
 ButtonType = car.CarState.ButtonEvent.Type
 
+CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
+                       ButtonType.setCruise: 0, ButtonType.resumeCruise: 0,
+                       ButtonType.cancel: 0, ButtonType.mainCruise: 0}
+
+V_CRUISE_MIN = 8
+
+
+def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarState.ButtonEvent.Type, int]) -> None:
+  # increment timer for buttons still pressed
+  for k in button_timers:
+    if button_timers[k] > 0:
+      button_timers[k] += 1
+
+  for b in CS.buttonEvents:
+    if b.type.raw in button_timers:
+      # Start/end timer and store current state on change of button pressed
+      button_timers[b.type.raw] = 1 if b.pressed else 0
+
+
 
 class VCruiseHelperSP:
-  def __init__(self) -> None:
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> None:
+    self.CP = CP
+    self.CP_SP = CP_SP
     self.params = Params()
+    self.v_cruise_min = 0
+    self.enabled_prev = False
 
     self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
     self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
     self.long_increment = self.params.get("CustomAccLongPressIncrement", return_default=True)
 
+    self.enable_button_timers = CRUISE_BUTTON_TIMER
+
   def read_custom_set_speed_params(self) -> None:
     self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
     self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
@@ -39,3 +66,26 @@ class VCruiseHelperSP:
     v_cruise_delta = v_cruise_delta * actual_increment
 
     return round_to_nearest, v_cruise_delta
+
+  def get_minimum_set_speed(self, is_metric: bool) -> None:
+    if self.CP_SP.pcmCruiseSpeed:
+      self.v_cruise_min = V_CRUISE_MIN
+      return
+
+    self.v_cruise_min = get_minimum_set_speed(is_metric)
+
+  def update_enabled_state(self, CS: car.CarState, enabled: bool) -> bool:
+    # special enabled state for non pcmCruiseSpeed, unchanged for non pcmCruise
+    if not self.CP_SP.pcmCruiseSpeed:
+      update_manual_button_timers(CS, self.enable_button_timers)
+      button_pressed = any(self.enable_button_timers[k] > 0 for k in self.enable_button_timers)
+
+      if enabled and not self.enabled_prev:
+        self.enabled_prev = not button_pressed
+        enabled = False
+      elif not enabled:
+        self.enabled_prev = enabled
+
+      return enabled and self.enabled_prev
+
+    return enabled

sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py

@@ -0,0 +1,137 @@
+"""
+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 cereal import car, custom
+from opendbc.car import structs, apply_hysteresis
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_CTRL
+from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
+from openpilot.sunnypilot.selfdrive.car.cruise_ext import CRUISE_BUTTON_TIMER, update_manual_button_timers
+
+LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
+State = custom.IntelligentCruiseButtonManagement.IntelligentCruiseButtonManagementState
+SendButtonState = custom.IntelligentCruiseButtonManagement.SendButtonState
+
+ALLOWED_SPEED_THRESHOLD = 1.8  # m/s, ~4 MPH
+HYST_GAP = 0.75
+INACTIVE_TIMER = 0.4
+
+
+SEND_BUTTONS = {
+  State.increasing: SendButtonState.increase,
+  State.decreasing: SendButtonState.decrease,
+}
+
+
+class IntelligentCruiseButtonManagement:
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
+    self.CP = CP
+    self.CP_SP = CP_SP
+
+    self.v_target = 0
+    self.v_cruise_cluster = 0
+    self.v_cruise_min = 0
+    self.cruise_button = SendButtonState.none
+    self.state = State.inactive
+    self.pre_active_timer = 0
+
+    self.is_ready = False
+    self.is_ready_prev = False
+    self.v_target_ms_last = 0.0
+    self.is_metric = False
+
+    self.cruise_button_timers = CRUISE_BUTTON_TIMER
+
+  @property
+  def v_cruise_equal(self) -> bool:
+    return self.v_target == self.v_cruise_cluster
+
+  def update_calculations(self, CS: car.CarState) -> 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 = round(self.v_target_ms_last * speed_conv)
+    self.v_cruise_min = get_minimum_set_speed(self.is_metric)
+    self.v_cruise_cluster = round(CS.cruiseState.speedCluster * speed_conv)
+
+  def update_state_machine(self) -> custom.IntelligentCruiseButtonManagement.SendButtonState:
+    self.pre_active_timer = max(0, self.pre_active_timer - 1)
+
+    # HOLDING, ACCELERATING, DECELERATING, PRE_ACTIVE
+    if self.state != State.inactive:
+      if not self.is_ready:
+        self.state = State.inactive
+
+      else:
+        # PRE_ACTIVE
+        if self.state == State.preActive:
+          if self.pre_active_timer <= 0:
+            if self.v_cruise_equal:
+              self.state = State.holding
+
+            elif self.v_target > self.v_cruise_cluster:
+              self.state = State.increasing
+
+            elif self.v_target < self.v_cruise_cluster and self.v_cruise_cluster > self.v_cruise_min:
+              self.state = State.decreasing
+
+        # HOLDING
+        elif self.state == State.holding:
+          if not self.v_cruise_equal:
+            self.state = State.preActive
+
+        # ACCELERATING
+        elif self.state == State.increasing:
+          if self.v_target <= self.v_cruise_cluster:
+            self.state = State.holding
+
+        # DECELERATING
+        elif self.state == State.decreasing:
+          if self.v_target >= self.v_cruise_cluster or self.v_cruise_cluster <= self.v_cruise_min:
+            self.state = State.holding
+
+    # INACTIVE
+    elif self.state == State.inactive:
+      if self.is_ready and not self.is_ready_prev:
+        self.pre_active_timer = int(INACTIVE_TIMER / DT_CTRL)
+        self.state = State.preActive
+
+    send_button = SEND_BUTTONS.get(self.state, SendButtonState.none)
+
+    return send_button
+
+  def update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None:
+    update_manual_button_timers(CS, self.cruise_button_timers)
+
+    allowed_speed = CS.vEgo > ALLOWED_SPEED_THRESHOLD
+    ready = CS.cruiseState.enabled and allowed_speed and not CC.cruiseControl.override and not CC.cruiseControl.cancel and \
+            not CC.cruiseControl.resume
+    button_pressed = any(self.cruise_button_timers[k] > 0 for k in self.cruise_button_timers)
+
+    self.is_ready = ready and not button_pressed
+
+  def run(self, CS: car.CarState, CC: car.CarControl, is_metric: bool) -> None:
+    if self.CP_SP.pcmCruiseSpeed:
+      return
+
+    self.is_metric = is_metric
+
+    self.update_calculations(CS)
+    self.update_readiness(CS, CC)
+
+    self.cruise_button = self.update_state_machine()
+
+    self.is_ready_prev = self.is_ready

sunnypilot/selfdrive/car/intelligent_cruise_button_management/helpers.py

@@ -0,0 +1,8 @@
+"""
+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.
+"""
+def get_minimum_set_speed(is_metric: bool) -> int:
+  return 30 if is_metric else 20

sunnypilot/selfdrive/car/interfaces.py

@@ -43,11 +43,21 @@ def _initialize_neural_network_lateral_control(CI: CarInterfaceBase, CP: structs
   CP_SP.neuralNetworkLateralControl.fuzzyFingerprint = not exact_match
 
 
+def _initialize_intelligent_cruise_button_management(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params = None) -> None:
+  if params is None:
+    params = Params()
+
+  icbm_enabled = params.get_bool("IntelligentCruiseButtonManagement")
+  if icbm_enabled and CP_SP.intelligentCruiseButtonManagementAvailable and not CP.openpilotLongitudinalControl:
+    CP_SP.pcmCruiseSpeed = False
+
+
 def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:
   CP = CI.CP
   CP_SP = CI.CP_SP
 
   _initialize_neural_network_lateral_control(CI, CP, CP_SP, params)
+  _initialize_intelligent_cruise_button_management(CP, CP_SP, params)
 
 
 def initialize_params(params) -> list[dict[str, Any]]:

sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -75,6 +75,8 @@ class ControlsExt:
 
     CC_SP.params = self.param_store.param_list
 
+    CC_SP.intelligentCruiseButtonManagement = sm['selfdriveStateSP'].intelligentCruiseButtonManagement
+
     return CC_SP
 
   @staticmethod

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -8,15 +8,19 @@ See the LICENSE.md file in the root directory for more details.
 from cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
+Source = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, mpc):
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.scc = SmartCruiseControl()
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
+    self.source = Source.cruise
 
   @property
   def mlsim(self) -> bool:
@@ -29,6 +33,19 @@ class LongitudinalPlannerSP:
 
     return self.dec.mode()
 
+  def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
+    self.scc.update(sm, v_ego, a_ego, v_cruise)
+
+    targets = {
+      Source.cruise: (v_cruise, a_ego),
+      Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target)
+    }
+
+    self.source = min(targets, key=lambda k: targets[k][0])
+    v_target, a_target = targets[self.source]
+
+    return v_target, a_target
+
   def update(self, sm: messaging.SubMaster) -> None:
     self.dec.update(sm)
 
@@ -38,6 +55,7 @@ class LongitudinalPlannerSP:
     plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
 
     longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
+    longitudinalPlanSP.longitudinalPlanSource = self.source
 
     # Dynamic Experimental Control
     dec = longitudinalPlanSP.dec
@@ -45,4 +63,16 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
+    # Smart Cruise Control
+    smartCruiseControl = longitudinalPlanSP.smartCruiseControl
+    # Vision Turn Speed Control
+    sccVision = smartCruiseControl.vision
+    sccVision.state = self.scc.vision.state
+    sccVision.vTarget = float(self.scc.vision.output_v_target)
+    sccVision.aTarget = float(self.scc.vision.output_a_target)
+    sccVision.currentLateralAccel = float(self.scc.vision.current_lat_acc)
+    sccVision.maxPredictedLateralAccel = float(self.scc.vision.max_pred_lat_acc)
+    sccVision.enabled = self.scc.vision.is_enabled
+    sccVision.active = self.scc.vision.is_active
+
     pm.send('longitudinalPlanSP', plan_sp_send)

sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py

@@ -0,0 +1,19 @@
+"""
+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.
+"""
+import cereal.messaging as messaging
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
+
+
+class SmartCruiseControl:
+  def __init__(self):
+    self.vision = SmartCruiseControlVision()
+
+  def update(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> None:
+    long_enabled = sm['carControl'].enabled
+    long_override = sm['carControl'].cruiseControl.override
+
+    self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)

sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py

@@ -0,0 +1,104 @@
+"""
+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.
+"""
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal import custom, log
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
+
+VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
+
+
+def generate_modelV2():
+  model = messaging.new_message('modelV2')
+  position = log.XYZTData.new_message()
+  speed = 30
+  position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)]
+  model.modelV2.position = position
+  orientation = log.XYZTData.new_message()
+  curvature = 0.05
+  orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS]
+  orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
+  model.modelV2.orientation = orientation
+  orientationRate = log.XYZTData.new_message()
+  orientationRate.z = [float(z) for z in ModelConstants.T_IDXS]
+  model.modelV2.orientationRate = orientationRate
+  velocity = log.XYZTData.new_message()
+  velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+  velocity.x[0] = float(speed)  # always start at current speed
+  model.modelV2.velocity = velocity
+  acceleration = log.XYZTData.new_message()
+  acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
+  acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)]
+  model.modelV2.acceleration = acceleration
+
+  return model
+
+
+def generate_carState():
+  car_state = messaging.new_message('carState')
+  speed = 30
+  v_cruise = 50
+  car_state.carState.vEgo = float(speed)
+  car_state.carState.standstill = False
+  car_state.carState.vCruise = float(v_cruise * 3.6)
+
+  return car_state
+
+
+def generate_controlsState():
+  controls_state = messaging.new_message('controlsState')
+  controls_state.controlsState.curvature = 0.05
+
+  return controls_state
+
+
+class TestSmartCruiseControlVision:
+
+  def setup_method(self):
+    self.params = Params()
+    self.reset_params()
+    self.scc_v = SmartCruiseControlVision()
+
+    mdl = generate_modelV2()
+    cs = generate_carState()
+    controls_state = generate_controlsState()
+    self.sm = {'modelV2': mdl.modelV2, 'carState': cs.carState, 'controlsState': controls_state.controlsState}
+
+  def reset_params(self):
+    self.params.put_bool("SmartCruiseControlVision", True)
+
+  def test_initial_state(self):
+    assert self.scc_v.state == VisionState.disabled
+    assert not self.scc_v.is_active
+    assert self.scc_v.output_v_target == V_CRUISE_UNSET
+    assert self.scc_v.output_a_target == 0.
+
+  def test_system_disabled(self):
+    self.params.put_bool("SmartCruiseControlVision", False)
+    self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision")
+
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_v.update(self.sm, True, False, 0., 0., 0.)
+    assert self.scc_v.state == VisionState.disabled
+    assert not self.scc_v.is_active
+
+  def test_disabled(self):
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_v.update(self.sm, False, False, 0., 0., 0.)
+    assert self.scc_v.state == VisionState.disabled
+
+  def test_transition_disabled_to_enabled(self):
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_v.update(self.sm, True, False, 0., 0., 0.)
+    assert self.scc_v.state == VisionState.enabled
+
+  # TODO-SP: mock modelV2 data to test other states

sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py

@@ -0,0 +1,205 @@
+"""
+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.
+"""
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal import custom
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+
+VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
+
+ACTIVE_STATES = (VisionState.entering, VisionState.turning, VisionState.leaving)
+ENABLED_STATES = (VisionState.enabled, VisionState.overriding, *ACTIVE_STATES)
+
+_MIN_V = 20 * CV.KPH_TO_MS  # Do not operate under 20 km/h
+
+_ENTERING_PRED_LAT_ACC_TH = 1.3  # Predicted Lat Acc threshold to trigger entering turn state.
+_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1  # Predicted Lat Acc threshold to abort entering state if speed drops.
+
+_TURNING_LAT_ACC_TH = 1.6  # Lat Acc threshold to trigger turning state.
+
+_LEAVING_LAT_ACC_TH = 1.3  # Lat Acc threshold to trigger leaving turn state.
+_FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger the end of the turn cycle.
+
+_A_LAT_REG_MAX = 2.  # Maximum lateral acceleration
+
+_NO_OVERSHOOT_TIME_HORIZON = 4.  # s. Time to use for velocity desired based on a_target when not overshooting.
+
+# Lookup table for the minimum smooth deceleration during the ENTERING state
+# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
+_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.]  # min decel value allowed on ENTERING state
+_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.]  # absolute value of lat acc ahead
+
+# Lookup table for the acceleration for the TURNING state
+# depending on the current lateral acceleration of the vehicle.
+_TURNING_ACC_V = [0.5, 0., -0.4]  # acc value
+_TURNING_ACC_BP = [1.5, 2.3, 3.]  # absolute value of current lat acc
+
+_LEAVING_ACC = 0.5  # Conformable acceleration to regain speed while leaving a turn.
+
+
+class SmartCruiseControlVision:
+  v_target: float = 0
+  a_target: float = 0.
+  v_ego: float = 0.
+  a_ego: float = 0.
+  output_v_target: float = V_CRUISE_UNSET
+  output_a_target: float = 0.
+
+  def __init__(self):
+    self.params = Params()
+    self.frame = -1
+    self.long_enabled = False
+    self.long_override = False
+    self.is_enabled = False
+    self.is_active = False
+    self.enabled = self.params.get_bool("SmartCruiseControlVision")
+    self.v_cruise_setpoint = 0.
+
+    self.state = VisionState.disabled
+    self.current_lat_acc = 0.
+    self.max_pred_lat_acc = 0.
+
+  def get_a_target_from_control(self) -> float:
+    return self.a_target
+
+  def get_v_target_from_control(self) -> float:
+    if self.is_active:
+      return max(self.v_target, _MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
+
+    return V_CRUISE_UNSET
+
+  def _update_params(self) -> None:
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.enabled = self.params.get_bool("SmartCruiseControlVision")
+
+  def _update_calculations(self, sm: messaging.SubMaster) -> None:
+    if not self.long_enabled:
+      return
+    else:
+      rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
+      vel_plan = np.array(sm['modelV2'].velocity.x)
+
+      self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
+
+      # get the maximum lat accel from the model
+      predicted_lat_accels = rate_plan * vel_plan
+      self.max_pred_lat_acc = np.amax(predicted_lat_accels)
+
+      # get the maximum curve based on the current velocity
+      v_ego = max(self.v_ego, 0.1)  # ensure a value greater than 0 for calculations
+      max_curve = self.max_pred_lat_acc / (v_ego**2)
+
+      # Get the target velocity for the maximum curve
+      self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
+
+  def _update_state_machine(self) -> tuple[bool, bool]:
+    # ENABLED, ENTERING, TURNING, LEAVING
+    if self.state != VisionState.disabled:
+      # longitudinal and feature disable always have priority in a non-disabled state
+      if not self.long_enabled or not self.enabled:
+        self.state = VisionState.disabled
+      elif self.long_override:
+        self.state = VisionState.overriding
+
+      else:
+        # ENABLED
+        if self.state == VisionState.enabled:
+          # Do not enter a turn control cycle if the speed is low.
+          if self.v_ego <= _MIN_V:
+            pass
+          # If significant lateral acceleration is predicted ahead, then move to Entering turn state.
+          elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
+            self.state = VisionState.entering
+
+        # OVERRIDING
+        elif self.state == VisionState.overriding:
+          if not self.long_override:
+            self.state = VisionState.enabled
+
+        # ENTERING
+        elif self.state == VisionState.entering:
+          # Transition to Turning if current lateral acceleration is over the threshold.
+          if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
+            self.state = VisionState.turning
+          # Abort if the predicted lateral acceleration drops
+          elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
+            self.state = VisionState.enabled
+
+        # TURNING
+        elif self.state == VisionState.turning:
+          # Transition to Leaving if current lateral acceleration drops below a threshold.
+          if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
+            self.state = VisionState.leaving
+
+        # LEAVING
+        elif self.state == VisionState.leaving:
+          # Transition back to Turning if current lateral acceleration goes back over the threshold.
+          if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
+            self.state = VisionState.turning
+          # Finish if current lateral acceleration goes below a threshold.
+          elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
+            self.state = VisionState.enabled
+
+    # DISABLED
+    elif self.state == VisionState.disabled:
+      if self.long_enabled and self.enabled:
+        if self.long_override:
+          self.state = VisionState.overriding
+        else:
+          self.state = VisionState.enabled
+
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+
+    return enabled, active
+
+  def _update_solution(self) -> float:
+    # DISABLED, ENABLED
+    if self.state not in ACTIVE_STATES:
+      # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
+      # the smooth deceleration.
+      a_target = self.a_ego
+    # ENTERING
+    elif self.state == VisionState.entering:
+      # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
+      a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
+    # TURNING
+    elif self.state == VisionState.turning:
+      # When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
+      a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
+    # LEAVING
+    elif self.state == VisionState.leaving:
+      # When leaving, we provide a comfortable acceleration to regain speed.
+      a_target = _LEAVING_ACC
+    else:
+      raise NotImplementedError(f"SCC-V state not supported: {self.state}")
+
+    return a_target
+
+  def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
+             v_cruise_setpoint: float) -> None:
+    self.long_enabled = long_enabled
+    self.long_override = long_override
+    self.v_ego = v_ego
+    self.a_ego = a_ego
+    self.v_cruise_setpoint = v_cruise_setpoint
+
+    self._update_params()
+    self._update_calculations(sm)
+
+    self.is_enabled, self.is_active = self._update_state_machine()
+    self.a_target = self._update_solution()
+
+    self.output_v_target = self.get_v_target_from_control()
+    self.output_a_target = self.get_a_target_from_control()
+
+    self.frame += 1