mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-24 05:32:10 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'feature/slc' into ui/slc-ui

Browse Source
This commit is contained in:
Jason Wen
2025-06-05 02:45:17 -04:00
parent 810ffc266c 7ca82593f0
commit bf277d68f0
2 changed files with 40 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py
+23 -28
View File
@@ -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)
sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py
+17 -14
View File
@@ -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.