auto draft

cereal/custom.capnp

@@ -152,6 +152,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     preActive @2;
     adapting @3; # Reducing speed to match new speed limit.
     active @4; # Cruising at speed limit.
+    pending @5; # Awaiting new speed limit.
   }
 }
 
@@ -196,6 +197,7 @@ struct OnroadEventSP @0xda96579883444c35 {
     speedLimitActive @18;
     speedLimitConfirmed @19;
     speedLimitValueChange @20;
+    speedLimitPreActive @21;
   }
 }
 

sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py

@@ -1,8 +1,10 @@
 from cereal import custom
 
+SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState
+
 DEBUG = True
-PARAMS_UPDATE_PERIOD = 2.  # secs. Time between parameter updates.
-TEMP_INACTIVE_GUARD_PERIOD = 1.  # secs. Time to wait after activation before considering temp deactivation signal.
+PARAMS_UPDATE_PERIOD = 3.  # secs. Time between parameter updates.
+PRE_ACTIVE_GUARD_PERIOD = 5.  # secs. Time to wait after activation before considering temp deactivation signal.
 
 # Lookup table for speed limit percent offset depending on speed.
 LIMIT_PERC_OFFSET_V = [0.1, 0.05, 0.038]  # 55, 105, 135 km/h
@@ -16,4 +18,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.
 
-SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState
+# Speed Limit Control Auto mode constants
+REQUIRED_INITIAL_CRUISE_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

sunnypilot/selfdrive/controls/lib/speed_limit_controller/helpers.py

@@ -11,9 +11,13 @@ def debug(msg):
 def description_for_state(speed_limit_control_state):
   if speed_limit_control_state == SpeedLimitControlState.inactive:
     return 'INACTIVE'
-  if speed_limit_control_state == SpeedLimitControlState.tempInactive:
-    return 'TEMP_INACTIVE'
+  if speed_limit_control_state == SpeedLimitControlState.preActive:
+    return 'PRE_ACTIVE'
+  if speed_limit_control_state == SpeedLimitControlState.pending:
+    return 'PENDING'
   if speed_limit_control_state == SpeedLimitControlState.adapting:
     return 'ADAPTING'
   if speed_limit_control_state == SpeedLimitControlState.active:
     return 'ACTIVE'
+
+  return ''

sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py

@@ -5,7 +5,8 @@ from cereal import messaging, custom
 from openpilot.common.constants import CV
 from openpilot.common.params import Params
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V, \
-  PARAMS_UPDATE_PERIOD, TEMP_INACTIVE_GUARD_PERIOD, LIMIT_SPEED_OFFSET_TH, SpeedLimitControlState
+  PARAMS_UPDATE_PERIOD, LIMIT_SPEED_OFFSET_TH, SpeedLimitControlState, PRE_ACTIVE_GUARD_PERIOD, REQUIRED_INITIAL_CRUISE_SPEED, \
+  CRUISE_SPEED_TOLERANCE, FALLBACK_CRUISE_SPEED
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy, Engage, OffsetType
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import description_for_state, debug
@@ -16,7 +17,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
 EventNameSP = custom.OnroadEventSP.EventName
 
 ACTIVE_STATES = (SpeedLimitControlState.active, SpeedLimitControlState.adapting)
-ENABLED_STATES = (SpeedLimitControlState.preActive, SpeedLimitControlState.tempInactive, *ACTIVE_STATES)
+ENABLED_STATES = (SpeedLimitControlState.preActive, SpeedLimitControlState.pending, *ACTIVE_STATES)
 
 
 class SpeedLimitController:
@@ -44,9 +45,11 @@ class SpeedLimitController:
     self._v_cruise_setpoint = 0.
     self._v_cruise_setpoint_prev = 0.
     self._v_cruise_setpoint_changed = False
+    self._initial_max_set = False
     self._speed_limit = 0.
     self._speed_limit_prev = 0.
     self._speed_limit_changed = False
+    self._last_valid_speed_limit_offsetted = 0.
     self._distance = 0.
     self._source = Source.none
     self._state = SpeedLimitControlState.inactive
