Smart Cruise Control: Map (SCC-M) (#1280)

* init

* more

* a bit more

* expose

* bruh

* some fixes

* ui

* lint

* Update map_controller.py

* add overriding

* draw in orders

cereal/custom.capnp

@@ -162,6 +162,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
 
   struct SmartCruiseControl {
     vision @0 :Vision;
+    map @1 :Map;
 
     struct Vision {
       state @0 :VisionState;
@@ -173,6 +174,14 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
       active @6 :Bool;
     }
 
+    struct Map {
+      state @0 :MapState;
+      vTarget @1 :Float32;
+      aTarget @2 :Float32;
+      enabled @3 :Bool;
+      active @4 :Bool;
+    }
+
     enum VisionState {
       disabled @0; # System disabled or inactive.
       enabled @1; # No predicted substantial turn on vision range.
@@ -181,6 +190,13 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
       leaving @4; # Road ahead straightens. Start to allow positive acceleration.
       overriding @5; # System overriding with manual control.
     }
+
+    enum MapState {
+      disabled @0; # System disabled or inactive.
+      enabled @1; # No predicted substantial turn on map range.
+      turning @2; # Actively turning. Managing acceleration to provide a roll on turn feeling.
+      overriding @3; # System overriding with manual control.
+    }
   }
 
   struct SpeedLimit {
@@ -203,6 +219,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   enum LongitudinalPlanSource {
     cruise @0;
     sccVision @1;
+    sccMap @2;
   }
 }
 

common/params_keys.h

@@ -160,7 +160,6 @@ 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
@@ -232,4 +231,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}},
     {"SpeedLimitPolicy", {PERSISTENT | BACKUP, INT, "3"}},
     {"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}},
+
+    // Smart Cruise Control
+    {"MapTargetVelocities", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
+    {"SmartCruiseControlMap", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"SmartCruiseControlVision", {PERSISTENT | BACKUP, BOOL, "0"}},
 };

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

@@ -50,6 +50,13 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
     "");
   list->addItem(SmartCruiseControlVision);
 
+  SmartCruiseControlMap = new ParamControl(
+    "SmartCruiseControlMap",
+    tr("Smart Cruise Control - Map"),
+    tr("Use map data to estimate the appropriate speed to drive through turns ahead."),
+    "");
+  list->addItem(SmartCruiseControlMap);
+
   customAccIncrement = new CustomAccIncrement("CustomAccIncrementsEnabled", tr("Custom ACC Speed Increments"), "", "", this);
   list->addItem(customAccIncrement);
 
@@ -136,6 +143,7 @@ void LongitudinalPanel::refresh(bool _offroad) {
   customAccIncrement->refresh();
 
   SmartCruiseControlVision->setEnabled(has_longitudinal_control || icbm_allowed);
+  SmartCruiseControlMap->setEnabled(has_longitudinal_control || icbm_allowed);
 
   offroad = _offroad;
 }

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

@@ -33,6 +33,7 @@ private:
   QWidget *cruisePanelScreen = nullptr;
   CustomAccIncrement *customAccIncrement = nullptr;
   ParamControl *SmartCruiseControlVision;
+  ParamControl *SmartCruiseControlMap;
   ParamControl *intelligentCruiseButtonManagement = nullptr;
   SpeedLimitSettings *speedLimitScreen;
   PushButtonSP *speedLimitSettings;

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

@@ -100,6 +100,8 @@ void HudRendererSP::updateState(const UIState &s) {
   longOverride = car_control.getCruiseControl().getOverride();
   smartCruiseControlVisionEnabled = lp_sp.getSmartCruiseControl().getVision().getEnabled();
   smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive();
+  smartCruiseControlMapEnabled = lp_sp.getSmartCruiseControl().getMap().getEnabled();
+  smartCruiseControlMapActive = lp_sp.getSmartCruiseControl().getMap().getActive();
 }
 
 void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
@@ -108,18 +110,28 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
     // Smart Cruise Control
     int x_offset = -260;
     int y1_offset = -80;
-    // int y2_offset = -140;  // reserved for 2 icons
+    int y2_offset = -140;
 
+    int y_scc_v = 0, y_scc_m = 0;
+    const int orders[2] = {y1_offset, y2_offset};
+    int i = 0;
+    // SCC-V takes first order
+    if (smartCruiseControlVisionEnabled) y_scc_v = orders[i++];
+    if (smartCruiseControlMapEnabled) y_scc_m = orders[i++];
+
+    // Smart Cruise Control - Vision
     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");
