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

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

cereal/custom.capnp

@@ -122,9 +122,10 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
 
 struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   dec @0 :DynamicExperimentalControl;
-
-  events @1 :List(OnroadEventSP.Event);
-  slc @2 :SpeedLimitControl;
+  longitudinalPlanSource @1 :LongitudinalPlanSource;
+  smartCruiseControl @2 :SmartCruiseControl;
+  slc @3 :SpeedLimitControl;
+  events @4 :List(OnroadEventSP.Event);
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -137,6 +138,29 @@ 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.
+    }
+  }
+
   struct SpeedLimitControl {
     state @0 :SpeedLimitControlState;
     enabled @1 :Bool;
@@ -147,6 +171,11 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     source @6 :SpeedLimitSource;
   }
 
+  enum LongitudinalPlanSource {
+    cruise @0;
+    sccVision @1;
+  }
+
   enum SpeedLimitControlState {
     disabled @0;
     inactive @1; # No speed limit set or not enabled by parameter.

common/params_keys.h

@@ -159,6 +159,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/controls/lib/longitudinal_planner.py

@@ -146,8 +146,8 @@ 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 from Speed Limit Control
-    v_cruise = LongitudinalPlannerSP.update_v_cruise(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
+    # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit 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/modeld/fill_model_msg.py

@@ -3,6 +3,7 @@ import capnp
 import numpy as np
 from cereal import log
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
+from openpilot.sunnypilot.models.helpers import plan_x_idxs_helper
 
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
@@ -95,8 +96,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   # action
   modelV2.action = action
 
-  # times at X_IDXS of edges and lines aren't used
-  LINE_T_IDXS: list[float] = []
+  # times at X_IDXS of edges and lines
+  LINE_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data)
 
   # lane lines
   modelV2.init('laneLines', 4)

selfdrive/pandad/pandad.py

@@ -29,6 +29,12 @@ def flash_panda(panda_serial: str) -> Panda:
     HARDWARE.recover_internal_panda()
     raise
 
+  # skip flashing if the detected panda is not supported
+  supported_panda = check_panda_support(panda)
+  if not supported_panda:
+    cloudlog.warning(f"Panda {panda_serial} is not supported (hw_type: {panda.get_type()}), skipping flash...")
+    return panda
+
   fw_signature = get_expected_signature(panda)
   internal_panda = panda.is_internal()
 
@@ -36,12 +42,6 @@ def flash_panda(panda_serial: str) -> Panda:
   panda_signature = b"" if panda.bootstub else panda.get_signature()
   cloudlog.warning(f"Panda {panda_serial} connected, version: {panda_version}, signature {panda_signature.hex()[:16]}, expected {fw_signature.hex()[:16]}")
 
-  # skip flashing if the detected device is not supported from upstream
-  hw_type = panda.get_type()
-  if hw_type not in Panda.SUPPORTED_DEVICES:
-    cloudlog.warning(f"Panda {panda_serial} is not supported (hw_type: {hw_type}), skipping flash...")
-    return panda
-
   if panda.bootstub or panda_signature != fw_signature:
     cloudlog.info("Panda firmware out of date, update required")
     panda.flash()
@@ -67,6 +67,14 @@ def flash_panda(panda_serial: str) -> Panda:
   return panda
 
 
+def check_panda_support(panda) -> bool:
+  hw_type = panda.get_type()
+  if hw_type in Panda.SUPPORTED_DEVICES:
+    return True
+
+  return False
+
+
 def main() -> None:
   # signal pandad to close the relay and exit
   def signal_handler(signum, frame):
@@ -140,6 +148,12 @@ def main() -> None:
       params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas))
 
       for panda in pandas:
+        # skip health check if the detected panda is not supported
+        supported_panda = check_panda_support(panda)
+        if not supported_panda:
+          cloudlog.warning(f"Panda {panda.get_usb_serial()} is not supported (hw_type: {panda.get_type()}), skipping health check...")
+          continue
+
         # check health for lost heartbeat
         health = panda.health()
         if health["heartbeat_lost"]:

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/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/modeld/fill_model_msg.py

@@ -3,6 +3,7 @@ import capnp
 import numpy as np
 from cereal import log
 from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan
+from openpilot.sunnypilot.models.helpers import plan_x_idxs_helper
 from openpilot.sunnypilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_lag_adjusted_curvature, MIN_SPEED
 
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -120,23 +121,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   modelV2_action.desiredCurvature = desired_curvature
 
   # times at X_IDXS according to model plan
-  PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
-  PLAN_T_IDXS[0] = 0.0
-  plan_x = net_output_data['plan'][0,:,Plan.POSITION][:,0].tolist()
-  for xidx in range(1, ModelConstants.IDX_N):
-    tidx = 0
-    # increment tidx until we find an element that's further away than the current xidx
-    while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx+1] < ModelConstants.X_IDXS[xidx]:
-      tidx += 1
-    if tidx == ModelConstants.IDX_N - 1:
-      # if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
-      PLAN_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
-      break
-    # interpolate to find `t` for the current xidx
-    current_x_val = plan_x[tidx]
-    next_x_val = plan_x[tidx+1]
-    p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(next_x_val - current_x_val) > 1e-9 else float('nan')
-    PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx]
+  PLAN_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data)
 
   # lane lines
   modelV2.init('laneLines', 4)

sunnypilot/modeld_v2/fill_model_msg.py

@@ -3,6 +3,7 @@ import capnp
 import numpy as np
 from cereal import log
 from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
+from openpilot.sunnypilot.models.helpers import plan_x_idxs_helper
 from openpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan
 
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -118,8 +119,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   # action (includes lateral planning now)
   modelV2.action = action
 
-  # times at X_IDXS of edges and lines aren't used
-  LINE_T_IDXS: list[float] = []
+  # times at X_IDXS of edges and lines
+  LINE_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data)
 
   # lane lines
   modelV2.init('laneLines', 4)

sunnypilot/models/helpers.py

@@ -185,3 +185,27 @@ def load_meta_constants(model_metadata):
       meta = MetaTombRaider
 
   return meta
+
+
+# The following method(s) are modeld helper methods
+def plan_x_idxs_helper(constants, plan, model_output) -> list[float]:
+  # times at X_IDXS according to plan.
+  LINE_T_IDXS = [np.nan] * constants.IDX_N
+  LINE_T_IDXS[0] = 0.0
+  plan_x = model_output['plan'][0, :, plan.POSITION][:, 0].tolist()
+  for xidx in range(1, constants.IDX_N):
+    tidx = 0
+    # increment tidx until we find an element that's further away than the current xidx
+    while tidx < constants.IDX_N - 1 and plan_x[tidx + 1] < constants.X_IDXS[xidx]:
+      tidx += 1
+    if tidx == constants.IDX_N - 1:
+      # if the plan doesn't extend far enough, set plan_t to the max value (10s), then break
+      LINE_T_IDXS[xidx] = constants.T_IDXS[constants.IDX_N - 1]
+      break
+    # interpolate to find `t` for the current xidx
+    current_x_val = plan_x[tidx]
+    next_x_val = plan_x[tidx + 1]
+    p = (constants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(
+      next_x_val - current_x_val) > 1e-9 else float('nan')
+    LINE_T_IDXS[xidx] = p * constants.T_IDXS[tidx + 1] + (1 - p) * constants.T_IDXS[tidx]
+  return LINE_T_IDXS

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -8,12 +8,14 @@ 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.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
+Source = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 
 class LongitudinalPlannerSP:
@@ -23,8 +25,10 @@ class LongitudinalPlannerSP:
     self.resolver = SpeedLimitResolver()
 
     self.dec = DynamicExperimentalController(CP, mpc)
-    self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
+    self.scc = SmartCruiseControl()
     self.slc = SpeedLimitController(CP)
+    self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
+    self.source = Source.cruise
 
   @property
   def mlsim(self) -> bool:
@@ -37,17 +41,26 @@ class LongitudinalPlannerSP:
 
     return self.dec.mode()
 
-  def update_v_cruise(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> float:
+  def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     self.events_sp.clear()
 
+    self.scc.update(sm, v_ego, a_ego, v_cruise)
+
     # Speed Limit Control
     self.resolver.update(v_ego, sm)
     v_cruise_slc = self.slc.update(sm['carControl'].longActive, v_ego, a_ego, sm['carState'].vCruiseCluster,
                                    self.resolver.speed_limit, self.resolver.distance, self.resolver.source, self.events_sp)
 
-    v_cruise_final = min(v_cruise, v_cruise_slc)
+    targets = {
+      Source.cruise: (v_cruise, a_ego),
+      Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target),
+      Source.sla: (v_cruise_slc, a_ego),
+    }
 
-    return v_cruise_final
+    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)
@@ -58,6 +71,7 @@ class LongitudinalPlannerSP:
     plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
 
     longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
+    longitudinalPlanSP.longitudinalPlanSource = self.source
     longitudinalPlanSP.events = self.events_sp.to_msg()
 
     # Dynamic Experimental Control
@@ -66,6 +80,18 @@ 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
+
     # Speed Limit Control
     slc = longitudinalPlanSP.slc
     slc.state = self.slc.state

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