diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py index 0ec7445ebb..9a28611259 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py @@ -24,6 +24,9 @@ class SpeedLimitController: _speed_limit: float _distance: float _source: Source + _v_ego: float + _a_ego: float + _v_offset: float def __init__(self, CP): self._params = Params() @@ -31,12 +34,11 @@ class SpeedLimitController: self._policy = self._read_policy_param() self._resolver = SpeedLimitResolver(self._policy) self._last_params_update = 0.0 - self._last_op_enabled_time = 0.0 + self._last_op_engaged_time = 0.0 self._is_metric = self._params.get_bool("IsMetric") self._is_enabled = self._params.get_bool("SpeedLimitControl") - self._disengage_on_accelerator = self._params.get_bool("DisengageOnAccelerator") - self._op_enabled = False - self._op_enabled_prev = False + self._op_engaged = False + self._op_engaged_prev = False self._v_ego = 0. self._a_ego = 0. self._v_offset = 0. @@ -59,8 +61,6 @@ class SpeedLimitController: self._warning_offset_type = OffsetType(self._read_int_param("SpeedLimitWarningOffsetType")) self._warning_offset_value = self._read_int_param("SpeedLimitWarningValueOffset") self._engage_type = self._read_engage_type_param() - self._brake_pressed = False - self._brake_pressed_prev = False self._current_time = 0. self._v_cruise_rounded = 0. self._v_cruise_prev_rounded = 0. @@ -196,8 +196,8 @@ class SpeedLimitController: # op enabling since controlsd will change the cruise speed every time on enabling and this will # cause a temp inactive transition if the controller is updated before controlsd sets actual cruise # speed. - if not self._op_enabled_prev and self._op_enabled: - self._last_op_enabled_time = self._current_time + if not self._op_engaged_prev and self._op_engaged: + self._last_op_engaged_time = self._current_time # Update change tracking variables self._speed_limit_changed = self._speed_limit != self._speed_limit_prev @@ -205,8 +205,7 @@ class SpeedLimitController: self._speed_limit_prev = self._speed_limit if self._engage_type != Engage.user_confirm: self._update_v_cruise_setpoint_prev() - self._op_enabled_prev = self._op_enabled - self._brake_pressed_prev = self._brake_pressed + self._op_engaged_prev = self._op_engaged self._v_cruise_rounded = int(round(self._v_cruise_setpoint * self._speed_factor)) self._v_cruise_prev_rounded = int(round(self._v_cruise_setpoint_prev * self._speed_factor)) @@ -217,10 +216,10 @@ class SpeedLimitController: def transition_state_from_inactive(self) -> None: """ Make state transition from inactive state """ if self._engage_type == Engage.user_confirm: - if (((self._last_op_enabled_time + 7.) >= self._current_time >= (self._last_op_enabled_time + 2.)) or + if (((self._last_op_engaged_time + 7.) >= self._current_time >= (self._last_op_engaged_time + 2.)) or self._speed_limit_changed): if self._speed_limit_changed: - self._last_op_enabled_time = self._current_time - 2. # immediately prompt confirmation + self._last_op_engaged_time = self._current_time - 2. # immediately prompt confirmation self.state = SpeedLimitControlState.preActive elif self._engage_type == Engage.auto: if self._v_offset < LIMIT_SPEED_OFFSET_TH: @@ -232,18 +231,18 @@ class SpeedLimitController: """ Make state transition from temporary inactive state """ if self._speed_limit_changed: if self._engage_type == Engage.user_confirm: - self._last_op_enabled_time = self._current_time - 2. # immediately prompt confirmation + self._last_op_engaged_time = self._current_time - 2. # immediately prompt confirmation self.state = SpeedLimitControlState.preActive elif self._engage_type == Engage.auto: self.state = SpeedLimitControlState.inactive def transition_state_from_pre_active(self) -> None: """ Make state transition from preActive state """ - if self._current_time >= (self._last_op_enabled_time + 7.): + if self._current_time >= (self._last_op_engaged_time + 7.): self.state = SpeedLimitControlState.inactive - elif (self._last_op_enabled_time + 7.) > self._current_time > (self._last_op_enabled_time + 2.): + elif (self._last_op_engaged_time + 7.) > self._current_time > (self._last_op_engaged_time + 2.): if self._speed_limit_changed: - self._last_op_enabled_time = self._current_time - 2. # immediately prompt confirmation + self._last_op_engaged_time = self._current_time - 2. # immediately prompt confirmation elif self._v_cruise_prev_rounded < self._speed_limit_offsetted_rounded: if self._v_cruise_setpoint > self._v_cruise_setpoint_prev: self.state = SpeedLimitControlState.active @@ -265,7 +264,7 @@ class SpeedLimitController: if self._v_cruise_setpoint_changed and self._v_cruise_rounded != self._speed_limit_offsetted_rounded: self.state = SpeedLimitControlState.tempInactive elif self._speed_limit_changed: - self._last_op_enabled_time = self._current_time - 2. # immediately prompt confirmation + self._last_op_engaged_time = self._current_time - 2. # immediately prompt confirmation self.state = SpeedLimitControlState.preActive elif self._engage_type == Engage.auto: if self._v_offset < LIMIT_SPEED_OFFSET_TH: @@ -276,8 +275,7 @@ class SpeedLimitController: # In any case, if op is disabled, or speed limit control is disabled # or the reported speed limit is 0 or gas is pressed, deactivate. - if not self._op_enabled or not self._is_enabled or self._speed_limit == 0 or \ - (self._gas_pressed and self._disengage_on_accelerator): + if not self._op_engaged or not self._is_enabled or self._speed_limit == 0: self.state = SpeedLimitControlState.inactive return @@ -285,7 +283,7 @@ class SpeedLimitController: # Ignore if a minimum amount of time has not passed since activation. This is to prevent temp inactivations # due to controlsd logic changing cruise setpoint when going active. if self._engage_type == Engage.auto and self._v_cruise_setpoint_changed and \ - self._current_time > (self._last_op_enabled_time + TEMP_INACTIVE_GUARD_PERIOD): + self._current_time > (self._last_op_engaged_time + TEMP_INACTIVE_GUARD_PERIOD): self.state = SpeedLimitControlState.tempInactive return @@ -296,18 +294,18 @@ class SpeedLimitController: def get_current_acceleration_as_target(self) -> float: """ When state is inactive or tempInactive, preserve current acceleration """ - return float(self._a_ego) + return self._a_ego def get_adapting_state_target_acceleration(self) -> float: """ In adapting state, calculate target acceleration based on speed limit and current velocity """ if self.distance > 0: - return float((self.speed_limit_offseted ** 2 - self._v_ego ** 2) / (2. * self.distance)) + return (self.speed_limit_offseted ** 2 - self._v_ego ** 2) / (2. * self.distance) - return float(self._v_offset / ModelConstants.T_IDXS[CONTROL_N]) + return self._v_offset / float(ModelConstants.T_IDXS[CONTROL_N]) def get_active_state_target_acceleration(self) -> float: """ In active state, aim to keep speed constant around control time horizon """ - return float(self._v_offset / ModelConstants.T_IDXS[CONTROL_N]) + return self._v_offset / float(ModelConstants.T_IDXS[CONTROL_N]) def _update_events(self, events_sp: EventsSP) -> None: if self._speed_limit > 0 and self._warning_type == 2 and \ @@ -331,14 +329,11 @@ class SpeedLimitController: def update(self, enabled: bool, v_ego: float, a_ego: float, sm: messaging.SubMaster, v_cruise_setpoint: float, events_sp: EventsSP) -> None: _car_state = sm['carState'] - self._op_enabled = enabled and \ - not (_car_state.brakePressed and (not self._brake_pressed_prev or not _car_state.standstill)) and \ - not (_car_state.gasPressed and self._disengage_on_accelerator) + self._op_engaged = enabled and self._CP.openpilotLongitudinalControl self._v_ego = v_ego self._a_ego = a_ego self._v_cruise_setpoint = v_cruise_setpoint self._gas_pressed = _car_state.gasPressed - self._brake_pressed = _car_state.brakePressed self._current_time = time.monotonic() self._speed_limit, self._distance, self._source = self._resolver.resolve(v_ego, self.speed_limit, sm) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py index 65c1a97c9f..cae5f2ea9c 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py @@ -1,14 +1,19 @@ import time import numpy as np +from cereal import messaging, custom + from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import debug class SpeedLimitResolver: + _sm: messaging.SubMaster _limit_solutions: dict[Source, float] # Store for speed limit solutions from different sources _distance_solutions: dict[Source, float] # Store for distance to current speed limit start for different sources + _v_ego: float + _current_speed_limit: float def __init__(self, policy: Policy): self._limit_solutions = {} @@ -25,17 +30,17 @@ class SpeedLimitResolver: for source in Source: self._reset_limit_sources(source) - def change_policy(self, policy: Policy): + def change_policy(self, policy: Policy) -> None: self._policy = policy - def _reset_limit_sources(self, source): + def _reset_limit_sources(self, source: Source) -> None: self._limit_solutions[source] = 0. self._distance_solutions[source] = 0. - def _is_sock_updated(self, sock): + def _is_sock_updated(self, sock: str) -> bool: return self._sm.alive[sock] and self._sm.updated[sock] - def resolve(self, v_ego, current_speed_limit, sm): + def resolve(self, v_ego: float, current_speed_limit: float, sm: messaging.SubMaster) -> tuple[float, float, Source]: self._v_ego = v_ego self._current_speed_limit = current_speed_limit self._sm = sm @@ -43,12 +48,12 @@ class SpeedLimitResolver: self._resolve_limit_sources() return self._consolidate() - def _resolve_limit_sources(self): + def _resolve_limit_sources(self) -> None: """Get limit solutions from each data source""" self._get_from_car_state() self._get_from_map_data() - def _get_from_car_state(self): + def _get_from_car_state(self) -> None: if not self._is_sock_updated('carStateSP'): debug('SL: No carStateSP instruction for speed limit') return @@ -57,18 +62,16 @@ class SpeedLimitResolver: self._limit_solutions[Source.car_state] = self._sm['carStateSP'].speedLimit self._distance_solutions[Source.car_state] = 0. - def _get_from_map_data(self): - sock = 'liveMapDataSP' - - if not self._is_sock_updated(sock): + def _get_from_map_data(self) -> None: + if not self._is_sock_updated("liveMapDataSP"): debug('SL: No map data for speed limit') return # Load limits from map_data self._reset_limit_sources(Source.map_data) - self._process_map_data(self._sm[sock]) + self._process_map_data(self._sm["liveMapDataSP"]) - def _process_map_data(self, map_data): + def _process_map_data(self, map_data: custom.LiveMapDataSP) -> None: gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3 if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE: debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}') @@ -79,7 +82,7 @@ class SpeedLimitResolver: self._calculate_map_data_limits(speed_limit, next_speed_limit, map_data) - def _calculate_map_data_limits(self, speed_limit, next_speed_limit, map_data): + def _calculate_map_data_limits(self, speed_limit: float, next_speed_limit: float, map_data: custom.LiveMapDataSP) -> None: distance_since_fix = self._v_ego * (time.time() - map_data.lastGpsTimestamp * 1e-3) distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix) @@ -94,7 +97,7 @@ class SpeedLimitResolver: self._limit_solutions[Source.map_data] = next_speed_limit self._distance_solutions[Source.map_data] = distance_to_speed_limit_ahead - def _consolidate(self): + def _consolidate(self) -> tuple[float, float, Source]: source = self._get_source_solution_according_to_policy() self.speed_limit = self._limit_solutions[source] if source else 0. self.distance = self._distance_solutions[source] if source else 0.