From 40c456b56b67884fb5a702948d2a41c5f8249dfc Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 02:19:26 -0400 Subject: [PATCH 1/7] simpler op enabled check --- .../lib/speed_limit_controller/speed_limit_controller.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) 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..e71ee0fb23 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 @@ -331,9 +331,7 @@ 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_enabled = enabled and self._CP.openpilotLongitudinalControl self._v_ego = v_ego self._a_ego = a_ego self._v_cruise_setpoint = v_cruise_setpoint From cb3f57110bf10e3b0b72e7a74498f117801abea5 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 02:22:45 -0400 Subject: [PATCH 2/7] more mypy stuff --- .../speed_limit_controller/speed_limit_controller.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) 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 e71ee0fb23..ca601fa0b3 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() @@ -296,18 +299,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 \ From 845a57a21ac2092bcd7963efaf0e750edf159516 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 02:24:43 -0400 Subject: [PATCH 3/7] rename --- .../speed_limit_controller.py | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) 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 ca601fa0b3..c154061c63 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 @@ -34,12 +34,12 @@ 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. @@ -199,8 +199,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 @@ -208,7 +208,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._op_engaged_prev = self._op_engaged self._brake_pressed_prev = self._brake_pressed self._v_cruise_rounded = int(round(self._v_cruise_setpoint * self._speed_factor)) @@ -220,10 +220,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: @@ -235,18 +235,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 @@ -268,7 +268,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: @@ -279,7 +279,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 \ + if not self._op_engaged or not self._is_enabled or self._speed_limit == 0 or \ (self._gas_pressed and self._disengage_on_accelerator): self.state = SpeedLimitControlState.inactive return @@ -288,7 +288,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 @@ -334,7 +334,7 @@ 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 self._CP.openpilotLongitudinalControl + 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 From 5d705d3f6d48aa052051358e25f7df5b6b233fab Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 02:25:05 -0400 Subject: [PATCH 4/7] no need for brake pressed --- .../lib/speed_limit_controller/speed_limit_controller.py | 4 ---- 1 file changed, 4 deletions(-) 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 c154061c63..fe09880bdd 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 @@ -62,8 +62,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. @@ -209,7 +207,6 @@ class SpeedLimitController: if self._engage_type != Engage.user_confirm: self._update_v_cruise_setpoint_prev() self._op_engaged_prev = self._op_engaged - self._brake_pressed_prev = self._brake_pressed 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)) @@ -339,7 +336,6 @@ class SpeedLimitController: 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) From 0306ba701b4f7bfc6ae5cce6e76351979c776166 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 02:25:44 -0400 Subject: [PATCH 5/7] don't reset if gas pressed --- .../lib/speed_limit_controller/speed_limit_controller.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) 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 fe09880bdd..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 @@ -37,7 +37,6 @@ class SpeedLimitController: 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_engaged = False self._op_engaged_prev = False self._v_ego = 0. @@ -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_engaged 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 From dc2e5ef460bf3f05f0d07ca388e65f26e84c00e7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 02:43:56 -0400 Subject: [PATCH 6/7] type hint all --- .../speed_limit_resolver.py | 25 +++++++++++-------- 1 file changed, 15 insertions(+), 10 deletions(-) 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..da9313316a 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,7 +62,7 @@ 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): + def _get_from_map_data(self) -> None: sock = 'liveMapDataSP' if not self._is_sock_updated(sock): @@ -68,7 +73,7 @@ class SpeedLimitResolver: self._reset_limit_sources(Source.map_data) self._process_map_data(self._sm[sock]) - 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 +84,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 +99,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. From 7ca82593f0aadc2cee85e89127088e3b9b14c3b5 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 02:44:55 -0400 Subject: [PATCH 7/7] type hint all --- .../lib/speed_limit_controller/speed_limit_resolver.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) 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 da9313316a..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 @@ -63,15 +63,13 @@ class SpeedLimitResolver: self._distance_solutions[Source.car_state] = 0. def _get_from_map_data(self) -> None: - sock = 'liveMapDataSP' - - if not self._is_sock_updated(sock): + 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: custom.LiveMapDataSP) -> None: gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3