Smart Cruise Control: Vision (SCC-V) (#997)

* Controls: Vision Turn Speed Control

* fix

* Data type temp fix

* format

* more

* even more

* self contain targets

* state cleanup

* fix

* param updates

* no need

* use similar state machine

* raise exception if not found

* new state

* entirely internal

* use long active

* more

* rename and expose aTarget

* rename to SCC-V

* init tests

* slight tests

* expose toggle

* lint

* todo

* remove lat planner sub and mock sm data

* introduce aTarget

* rename

* rename

* update fill_model_msg.py to calculate PLAN_T_IDXS for lanelines and road edges

* sync upstream

* no SCC-V yet

* Revert "no SCC-V yet"

This reverts commit b67281bcac.

* wrap it with SCC main

* leave enabled out of here

* wat

* enabled and active on cereal

* OP long for now, enable for ICBM once merged

* need this

* unused

* let's go hybrid

* fix

* add override state

* update tests

* huh

* don't math here if not enabled

* ui: Smart Cruise Control - Vision (SCC-V) (#1253)

* vtsc-ui

* slight cleanup

* more cleanup

* unused

* a bit more

* pulse like it's hot

* draw only enabled and active

* let's try this for now

* settle

* finalize UI

* brighter color so we blind devtekve

* add long override

---------

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>

* slight cleanup

* more

* type hints

---------

Co-authored-by: discountchubbs <alexgrant990@gmail.com>
Co-authored-by: Kumar <36933347+rav4kumar@users.noreply.github.com>

cereal/custom.capnp

@@ -137,10 +137,31 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   }
 
   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;
   }
 }
 

common/params_keys.h

@@ -159,6 +159,7 @@ 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

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

@@ -18,6 +18,13 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
   cruisePanelScroller = new ScrollViewSP(list, this);
   vlayout->addWidget(cruisePanelScroller);
 
+  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);
 
@@ -75,5 +82,7 @@ void LongitudinalPanel::refresh(bool _offroad) {
   customAccIncrement->setEnabled(has_longitudinal_control && !is_pcm_cruise && !offroad);
   customAccIncrement->refresh();
 
+  SmartCruiseControlVision->setEnabled(has_longitudinal_control);
+
   offroad = _offroad;
 }

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

@@ -29,4 +29,5 @@ private:
   ScrollViewSP *cruisePanelScroller = nullptr;
   QWidget *cruisePanelScreen = nullptr;
   CustomAccIncrement *customAccIncrement = nullptr;
+  ParamControl *SmartCruiseControlVision;
 };

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;
@@ -78,11 +80,30 @@ void HudRendererSP::updateState(const UIState &s) {
 
   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);
@@ -112,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));

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

@@ -26,6 +26,8 @@ private:
   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;
@@ -57,4 +59,8 @@ private:
   bool standstillTimer;
   bool isStandstill;
   float standstillElapsedTime;
+  bool longOverride;
+  bool smartCruiseControlVisionEnabled;
+  bool smartCruiseControlVisionActive;
+  int smartCruiseControlVisionFrame;
 };

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -37,7 +37,8 @@ class LongitudinalPlannerSP:
     self.scc.update(sm, v_ego, a_ego, v_cruise)
 
     targets = {
-      Source.cruise : (v_cruise, a_ego),
+      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])
@@ -63,6 +64,15 @@ class LongitudinalPlannerSP:
     dec.active = self.dec.active()
 
     # Smart Cruise Control
-    smartCruiseControl = longitudinalPlanSP.smartCruiseControl  # noqa: F841
+    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

@@ -4,10 +4,16 @@ 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 messaging
+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):
-    pass
+  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