@@ -68,23 +71,20 @@ class SpeedLimitController:
 
     # Mapping functions to state transitions
     self.state_transition_strategy = {
-      # Transition functions for each state
       SpeedLimitControlState.inactive: self.transition_state_from_inactive,
-      SpeedLimitControlState.tempInactive: self.transition_state_from_temp_inactive,
+      SpeedLimitControlState.preActive: self.transition_state_from_preactive,
+      SpeedLimitControlState.pending: self.transition_state_from_pending,
       SpeedLimitControlState.adapting: self.transition_state_from_adapting,
       SpeedLimitControlState.active: self.transition_state_from_active,
-      SpeedLimitControlState.preActive: self.transition_state_from_pre_active,
     }
 
-    # FIXME-SP: unused?
-    # Solution functions mapped to respective states
+    # Solution functions mapped to respective states  
     self.acceleration_solutions = {
-      # Solution functions for each state
-      SpeedLimitControlState.tempInactive: 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,
-      SpeedLimitControlState.preActive: self.get_current_acceleration_as_target,
     }
 
   @property
@@ -95,12 +95,6 @@ class SpeedLimitController:
   def state(self, value) -> None:
     if value != self._state:
       debug(f'Speed Limit Controller state: {description_for_state(value)}')
-
-      if value == SpeedLimitControlState.tempInactive:
-        # Reset previous speed limit to current value as to prevent going out of tempInactive in
-        # a single cycle when the speed limit changes at the same time the user has temporarily deactivated it.
-        self._speed_limit_prev = self._speed_limit
-
     self._state = value
 
   @property
@@ -113,7 +107,18 @@ class SpeedLimitController:
 
   @property
   def speed_limit_offseted(self) -> float:
-    return self._speed_limit + self.speed_limit_offset
+    # If we have a current valid speed limit, use it
+    if self._speed_limit > 0:
+      current_offsetted = self._speed_limit + self.speed_limit_offset
+      self._last_valid_speed_limit_offsetted = current_offsetted
+      return current_offsetted
+
+    # If no current speed limit but we have a last valid one, use that
+    if self._last_valid_speed_limit_offsetted > 0:
+      return self._last_valid_speed_limit_offsetted
+
+    # Fallback
+    return FALLBACK_CRUISE_SPEED
 
   @property
   def speed_limit_offset(self) -> float:
@@ -164,12 +169,24 @@ class SpeedLimitController:
 
       self._last_params_update = self._current_time
 
-  def _read_engage_type_param(self) -> Engage:
-    if self._pcm_cruise_op_long:
-      return Engage.auto
-
+  @staticmethod
+  def _read_engage_type_param() -> Engage:
     return Engage.auto
 
+  def _initial_max_set_confirmed(self) -> bool:
+    return abs(self._v_cruise_setpoint - REQUIRED_INITIAL_CRUISE_SPEED) <= CRUISE_SPEED_TOLERANCE
+
+  def _detect_manual_cruise_change(self) -> bool:
+    if not self.is_active:
+      return False
+
+    # If cruise speed changed and it's not what SLC would set
+    if self._v_cruise_setpoint_changed:
+      expected_cruise = self.speed_limit_offseted
+      return abs(self._v_cruise_setpoint - expected_cruise) > CRUISE_SPEED_TOLERANCE
+
+    return False
+
   def _update_calculations(self, v_ego: float, a_ego: float, v_cruise_setpoint: float) -> None:
     self._v_cruise_setpoint = v_cruise_setpoint if not np.isnan(v_cruise_setpoint) else 0.0
     self._v_ego = v_ego
@@ -199,77 +216,73 @@ class SpeedLimitController:
                                                   int(round((self._speed_limit + self.speed_limit_warning_offset) * self._speed_factor))
 
   def transition_state_from_inactive(self) -> None:
-    """ Make state transition from inactive state """
-    if self._engage_type == Engage.auto:
+    self.state = SpeedLimitControlState.preActive
+    self._initial_max_set = False
+
+  def transition_state_from_preactive(self) -> None:
+    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
+        else:
+          self.state = SpeedLimitControlState.active
+      else:
+        self.state = SpeedLimitControlState.pending
+    elif self._v_cruise_setpoint_changed and self._current_time > (self._last_op_engaged_time + PRE_ACTIVE_GUARD_PERIOD):
+      # User set cruise to something other than 80 MPH, permanently disable for this session
+      self.state = SpeedLimitControlState.inactive
+
+  def transition_state_from_pending(self) -> None:
+    if self._speed_limit > 0:
       if self._v_offset < LIMIT_SPEED_OFFSET_TH:
         self.state = SpeedLimitControlState.adapting
       else:
         self.state = SpeedLimitControlState.active
 
-  def transition_state_from_temp_inactive(self) -> None:
-    """ Make state transition from temporary inactive state """
-    if self._speed_limit_changed:
-      if self._engage_type == Engage.auto:
-        self.state = SpeedLimitControlState.inactive
-
-  def transition_state_from_pre_active(self) -> None:
-    """ Make state transition from preActive state """
-    pass
-
   def transition_state_from_adapting(self) -> None:
-    """ Make state transition from adapting state """
-    if self._v_offset >= LIMIT_SPEED_OFFSET_TH:
+    if self._detect_manual_cruise_change():
+      self.state = SpeedLimitControlState.inactive
+    elif self._v_offset >= LIMIT_SPEED_OFFSET_TH:
       self.state = SpeedLimitControlState.active
 
   def transition_state_from_active(self) -> None:
-    """ Make state transition from active state """
-    if self._engage_type == Engage.auto:
-      if self._v_offset < LIMIT_SPEED_OFFSET_TH:
-        self.state = SpeedLimitControlState.adapting
+    if self._detect_manual_cruise_change():
+      self.state = SpeedLimitControlState.inactive
+    elif self._speed_limit > 0 and self._v_offset < LIMIT_SPEED_OFFSET_TH:
+      self.state = SpeedLimitControlState.adapting
 
   def _state_transition(self) -> None:
     self._state_prev = self._state
 
-    # In any case, if op is disabled, or speed limit control is disabled or no valid speed limit
-    # or gas is pressed, deactivate.
-    if not self._op_engaged or not self._enabled or self._speed_limit == 0:
+    # If op is disabled or SLC is disabled, go inactive
+    if not self._op_engaged or not self._enabled:
       self.state = SpeedLimitControlState.inactive
-      return
-
-    # In any case, we deactivate the speed limit controller temporarily if the user changes the cruise speed.
-    # 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_engaged_time + TEMP_INACTIVE_GUARD_PERIOD):
-      self.state = SpeedLimitControlState.tempInactive
+      self._initial_max_set = False
       return
 
     self.state_transition_strategy[self.state]()
 
-    self._update_v_cruise_setpoint_prev()  # always for Engage.auto
-
   def get_current_acceleration_as_target(self) -> float:
-    """ When state is inactive or tempInactive, preserve current acceleration """
     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 (self.speed_limit_offseted ** 2 - self._v_ego ** 2) / (2. * self.distance)
 
     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 self._v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
 
   def _update_events(self, events_sp: EventsSP) -> None:
     if self.is_active:
-      if self._engage_type == Engage.auto:
-        if self._state_prev not in ACTIVE_STATES:
-          events_sp.add(EventNameSP.speedLimitActive)
-        elif self._speed_limit_changed != 0:
-          events_sp.add(EventNameSP.speedLimitValueChange)
+      if self.state == SpeedLimitControlState.preActive:
+        events_sp.add(EventNameSP.speedLimitPreActive)
+      elif self._state_prev not in ACTIVE_STATES:
+        events_sp.add(EventNameSP.speedLimitActive)
+      elif self._speed_limit_changed:
+        events_sp.add(EventNameSP.speedLimitValueChange)
 
   def update(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise_setpoint: float, events_sp: EventsSP) -> float:
     self._op_engaged = sm['carControl'].longActive

sunnypilot/selfdrive/selfdrived/events.py

@@ -159,4 +159,11 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.WARNING: speed_limit_adjust_alert,
   },
 
+  EventNameSP.speedLimitPreActive: {
+    ET.WARNING: Alert(
+      "Auto Speed Limit Control: Activation Required",
+      "Manually change set speed to 80 MPH to activate",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 3.),
+  },
 }