From ea6178e53ee291c51574c0a6b7eb441637ae6b12 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Sep 2025 22:54:45 -0400 Subject: [PATCH] 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 | 17 ++ common/params_keys.h | 6 +- .../qt/offroad/settings/longitudinal_panel.cc | 8 + .../qt/offroad/settings/longitudinal_panel.h | 1 + selfdrive/ui/sunnypilot/qt/onroad/hud.cc | 24 +- selfdrive/ui/sunnypilot/qt/onroad/hud.h | 3 + .../controls/lib/longitudinal_planner.py | 12 +- .../lib/smart_cruise_control/__init__.py | 9 + .../smart_cruise_control/map_controller.py | 261 ++++++++++++++++++ .../smart_cruise_control.py | 3 + .../tests/test_map_controller.py | 58 ++++ .../smart_cruise_control/vision_controller.py | 8 +- 12 files changed, 396 insertions(+), 14 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py create mode 100644 sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index c4360ae31..854988dec 100644 --- a/cereal/custom.capnp +++ b/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; } } diff --git a/common/params_keys.h b/common/params_keys.h index 32a7695b3..3faae9ce1 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -160,7 +160,6 @@ inline static std::unordered_map 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 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"}}, }; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc index 8faa907a6..8987f3851 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc +++ b/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; } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h index 1e26ea22f..2fd15e955 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h +++ b/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; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index 33462e620..6da6baef3 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/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) { diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index bae1c3b71..1e153f154 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/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; diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 3e4dbd9ab..af5f5dbd7 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py index e69de29bb..271f49dcc 100644 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py new file mode 100644 index 000000000..cd45870ef --- /dev/null +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py index c66c2c392..0f6062165 100644 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py +++ b/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) diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py new file mode 100644 index 000000000..537f0033f --- /dev/null +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py index 491b17903..d144178e9 100644 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py +++ b/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: