mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-24 01:12:06 +08:00
Speed Limit Control -> Speed Limit Assist
This commit is contained in:
Jason Wen
+5
-4
@@ -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;
|
||||
|
||||
@@ -226,8 +226,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -30,4 +30,5 @@ private:
|
||||
QWidget *cruisePanelScreen = nullptr;
|
||||
CustomAccIncrement *customAccIncrement = nullptr;
|
||||
ParamControl *SmartCruiseControlVision;
|
||||
ParamControl *speedLimitAssist;
|
||||
};
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
+2
-2
@@ -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
|
||||
+36
-36
@@ -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:
|
||||
+4
-4
@@ -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:
|
||||
+99
-99
@@ -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
|
||||
+3
-3
@@ -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
|
||||
|
||||
@@ -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(
|
||||
|
||||
Reference in New Issue
Block a user