diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 55264c8c21..f34e0b1828 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -124,7 +124,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { dec @0 :DynamicExperimentalControl; longitudinalPlanSource @1 :LongitudinalPlanSource; smartCruiseControl @2 :SmartCruiseControl; - slc @3 :SpeedLimitControl; + speedLimitAssist @3 :SpeedLimitAssist; events @4 :List(OnroadEventSP.Event); struct DynamicExperimentalControl { @@ -161,8 +161,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { } } - struct SpeedLimitControl { - state @0 :SpeedLimitControlState; + struct SpeedLimitAssist { + state @0 :SpeedLimitAssistState; enabled @1 :Bool; active @2 :Bool; speedLimit @3 :Float32; @@ -174,9 +174,10 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { enum LongitudinalPlanSource { cruise @0; sccVision @1; + speedLimitAssist @2; } - enum SpeedLimitControlState { + enum SpeedLimitAssistState { disabled @0; inactive @1; # No speed limit set or not enabled by parameter. preActive @2; diff --git a/common/params_keys.h b/common/params_keys.h index 4f53927383..0eaf6c2331 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -226,8 +226,8 @@ inline static std::unordered_map keys = { {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}}, // Speed Limit Control - {"SpeedLimitControl", {PERSISTENT | BACKUP, BOOL, "0"}}, - {"SpeedLimitControlPolicy", {PERSISTENT | BACKUP, INT, "3"}}, + {"SpeedLimitAssist", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"SpeedLimitAssistPolicy", {PERSISTENT | BACKUP, INT, "3"}}, {"SpeedLimitEngageType", {PERSISTENT | BACKUP, INT, "0"}}, {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}}, {"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}}, diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc index ac7b57e297..7b2ab508ab 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc @@ -25,6 +25,13 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) { ""); list->addItem(SmartCruiseControlVision); + speedLimitAssist = new ParamControl( + "SpeedLimitAssist", + tr("Speed Limit Assist (SLA)"), + tr("When you engage ACC, you will be prompted to set the cruising speed to the speed limit of the road adjusted by the Offset and Source Policy specified, or the current driving speed. The maximum cruising speed will always be the MAX set speed."), + ""); + list->addItem(speedLimitAssist); + customAccIncrement = new CustomAccIncrement("CustomAccIncrementsEnabled", tr("Custom ACC Speed Increments"), "", "", this); list->addItem(customAccIncrement); @@ -83,6 +90,7 @@ void LongitudinalPanel::refresh(bool _offroad) { customAccIncrement->refresh(); SmartCruiseControlVision->setEnabled(has_longitudinal_control); + speedLimitAssist->setEnabled(has_longitudinal_control); 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 36c35720e7..f497472ac2 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h @@ -30,4 +30,5 @@ private: QWidget *cruisePanelScreen = nullptr; CustomAccIncrement *customAccIncrement = nullptr; ParamControl *SmartCruiseControlVision; + ParamControl *speedLimitAssist; }; diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 97329e4659..e20e80f99c 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -9,8 +9,8 @@ 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.controls.lib.speed_limit_assist.speed_limit_assist import SpeedLimitAssist +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_resolver import SpeedLimitResolver from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP from openpilot.sunnypilot.models.helpers import get_active_bundle diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/__init__.py similarity index 91% rename from sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py rename to sunnypilot/selfdrive/controls/lib/speed_limit_assist/__init__.py index ce4ae1c63c..2ac5b6a6f0 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/__init__.py @@ -1,6 +1,6 @@ from cereal import custom -SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState +SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimitAssistState PARAMS_UPDATE_PERIOD = 3. # secs. Time between parameter updates. DISABLED_GUARD_PERIOD = 2 # secs. @@ -14,7 +14,7 @@ LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on lim LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state. LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers. -# Speed Limit Control Auto mode constants +# Speed Limit Assist Auto mode constants REQUIRED_INITIAL_MAX_SET_SPEED = 35.7632 # m/s 80 MPH # TODO-SP: customizable with params CRUISE_SPEED_TOLERANCE = 0.44704 # m/s ±1 MPH tolerance # TODO-SP: metric vs imperial FALLBACK_CRUISE_SPEED = 255.0 # m/s fallback when no speed limit available diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/common.py similarity index 100% rename from sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py rename to sunnypilot/selfdrive/controls/lib/speed_limit_assist/common.py diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py similarity index 75% rename from sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py rename to sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py index 57c3284f0a..de9ebecaac 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py @@ -11,21 +11,21 @@ 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.selfdrive.controls.lib.speed_limit_controller import PARAMS_UPDATE_PERIOD, LIMIT_SPEED_OFFSET_TH, \ - SpeedLimitControlState, PRE_ACTIVE_GUARD_PERIOD, REQUIRED_INITIAL_MAX_SET_SPEED, CRUISE_SPEED_TOLERANCE, DISABLED_GUARD_PERIOD +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import PARAMS_UPDATE_PERIOD, LIMIT_SPEED_OFFSET_TH, \ + SpeedLimitAssistState, PRE_ACTIVE_GUARD_PERIOD, REQUIRED_INITIAL_MAX_SET_SPEED, CRUISE_SPEED_TOLERANCE, DISABLED_GUARD_PERIOD from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import OffsetType +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import OffsetType from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP from openpilot.selfdrive.modeld.constants import ModelConstants EventNameSP = custom.OnroadEventSP.EventName SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource -ACTIVE_STATES = (SpeedLimitControlState.active, SpeedLimitControlState.adapting) -ENABLED_STATES = (SpeedLimitControlState.preActive, SpeedLimitControlState.pending, *ACTIVE_STATES) +ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting) +ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES) -class SpeedLimitController: +class SpeedLimitAssist: _speed_limit: float _distance: float _source: custom.LongitudinalPlanSP.SpeedLimitSource @@ -41,7 +41,7 @@ class SpeedLimitController: self.long_engaged_timer = 0 self.pre_active_timer = 0 self.is_metric = self.params.get_bool("IsMetric") - self.enabled = self.params.get_bool("SpeedLimitControl") + self.enabled = self.params.get_bool("SpeedLimitAssist") self.op_engaged = False self.op_engaged_prev = False self.is_enabled = False @@ -57,8 +57,8 @@ class SpeedLimitController: self.last_valid_speed_limit_final = 0. self._distance = 0. self._source = SpeedLimitSource.none - self.state = SpeedLimitControlState.disabled - self._state_prev = SpeedLimitControlState.disabled + self.state = SpeedLimitAssistState.disabled + self._state_prev = SpeedLimitAssistState.disabled self.pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise self.offset_type = OffsetType(self.params.get("SpeedLimitOffsetType", return_default=True)) @@ -66,12 +66,12 @@ class SpeedLimitController: # Solution functions mapped to respective states self.acceleration_solutions = { - SpeedLimitControlState.disabled: self.get_current_acceleration_as_target, - SpeedLimitControlState.inactive: self.get_current_acceleration_as_target, - SpeedLimitControlState.preActive: self.get_current_acceleration_as_target, - SpeedLimitControlState.pending: self.get_current_acceleration_as_target, - SpeedLimitControlState.adapting: self.get_adapting_state_target_acceleration, - SpeedLimitControlState.active: self.get_active_state_target_acceleration, + SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target, + SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target, + SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target, + SpeedLimitAssistState.pending: self.get_current_acceleration_as_target, + SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration, + SpeedLimitAssistState.active: self.get_active_state_target_acceleration, } @property @@ -116,7 +116,7 @@ class SpeedLimitController: def update_params(self) -> None: if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: - self.enabled = self.params.get_bool("SpeedLimitControl") + self.enabled = self.params.get_bool("SpeedLimitAssist") self.offset_type = OffsetType(self.params.get("SpeedLimitOffsetType", return_default=True)) self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True) self.is_metric = self.params.get_bool("IsMetric") @@ -157,61 +157,61 @@ class SpeedLimitController: self.pre_active_timer = max(0, self.pre_active_timer - 1) # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE - if self.state != SpeedLimitControlState.disabled: + if self.state != SpeedLimitAssistState.disabled: if not self.op_engaged or not self.enabled: - self.state = SpeedLimitControlState.disabled + self.state = SpeedLimitAssistState.disabled self.initial_max_set = False else: # ACTIVE - if self.state == SpeedLimitControlState.active: + if self.state == SpeedLimitAssistState.active: if self.detect_manual_cruise_change(): - self.state = SpeedLimitControlState.inactive + self.state = SpeedLimitAssistState.inactive elif self._speed_limit > 0 and self.v_offset < LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitControlState.adapting + self.state = SpeedLimitAssistState.adapting # ADAPTING - elif self.state == SpeedLimitControlState.adapting: + elif self.state == SpeedLimitAssistState.adapting: if self.detect_manual_cruise_change(): - self.state = SpeedLimitControlState.inactive + self.state = SpeedLimitAssistState.inactive elif self.v_offset >= LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitControlState.active + self.state = SpeedLimitAssistState.active # PENDING - elif self.state == SpeedLimitControlState.pending: + elif self.state == SpeedLimitAssistState.pending: if self._speed_limit > 0: if self.v_offset < LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitControlState.adapting + self.state = SpeedLimitAssistState.adapting else: - self.state = SpeedLimitControlState.active + self.state = SpeedLimitAssistState.active # PRE_ACTIVE - elif self.state == SpeedLimitControlState.preActive: + elif self.state == SpeedLimitAssistState.preActive: if self.initial_max_set_confirmed(): self.initial_max_set = True if self._speed_limit > 0: if self.v_offset < LIMIT_SPEED_OFFSET_TH: - self.state = SpeedLimitControlState.adapting + self.state = SpeedLimitAssistState.adapting else: - self.state = SpeedLimitControlState.active + self.state = SpeedLimitAssistState.active else: - self.state = SpeedLimitControlState.pending + self.state = SpeedLimitAssistState.pending elif self.pre_active_timer <= PRE_ACTIVE_GUARD_PERIOD: # Timeout - session ended - self.state = SpeedLimitControlState.inactive + self.state = SpeedLimitAssistState.inactive # INACTIVE - elif self.state == SpeedLimitControlState.inactive: + elif self.state == SpeedLimitAssistState.inactive: pass # DISABLED - elif self.state == SpeedLimitControlState.disabled: + elif self.state == SpeedLimitAssistState.disabled: if self.op_engaged and self.enabled: if not self.op_engaged_prev: self.pre_active_timer = int(DISABLED_GUARD_PERIOD / DT_MDL) elif self.pre_active_timer <= 0: - self.state = SpeedLimitControlState.preActive + self.state = SpeedLimitAssistState.preActive self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) self.initial_max_set = False @@ -221,7 +221,7 @@ class SpeedLimitController: return enabled, active def update_events(self, events_sp: EventsSP) -> None: - if self.state == SpeedLimitControlState.preActive and self._state_prev != SpeedLimitControlState.preActive: + if self.state == SpeedLimitAssistState.preActive and self._state_prev != SpeedLimitAssistState.preActive: events_sp.add(EventNameSP.speedLimitPreActive) elif self.is_active: if self._state_prev not in ACTIVE_STATES: diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_resolver.py similarity index 92% rename from sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py rename to sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_resolver.py index 23c25dfd39..ddc3252992 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_resolver.py @@ -6,8 +6,8 @@ from cereal import custom from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC, PARAMS_UPDATE_PERIOD -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC, PARAMS_UPDATE_PERIOD +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import Policy SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource @@ -30,7 +30,7 @@ class SpeedLimitResolver: self._limit_solutions = {} # Store for speed limit solutions from different sources self._distance_solutions = {} # Store for distance to current speed limit start for different sources - self.policy = self.params.get("SpeedLimitControlPolicy", return_default=True) + self.policy = self.params.get("SpeedLimitAssistPolicy", return_default=True) self._policy_to_sources_map = { Policy.car_state_only: [SpeedLimitSource.car], Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map], @@ -43,7 +43,7 @@ class SpeedLimitResolver: def update_params(self): if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: - self.policy = Policy(self.params.get("SpeedLimitControlPolicy", return_default=True)) + self.policy = Policy(self.params.get("SpeedLimitAssistPolicy", return_default=True)) self.change_policy(self.policy) def change_policy(self, policy: Policy) -> None: diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/__init__.py similarity index 100% rename from sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/__init__.py rename to sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/__init__.py diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py similarity index 53% rename from sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_controller.py rename to sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py index 66aead5c68..fa17494d2e 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py @@ -15,15 +15,15 @@ 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.car import interfaces as sunnypilot_interfaces -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import OffsetType -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitControlState, REQUIRED_INITIAL_MAX_SET_SPEED, \ +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import OffsetType +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import SpeedLimitAssistState, REQUIRED_INITIAL_MAX_SET_SPEED, \ PRE_ACTIVE_GUARD_PERIOD -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController, ACTIVE_STATES +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_assist import SpeedLimitAssist, ACTIVE_STATES from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource -ALL_STATES = tuple(SpeedLimitControlState.schema.enumerants.values()) +ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values()) SPEED_LIMITS = { 'residential': 25 * CV.MPH_TO_MS, # 25 mph @@ -33,15 +33,15 @@ SPEED_LIMITS = { } -class TestSpeedLimitController: +class TestSpeedLimitAssist: def setup_method(self): self.params = Params() self.reset_custom_params() self.events_sp = EventsSP() CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2_2022) - self.slc = SpeedLimitController(CI.CP) - self.slc.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + self.sla = SpeedLimitAssist(CI.CP) + self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) def teardown_method(self, method): self.reset_state() @@ -55,103 +55,103 @@ class TestSpeedLimitController: return CI def reset_custom_params(self): - self.params.put_bool("SpeedLimitControl", True) + self.params.put_bool("SpeedLimitAssist", True) self.params.put_bool("IsMetric", False) self.params.put("SpeedLimitOffsetType", 0) self.params.put("SpeedLimitValueOffset", 0) def reset_state(self): - self.slc.state = SpeedLimitControlState.disabled - self.slc.frame = -1 - self.slc.last_op_engaged_frame = 0 - self.slc.op_engaged = False - self.slc.op_engaged_prev = False - self.slc.initial_max_set = False - self.slc._speed_limit = 0. - self.slc.speed_limit_prev = 0. - self.slc.last_valid_speed_limit_offsetted = 0. - self.slc._distance = 0. - self.slc._source = SpeedLimitSource.none - self.slc.v_cruise_setpoint = 0. - self.slc.v_cruise_setpoint_prev = 0. + self.sla.state = SpeedLimitAssistState.disabled + self.sla.frame = -1 + self.sla.last_op_engaged_frame = 0 + self.sla.op_engaged = False + self.sla.op_engaged_prev = False + self.sla.initial_max_set = False + self.sla._speed_limit = 0. + self.sla.speed_limit_prev = 0. + self.sla.last_valid_speed_limit_offsetted = 0. + self.sla._distance = 0. + self.sla._source = SpeedLimitSource.none + self.sla.v_cruise_setpoint = 0. + self.sla.v_cruise_setpoint_prev = 0. self.events_sp.clear() def initialize_active_state(self, v_cruise_setpoint): - self.slc.state = SpeedLimitControlState.active - self.slc.v_cruise_setpoint = v_cruise_setpoint - self.slc.v_cruise_setpoint_prev = v_cruise_setpoint + self.sla.state = SpeedLimitAssistState.active + self.sla.v_cruise_setpoint = v_cruise_setpoint + self.sla.v_cruise_setpoint_prev = v_cruise_setpoint def test_initial_state(self): - assert self.slc.state == SpeedLimitControlState.disabled - assert not self.slc.is_enabled - assert not self.slc.is_active - assert V_CRUISE_UNSET == self.slc.get_v_target_from_control() + assert self.sla.state == SpeedLimitAssistState.disabled + assert not self.sla.is_enabled + assert not self.sla.is_active + assert V_CRUISE_UNSET == self.sla.get_v_target_from_control() def test_disabled(self): - self.params.put_bool("SpeedLimitControl", False) + self.params.put_bool("SpeedLimitAssist", False) for _ in range(int(10. / DT_MDL)): - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state == SpeedLimitControlState.disabled + _ = self.sla.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.disabled def test_transition_disabled_to_preactive(self): for _ in range(int(3. / DT_MDL)): - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state == SpeedLimitControlState.preActive - assert self.slc.is_enabled and not self.slc.is_active + _ = self.sla.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.preActive + assert self.sla.is_enabled and not self.sla.is_active def test_preactive_to_active_with_max_speed_confirmation(self): - self.slc.state = SpeedLimitControlState.preActive - v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state == SpeedLimitControlState.active - assert self.slc.is_enabled and self.slc.is_active - assert v_cruise_slc == SPEED_LIMITS['city'] + self.sla.state = SpeedLimitAssistState.preActive + v_cruise_sla = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.active + assert self.sla.is_enabled and self.sla.is_active + assert v_cruise_sla == SPEED_LIMITS['city'] def test_preactive_timeout_to_inactive(self): - self.slc.state = SpeedLimitControlState.preActive - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + self.sla.state = SpeedLimitAssistState.preActive + _ = self.sla.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)): - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state == SpeedLimitControlState.inactive + _ = self.sla.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.inactive def test_preactive_to_pending_no_speed_limit(self): - self.slc.state = SpeedLimitControlState.preActive - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.none, self.events_sp) - assert self.slc.state == SpeedLimitControlState.pending - assert self.slc.is_enabled and not self.slc.is_active + self.sla.state = SpeedLimitAssistState.preActive + _ = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.none, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.pending + assert self.sla.is_enabled and not self.sla.is_active def test_pending_to_active_when_speed_limit_available(self): - self.slc.state = SpeedLimitControlState.pending - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state == SpeedLimitControlState.active + self.sla.state = SpeedLimitAssistState.pending + _ = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.active def test_pending_to_adapting_when_below_speed_limit(self): - self.slc.state = SpeedLimitControlState.pending - _ = self.slc.update(True, SPEED_LIMITS['city'] + 5, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state == SpeedLimitControlState.adapting - assert self.slc.is_enabled and self.slc.is_active + self.sla.state = SpeedLimitAssistState.pending + _ = self.sla.update(True, SPEED_LIMITS['city'] + 5, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.adapting + assert self.sla.is_enabled and self.sla.is_active def test_active_to_adapting_transition(self): self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) - _ = self.slc.update(True, SPEED_LIMITS['city'] + 2, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state == SpeedLimitControlState.adapting + _ = self.sla.update(True, SPEED_LIMITS['city'] + 2, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.adapting def test_adapting_to_active_transition(self): - self.slc.state = SpeedLimitControlState.adapting - self.slc.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED + self.sla.state = SpeedLimitAssistState.adapting + self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state == SpeedLimitControlState.active + _ = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.active def test_manual_cruise_change_detection(self): - self.slc.state = SpeedLimitControlState.active + self.sla.state = SpeedLimitAssistState.active expected_cruise = SPEED_LIMITS['highway'] - self.slc.v_cruise_setpoint_prev = expected_cruise + self.sla.v_cruise_setpoint_prev = expected_cruise different_cruise = SPEED_LIMITS['highway'] + 5 - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state == SpeedLimitControlState.inactive + _ = self.sla.update(True, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.inactive @pytest.mark.parametrize("offset_type, offset_value, speed_limit, expected_offset", [ (OffsetType.fixed, 5, SPEED_LIMITS['city'], 5 * CV.MPH_TO_MS), # 5 MPH fixed offset @@ -161,8 +161,8 @@ class TestSpeedLimitController: (OffsetType.percentage, 5, SPEED_LIMITS['highway'], 0.05 * SPEED_LIMITS['highway']), # Different speed, percentage ]) def test_offset_calculations(self, offset_type, offset_value, speed_limit, expected_offset): - self.slc._speed_limit = speed_limit - actual_offset = self.slc.get_offset(offset_type, offset_value) + self.sla._speed_limit = speed_limit + actual_offset = self.sla.get_offset(offset_type, offset_value) assert actual_offset == pytest.approx(expected_offset, rel=0.01) def test_rapid_speed_limit_changes(self): @@ -170,78 +170,78 @@ class TestSpeedLimitController: speed_limits = [SPEED_LIMITS['city'], SPEED_LIMITS['highway'], SPEED_LIMITS['residential']] for _, speed_limit in enumerate(speed_limits): - _ = self.slc.update(True, speed_limit, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state in ACTIVE_STATES + _ = self.sla.update(True, speed_limit, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, SpeedLimitSource.car, self.events_sp) + assert self.sla.state in ACTIVE_STATES def test_invalid_speed_limits_handling(self): self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) - self.slc.last_valid_speed_limit_final = SPEED_LIMITS['city'] + self.sla.last_valid_speed_limit_final = SPEED_LIMITS['city'] invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS] for invalid_limit in invalid_limits: - v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, invalid_limit, 0, SpeedLimitSource.car, self.events_sp) - assert isinstance(v_cruise_slc, (int, float)) - assert v_cruise_slc == V_CRUISE_UNSET or v_cruise_slc > 0 + v_cruise_sla = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, invalid_limit, 0, SpeedLimitSource.car, self.events_sp) + assert isinstance(v_cruise_sla, (int, float)) + assert v_cruise_sla == V_CRUISE_UNSET or v_cruise_sla > 0 def test_stale_data_handling(self): self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) old_speed_limit = SPEED_LIMITS['city'] - self.slc.last_valid_speed_limit_final = old_speed_limit + self.sla.last_valid_speed_limit_final = old_speed_limit - v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state in ACTIVE_STATES - assert v_cruise_slc == old_speed_limit + v_cruise_sla = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.car, self.events_sp) + assert self.sla.state in ACTIVE_STATES + assert v_cruise_sla == old_speed_limit def test_different_speed_limit_sources(self): self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) for source in (SpeedLimitSource.car, SpeedLimitSource.map): - v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, source, self.events_sp) - assert v_cruise_slc != V_CRUISE_UNSET + v_cruise_sla = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, source, self.events_sp) + assert v_cruise_sla != V_CRUISE_UNSET def test_distance_based_adapting(self): - self.slc.state = SpeedLimitControlState.adapting - self.slc.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED + self.sla.state = SpeedLimitAssistState.adapting + self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED distance = 100.0 current_speed = SPEED_LIMITS['highway'] target_speed = SPEED_LIMITS['city'] - v_cruise_slc = self.slc.update(True, current_speed, 0, REQUIRED_INITIAL_MAX_SET_SPEED, target_speed, distance, SpeedLimitSource.map, self.events_sp) - assert self.slc.state == SpeedLimitControlState.adapting - assert v_cruise_slc == target_speed # TODO-SP: assert expected accel, need to enable self.acceleration_solutions + v_cruise_sla = self.sla.update(True, current_speed, 0, REQUIRED_INITIAL_MAX_SET_SPEED, target_speed, distance, SpeedLimitSource.map, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.adapting + assert v_cruise_sla == target_speed # TODO-SP: assert expected accel, need to enable self.acceleration_solutions def test_long_disengaged_to_disabled(self): self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) - v_cruise_slc = self.slc.update(False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], + v_cruise_sla = self.sla.update(False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state == SpeedLimitControlState.disabled - assert v_cruise_slc == V_CRUISE_UNSET + assert self.sla.state == SpeedLimitAssistState.disabled + assert v_cruise_sla == V_CRUISE_UNSET def test_maintain_states_with_no_changes(self): """Test that states are maintained when no significant changes occur""" test_states = [ - SpeedLimitControlState.preActive, - SpeedLimitControlState.pending, - SpeedLimitControlState.active, - SpeedLimitControlState.adapting + SpeedLimitAssistState.preActive, + SpeedLimitAssistState.pending, + SpeedLimitAssistState.active, + SpeedLimitAssistState.adapting ] for state in test_states: - self.slc.state = state - self.slc.op_engaged = True - if state in [SpeedLimitControlState.pending, SpeedLimitControlState.active, SpeedLimitControlState.adapting]: - self.slc.initial_max_set = True + self.sla.state = state + self.sla.op_engaged = True + if state in [SpeedLimitAssistState.pending, SpeedLimitAssistState.active, SpeedLimitAssistState.adapting]: + self.sla.initial_max_set = True initial_state = state - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED,SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + _ = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED,SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) - assert self.slc.state in ALL_STATES # Sanity check + assert self.sla.state in ALL_STATES # Sanity check - if initial_state == SpeedLimitControlState.preActive: - assert self.slc.state in [SpeedLimitControlState.preActive, SpeedLimitControlState.active] + if initial_state == SpeedLimitAssistState.preActive: + assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active] elif initial_state in ACTIVE_STATES: - assert self.slc.state in ACTIVE_STATES + assert self.sla.state in ACTIVE_STATES diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_resolver.py similarity index 93% rename from sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py rename to sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_resolver.py index e01a60164d..14433dc293 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_resolver.py @@ -5,10 +5,10 @@ import pytest from pytest_mock import MockerFixture from cereal import custom -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import LIMIT_MAX_MAP_DATA_AGE -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import Policy SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index b40e86ad9f..035c5f13ee 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -17,7 +17,7 @@ EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()} def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - speedLimit = sm['longitudinalPlanSP'].slc.speedLimit + speedLimit = sm['longitudinalPlanSP'].sla.speedLimit speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)) message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit' return Alert(