+      drawSmartCruiseControlOnroadIcon(p, surface_rect, x_offset, y_scc_v, "SCC-V");
     }
+    smartCruiseControlVisionFrame = smartCruiseControlVisionActive ? (smartCruiseControlVisionFrame + 1) : 0;
 
-    if (smartCruiseControlVisionActive) {
-      smartCruiseControlVisionFrame++;
-    } else {
-      smartCruiseControlVisionFrame = 0;
+    // Smart Cruise Control - Map
+    bool scc_map_active_pulse = pulseElement(smartCruiseControlMapFrame);
+    if ((smartCruiseControlMapEnabled && !smartCruiseControlMapActive) || (smartCruiseControlMapActive && scc_map_active_pulse)) {
+      drawSmartCruiseControlOnroadIcon(p, surface_rect, x_offset, y_scc_m, "SCC-M");
     }
+    smartCruiseControlMapFrame = smartCruiseControlMapActive ? (smartCruiseControlMapFrame + 1) : 0;
 
     // Bottom Dev UI
     if (devUiInfo == 2) {

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

@@ -69,6 +69,9 @@ private:
   bool smartCruiseControlVisionEnabled;
   bool smartCruiseControlVisionActive;
   int smartCruiseControlVisionFrame;
+  bool smartCruiseControlMapEnabled;
+  bool smartCruiseControlMapActive;
+  int smartCruiseControlMapFrame;
   float speedLimit;
   float speedLimitLast;
   float speedLimitOffset;

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -46,7 +46,8 @@ class LongitudinalPlannerSP:
 
     targets = {
       Source.cruise: (v_cruise, a_ego),
-      Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target)
+      Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target),
+      Source.sccMap: (self.scc.map.output_v_target, self.scc.map.output_a_target),
     }
 
     self.source = min(targets, key=lambda k: targets[k][0])
@@ -74,7 +75,7 @@ class LongitudinalPlannerSP:
 
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
-    # Vision Turn Speed Control
+    # Vision Control
     sccVision = smartCruiseControl.vision
     sccVision.state = self.scc.vision.state
     sccVision.vTarget = float(self.scc.vision.output_v_target)
@@ -83,6 +84,13 @@ class LongitudinalPlannerSP:
     sccVision.maxPredictedLateralAccel = float(self.scc.vision.max_pred_lat_acc)
     sccVision.enabled = self.scc.vision.is_enabled
     sccVision.active = self.scc.vision.is_active
+    # Map Control
+    sccMap = smartCruiseControl.map
+    sccMap.state = self.scc.map.state
+    sccMap.vTarget = float(self.scc.map.output_v_target)
+    sccMap.aTarget = float(self.scc.map.output_a_target)
+    sccMap.enabled = self.scc.map.is_enabled
+    sccMap.active = self.scc.map.is_active
 
     # Speed Limit
     speedLimit = longitudinalPlanSP.speedLimit

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

@@ -0,0 +1,9 @@
+"""
+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 openpilot.common.constants import CV
+
+MIN_V = 20 * CV.KPH_TO_MS  # Do not operate under 20 km/h

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

@@ -0,0 +1,261 @@
+import json
+import math
+import platform
+
+from cereal import custom
+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
+from openpilot.sunnypilot.navd.helpers import coordinate_from_param, Coordinate
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
+
+MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
+
+ACTIVE_STATES = (MapState.turning, MapState.overriding)
+ENABLED_STATES = (MapState.enabled, *ACTIVE_STATES)
+
+R = 6373000.0  # approximate radius of earth in meters
+TO_RADIANS = math.pi / 180
+TO_DEGREES = 180 / math.pi
+TARGET_JERK = -0.6  # m/s^3 There's some jounce limits that are not consistent so we're fudging this some
+TARGET_ACCEL = -1.2  # m/s^2 should match up with the long planner limit
+TARGET_OFFSET = 1.0  # seconds - This controls how soon before the curve you reach the target velocity. It also helps
+                     # reach the target velocity when inaccuracies in the distance modeling logic would cause overshoot.
+                     # The value is multiplied against the target velocity to determine the additional distance. This is
+                     # done to keep the distance calculations consistent but results in the offset actually being less
+                     # time than specified depending on how much of a speed differential there is between v_ego and the
+                     # target velocity.
+
+
+def velocities_from_param(param: str, params: Params):
+  if params is None:
+    params = Params()
+
+  json_str = params.get(param)
+  if json_str is None:
+    return None
+
+  velocities = json.loads(json_str)
+
+  return velocities
+
+
+def calculate_accel(t, target_jerk, a_ego):
+  return a_ego + target_jerk * t
+
+
+def calculate_velocity(t, target_jerk, a_ego, v_ego):
+  return v_ego + a_ego * t + target_jerk/2 * (t ** 2)
+
+
+def calculate_distance(t, target_jerk, a_ego, v_ego):
+  return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3)
+
+
+# points should be in radians
+# output is meters
+def distance_to_point(ax, ay, bx, by):
+  a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2)
+  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
+
+  return R * c  # in meters
+
+
+class SmartCruiseControlMap:
+  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.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
+    self.enabled = self.params.get_bool("SmartCruiseControlMap")
+    self.long_enabled = False
+    self.long_override = False
+    self.is_enabled = False
+    self.is_active = False
+    self.state = MapState.disabled
+    self.v_cruise = 0
+    self.target_lat = 0.0
+    self.target_lon = 0.0
+    self.frame = -1
+
+    self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
+    self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
+
+  def get_v_target_from_control(self) -> float:
+    if self.is_active:
+      return max(self.v_target, MIN_V)
+
+    return V_CRUISE_UNSET
+
+  def get_a_target_from_control(self) -> float:
+    return self.a_ego
+
+  def update_params(self):
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.enabled = self.params.get_bool("SmartCruiseControlMap")
+
+  def update_calculations(self) -> None:
+    self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
+    lat = self.last_position.latitude
+    lon = self.last_position.longitude
+
+    self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
+
+    if self.last_position is None or self.target_velocities is None:
+      return
+
+    min_dist = 1000
+    min_idx = 0
+    distances = []
+
+    # find our location in the path
+    for i in range(len(self.target_velocities)):
+      target_velocity = self.target_velocities[i]
+      tlat = target_velocity["latitude"]
+      tlon = target_velocity["longitude"]
+      d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS)
+      distances.append(d)
+      if d < min_dist:
+        min_dist = d
+        min_idx = i
+
+    # only look at values from our current position forward
+    forward_points = self.target_velocities[min_idx:]
+    forward_distances = distances[min_idx:]
+
+    # find velocities that we are within the distance we need to adjust for
+    valid_velocities = []
+    for i in range(len(forward_points)):
+      target_velocity = forward_points[i]
+      tlat = target_velocity["latitude"]
+      tlon = target_velocity["longitude"]
+      tv = target_velocity["velocity"]
+      if tv > self.v_ego:
+        continue
+
+      d = forward_distances[i]
+
+      a_diff = (self.a_ego - TARGET_ACCEL)
+      accel_t = abs(a_diff / TARGET_JERK)
+      min_accel_v = calculate_velocity(accel_t, TARGET_JERK, self.a_ego, self.v_ego)
+
+      max_d = 0
+      if tv > min_accel_v:
+        # calculate time needed based on target jerk
+        a = 0.5 * TARGET_JERK
+        b = self.a_ego
+        c = self.v_ego - tv
+        t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
+        t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
+        if not isinstance(t_a, complex) and t_a > 0:
+          t = t_a
+        else:
+          t = t_b
+        if isinstance(t, complex):
+          continue
+
+        max_d = max_d + calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
+      else:
+        t = accel_t
+        max_d = calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
+
+        # calculate additional time needed based on target accel
+        t = abs((min_accel_v - tv) / TARGET_ACCEL)
+        max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v)
+
+      if d < max_d + tv * TARGET_OFFSET:
+        valid_velocities.append((float(tv), tlat, tlon))
+
+    # Find the smallest velocity we need to adjust for
+    min_v = 100.0
+    target_lat = 0.0
+    target_lon = 0.0
+    for tv, lat, lon in valid_velocities:
+      if tv < min_v:
+        min_v = tv
+        target_lat = lat
+        target_lon = lon
+
+    if self.v_target < min_v and not (self.target_lat == 0 and self.target_lon == 0):
+      for i in range(len(forward_points)):
+        target_velocity = forward_points[i]
+        tlat = target_velocity["latitude"]
+        tlon = target_velocity["longitude"]
+        tv = target_velocity["velocity"]
+        if tv > self.v_ego:
+          continue
+
+        if tlat == self.target_lat and tlon == self.target_lon and tv == self.v_target:
+          return
+
+      # not found so let's reset
+      self.v_target = 0.0
+      self.target_lat = 0.0
+      self.target_lon = 0.0
+
+    self.v_target = min_v
+    self.target_lat = target_lat
+    self.target_lon = target_lon
+
+  def _update_state_machine(self) -> tuple[bool, bool]:
+    # ENABLED, TURNING
+    if self.state != MapState.disabled:
+      if not self.long_enabled or not self.enabled:
+        self.state = MapState.disabled
+      elif self.long_override:
+        self.state = MapState.overriding
+
+      else:
+        # ENABLED
+        if self.state == MapState.enabled:
+          if self.v_cruise > self.v_target != 0:
+            self.state = MapState.turning
+
+        # TURNING
+        elif self.state == MapState.turning:
+          if self.v_cruise <= self.v_target or self.v_target == 0:
+            self.state = MapState.enabled
+
+        # OVERRIDING
+        elif self.state == MapState.overriding:
+          if not self.long_override:
+            if self.v_cruise > self.v_target != 0:
+              self.state = MapState.turning
+            else:
+              self.state = MapState.enabled
+
+    # DISABLED
+    elif self.state == MapState.disabled:
+      if self.long_enabled and self.enabled:
+        if self.long_override:
+          self.state = MapState.overriding
+        else:
+          self.state = MapState.enabled
+
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+
+    return enabled, active
+
+  def update(self, long_enabled: bool, long_override: bool, v_ego, a_ego, v_cruise) -> None:
+    self.long_enabled = long_enabled
+    self.long_override = long_override
+    self.v_ego = v_ego
+    self.a_ego = a_ego
+    self.v_cruise = v_cruise
+
+    self.update_params()
+    self.update_calculations()
+
+    self.is_enabled, self.is_active = self._update_state_machine()
+
+    self.output_v_target = self.get_v_target_from_control()
+    self.output_a_target = self.get_a_target_from_control()
+
+    self.frame += 1

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

@@ -6,14 +6,17 @@ 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
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
 
 
 class SmartCruiseControl:
   def __init__(self):
     self.vision = SmartCruiseControlVision()
+    self.map = SmartCruiseControlMap()
 
   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.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise)
     self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)

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

@@ -0,0 +1,58 @@
+"""
+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 platform
+
+from cereal import custom
+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.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
+
+MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
+
+
+class TestSmartCruiseControlMap:
+
+  def setup_method(self):
+    self.params = Params()
+    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
+    self.reset_params()
+    self.scc_m = SmartCruiseControlMap()
+
+  def reset_params(self):
+    self.params.put_bool("SmartCruiseControlMap", True)
+
+    # TODO-SP: mock data from gpsLocation
+    self.params.put("LastGPSPosition", "{}")
+    self.params.put("MapTargetVelocities", "{}")
+
+  def test_initial_state(self):
+    assert self.scc_m.state == VisionState.disabled
+    assert not self.scc_m.is_active
+    assert self.scc_m.output_v_target == V_CRUISE_UNSET
+    assert self.scc_m.output_a_target == 0.
+
+  def test_system_disabled(self):
+    self.params.put_bool("SmartCruiseControlMap", False)
+    self.scc_m.enabled = self.params.get_bool("SmartCruiseControlMap")
+
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_m.update(True, False, 0., 0., 0.)
+    assert self.scc_m.state == VisionState.disabled
+    assert not self.scc_m.is_active
+
+  def test_disabled(self):
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_m.update(False, False, 0., 0., 0.)
+    assert self.scc_m.state == VisionState.disabled
+
+  def test_transition_disabled_to_enabled(self):
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_m.update(True, False, 0., 0., 0.)
+    assert self.scc_m.state == VisionState.enabled
+
+  # TODO-SP: mock data from modelV2 to test other states

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

@@ -8,19 +8,17 @@ 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
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
 
 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.
 
@@ -73,7 +71,7 @@ class SmartCruiseControlVision:
 
   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 max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
 
     return V_CRUISE_UNSET
 
@@ -114,7 +112,7 @@ class SmartCruiseControlVision:
         # ENABLED
         if self.state == VisionState.enabled:
           # Do not enter a turn control cycle if the speed is low.
-          if self.v_ego <= _MIN_V:
+          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: