StarPilot/starpilot/controls/lib/speed_limit_controller.py

#!/usr/bin/env python3
# PFEIFER - SLC - Modified by FrogAi
from openpilot.common.constants import CV
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET

from cereal import custom
from openpilot.starpilot.controls.lib.mapbox_speed_limit import MapboxSpeedLimit


SOURCE_NONE = "None"
SOURCE_DASHBOARD = "Dashboard"
SOURCE_MAP = "Map Data"
SOURCE_VISION = "Vision"
SOURCE_MAPBOX = "Mapbox"
REAL_SOURCES = (SOURCE_DASHBOARD, SOURCE_MAP, SOURCE_VISION, SOURCE_MAPBOX)

OFFSET_MAP_IMPERIAL = [
  (0, 11.2, "speed_limit_offset1"),     # 0–24 mph
  (11.2, 15.2, "speed_limit_offset2"),  # 25–34
  (15.2, 19.6, "speed_limit_offset3"),  # 35–44
  (19.6, 24.1, "speed_limit_offset4"),  # 45–54
  (24.1, 28.6, "speed_limit_offset5"),  # 55–64
  (28.6, 33.1, "speed_limit_offset6"),  # 65–74
  (33.1, 44.2, "speed_limit_offset7"),  # 75–99
]

OFFSET_MAP_METRIC = [
  (0, 8.1, "speed_limit_offset1"),      # 0–29 km/h
  (8.1, 13.6, "speed_limit_offset2"),   # 30–49
  (13.6, 16.4, "speed_limit_offset3"),  # 50–59
  (16.4, 21.9, "speed_limit_offset4"),  # 60–79
  (21.9, 27.5, "speed_limit_offset5"),  # 80–99
  (27.5, 33.1, "speed_limit_offset6"),  # 100–119
  (33.1, 38.9, "speed_limit_offset7"),  # 120–140
]

SLC_OVERRIDE_DISABLE_CLEAR_TIME = 0.75
SET_SPEED_CHANGE_TOLERANCE_METERS_PER_SECOND = 0.1
SAME_LIMIT_TOLERANCE = 1.0
VISION_LARGE_REFERENCE_SPEED_DELTA = 30 * CV.MPH_TO_MS
VISION_LARGE_SET_SPEED_MIN_SUPPORT = 3
VISION_SUPPORT_SPEED_TOLERANCE = 0.5 * CV.MPH_TO_MS


class SpeedLimitController:
  def __init__(self, StarPilotVCruise):
    self.starpilot_planner = StarPilotVCruise.starpilot_planner
    self.starpilot_toggles = None
    self.mapbox = MapboxSpeedLimit(self.starpilot_planner.params)

    self.source = SOURCE_NONE
    self.target = 0.0
    self.map_speed_limit = 0.0
    self.next_speed_limit = 0.0
    self.vision_limit = 0.0
    self.overridden_speed = 0.0

    self.last_valid_limit = max(self.starpilot_planner.params.get_float("PreviousSpeedLimit"), 0.0)
    self.last_valid_source = SOURCE_NONE  # The persisted number has no known live source.
    self.pending_limit = 0.0
    self.pending_source = SOURCE_NONE
    self.confirmation_time = 0.0
    self.denied_limit = 0.0
    self.previous_road_name = ""

    self.set_speed_override = 0.0
    self.pedal_override = 0.0
    self.previous_set_speed = None
    self.consume_set_speed_change = False
    self.override_disable_time = 0.0
    self.limit_change_started = False
    self.confirmation_button_consumed = False
    self._active_control = False
    self._using_experimental_fallback = False
    self._mode = "off"

  def shutdown(self):
    self.mapbox.shutdown()

  @property
  def mapbox_limit(self):
    return self.mapbox.limit

  @property
  def confirmation_pending(self):
    return self.pending_limit >= 1

  @property
  def unconfirmed_speed_limit(self):
    return self.pending_limit

  @property
  def experimental_mode(self):
    return self._active_control and self._using_experimental_fallback

  @property
  def target_to_use(self):
    # Keep Set Speed fallback from arming an override against a higher fake limit.
    if self.source == SOURCE_NONE and self.target > 0 and self.last_valid_limit > 0:
      return min(self.target, self.last_valid_limit)
    return self.target

  def get_offset(self, limit):
    if self.starpilot_toggles is None:
      return 0.0
    offset_map = OFFSET_MAP_METRIC if self.starpilot_toggles.is_metric else OFFSET_MAP_IMPERIAL
    return next((getattr(self.starpilot_toggles, name) for low, high, name in offset_map if low <= limit < high), 0.0)

  @property
  def offset(self):
    return self.get_offset(self.target)

  def low_vision_limit_filtered(self, limit):
    return (
      getattr(self.starpilot_toggles, "vision_speed_limit_low_limit_filter", False) and
      0 < limit <= max(getattr(self.starpilot_toggles, "vision_speed_limit_low_limit_threshold", 0), 0)
    )

  def reset_control_state(self):
    self._clear_pending()
    self.clear_override()
    self.previous_set_speed = None
    self.consume_set_speed_change = False
    self.override_disable_time = 0.0
    self.limit_change_started = False
    self.confirmation_button_consumed = False
    self._active_control = False
    self._using_experimental_fallback = False
    self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
    self.starpilot_planner.params_memory.remove("SLCAdoptSpeedLimit")

  def _get_vision_limit(self, v_ego, sm, display_only):
    enabled = getattr(self.starpilot_toggles, "vision_speed_limit_detection", False)
    self.vision_limit = self.starpilot_planner.params_memory.get_float("VisionSpeedLimit") if enabled else 0.0
    limit = self.vision_limit
    if not display_only and self.low_vision_limit_filtered(limit):
      return 0.0

    raw_set_speed_kph = float(sm["carState"].vCruise)
    selected_speed = raw_set_speed_kph * CV.KPH_TO_MS if 0 < raw_set_speed_kph < V_CRUISE_UNSET else 0.0
    reference_speed = selected_speed if selected_speed > 0 else max(float(v_ego), 0.0)
    if (limit > 0 and reference_speed > 0 and not sm["carState"].standstill and
        abs(limit - reference_speed) >= VISION_LARGE_REFERENCE_SPEED_DELTA):
      memory = self.starpilot_planner.params_memory
      count = memory.get_int("VisionSpeedLimitSupportCount")
      support_speed = memory.get_float("VisionSpeedLimitSupportSpeed")
      if count < VISION_LARGE_SET_SPEED_MIN_SUPPORT or abs(support_speed - limit) > VISION_SUPPORT_SPEED_TOLERANCE:
        return 0.0
    return limit

  def _update_map_speed_limit(self, v_ego, sm):
    map_data = sm["mapdOut"]
    way_sel = map_data.waySelectionType
    if way_sel in (custom.WaySelectionType.current, custom.WaySelectionType.extended):
      self.map_speed_limit = map_data.speedLimit
      self.next_speed_limit = map_data.nextSpeedLimit
    elif way_sel in (custom.WaySelectionType.predicted, custom.WaySelectionType.possible):
      speed = map_data.speedLimit
      if speed > 0 and (self.map_speed_limit == 0 or speed < self.map_speed_limit):
        self.map_speed_limit = speed
      self.next_speed_limit = 0.0
    else:
      # Explicit selection failure means the old current limit is no longer live.
      self.map_speed_limit = 0.0
      self.next_speed_limit = 0.0

    if self.next_speed_limit > 0:
      if self.map_speed_limit < self.next_speed_limit:
        lookahead = self.starpilot_toggles.map_speed_lookahead_higher * v_ego
      elif self.map_speed_limit > self.next_speed_limit:
        lookahead = self.starpilot_toggles.map_speed_lookahead_lower * v_ego
      else:
        lookahead = 0.0
      if map_data.nextSpeedLimitDistance < lookahead:
        self.map_speed_limit = self.next_speed_limit

  def _select_limit(self, dashboard, map_limit, vision_limit):
    priorities = (self.starpilot_toggles.speed_limit_priority1, self.starpilot_toggles.speed_limit_priority2)
    limits = {SOURCE_DASHBOARD: dashboard, SOURCE_MAP: map_limit}
    if SOURCE_VISION in priorities:
      limits[SOURCE_VISION] = vision_limit
    valid = {source: limit for source, limit in limits.items() if limit >= 1}
    if not valid:
      return SOURCE_NONE, 0.0
    if self.starpilot_toggles.speed_limit_priority_highest:
      source = max(valid, key=valid.get)
    elif self.starpilot_toggles.speed_limit_priority_lowest:
      source = min(valid, key=valid.get)
    else:
      source = next((name for name in priorities if name in valid), SOURCE_NONE)
    return source, valid.get(source, 0.0)

  def _apply_mapbox_filler(self, source, limit, now, time_validated, v_ego, sm):
    if source != SOURCE_NONE or not self.starpilot_toggles.slc_mapbox_filler:
      self.mapbox.reset()
      return source, limit
    self.mapbox.update(
      now, time_validated, v_ego, self.starpilot_planner.gps_valid, self.starpilot_planner.gps_position,
      sm["carState"].steeringAngleDeg, sm["liveParameters"].angleOffsetDeg,
    )
    if self.mapbox.limit >= 1:
      return SOURCE_MAPBOX, self.mapbox.limit
    return source, limit

  def _apply_fallback(self, v_cruise, enabled):
    self._using_experimental_fallback = False
    previous_vision_filtered = self.last_valid_source == SOURCE_VISION and self.low_vision_limit_filtered(self.last_valid_limit)
    if self.starpilot_toggles.slc_fallback_previous_speed_limit and self.last_valid_limit > 0 and not previous_vision_filtered:
      self.source = self.last_valid_source
      self.target = self.last_valid_limit
    elif enabled and self.starpilot_toggles.slc_fallback_set_speed:
      self.source = SOURCE_NONE
      self.target = v_cruise
    else:
      self.source = SOURCE_NONE
      self.target = 0.0
      self._using_experimental_fallback = bool(self.starpilot_toggles.slc_fallback_experimental_mode)

  def _confirmation_required(self, limit):
    current = self.last_valid_limit
    return ((limit < current and self.starpilot_toggles.speed_limit_confirmation_lower) or
            (limit > current and self.starpilot_toggles.speed_limit_confirmation_higher))

  def _clear_pending(self):
    self.pending_limit = 0.0
    self.pending_source = SOURCE_NONE
    self.confirmation_time = 0.0

  def _reconcile_set_speed_override(self, old_limit, new_limit):
    if self.set_speed_override <= 0 or old_limit <= 0 or new_limit <= 0 or abs(new_limit - old_limit) < 0.1:
      return
    if (new_limit < old_limit or
        self.set_speed_override <= new_limit + self.get_offset(new_limit) + SET_SPEED_CHANGE_TOLERANCE_METERS_PER_SECOND):
      self.set_speed_override = 0.0
      self.overridden_speed = self.pedal_override

  def _accept_limit(self, source, limit, *, persist=True):
    assert source in REAL_SOURCES and limit >= 1
    old_limit = self.last_valid_limit
    self._reconcile_set_speed_override(old_limit, limit)
    self.source = source
    self.target = limit
    self.last_valid_limit = limit
    self.last_valid_source = source
    self.denied_limit = 0.0
    self._clear_pending()
    if persist and abs(limit - old_limit) >= 0.1:
      self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", float(limit))

  def _reject_limit(self, limit):
    self.denied_limit = limit
    self.source = SOURCE_NONE
    self._clear_pending()
    self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")

  def _update_limit(self, source, limit, sm):
    road_name = sm["mapdOut"].roadName if source == SOURCE_MAP else ""
    if road_name and road_name != self.previous_road_name:
      self.denied_limit = 0.0
      self.previous_road_name = road_name

    if source == SOURCE_NONE:
      self._clear_pending()
      self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
      return

    current = self.last_valid_limit
    if current > 0 and abs(limit - current) < SAME_LIMIT_TOLERANCE:
      self._accept_limit(source, limit, persist=False)
      self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
      return

    confirmation_required = self._confirmation_required(limit)
    if self.denied_limit > 0 and abs(limit - self.denied_limit) < SAME_LIMIT_TOLERANCE:
      if confirmation_required:
        self._clear_pending()
        self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
        self.source = SOURCE_NONE
        return
      # Turning confirmation off applies the already-announced candidate.
      self._accept_limit(source, limit)
      self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
      return
    self.denied_limit = 0.0

    if self.pending_limit == 0 or abs(limit - self.pending_limit) >= SAME_LIMIT_TOLERANCE:
      self.pending_limit = limit
      self.pending_source = source
      self.confirmation_time = 0.0
      self.limit_change_started = True
      new_pending = True
    else:
      self.pending_source = source
      new_pending = False

    if not confirmation_required:
      self._accept_limit(source, limit)
      self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
      return

    self.source = SOURCE_NONE
    self.confirmation_time += DT_MDL
    long_active = sm["carControl"].longActive
    accel_accept = bool(sm["starpilotCarState"].accelPressed and long_active)
    if new_pending and accel_accept:
      # This press cannot confirm a candidate that was replaced on this frame.
      self.confirmation_button_consumed = True
      self.consume_set_speed_change = True
    memory = self.starpilot_planner.params_memory
    ui_accept = memory.get_bool("SpeedLimitAccepted")
    if ui_accept:
      memory.remove("SpeedLimitAccepted")

    fully_disengaged = not long_active and not sm["selfdriveState"].enabled
    if ((accel_accept or ui_accept) and not new_pending) or fully_disengaged:
      pending_limit, pending_source = self.pending_limit, self.pending_source
      higher = pending_limit > current
      self._accept_limit(pending_source, pending_limit)
      if accel_accept:
        self.consume_set_speed_change = True
        self.confirmation_button_consumed = True
      set_speed_kph = float(sm["carState"].vCruise)
      target_with_offset = self.target + self.offset
      if (higher and long_active and 0 < set_speed_kph < V_CRUISE_UNSET and
          set_speed_kph * CV.KPH_TO_MS < target_with_offset):
        memory.put_float("SLCForceCruiseSpeed", target_with_offset)
    elif sm["starpilotCarState"].decelPressed or (self.confirmation_time >= 30 and long_active):
      self._reject_limit(self.pending_limit)

  def _process_adopt_request(self, source, limit):
    memory = self.starpilot_planner.params_memory
    if not memory.get_bool("SLCAdoptSpeedLimit"):
      return
    memory.remove("SLCAdoptSpeedLimit")
    if source not in REAL_SOURCES or limit < 1:
      return
    self.clear_override()
    self.consume_set_speed_change = True
    self._accept_limit(source, limit)
    memory.put_float("SLCForceCruiseSpeed", self.target + self.offset)

  def clear_override(self):
    self.set_speed_override = 0.0
    self.pedal_override = 0.0
    self.overridden_speed = 0.0

  def _update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm):
    previous = self.previous_set_speed
    self.previous_set_speed = v_cruise
    changed = previous is not None and abs(v_cruise - previous) > SET_SPEED_CHANGE_TOLERANCE_METERS_PER_SECOND
    raised = previous is not None and v_cruise > previous + SET_SPEED_CHANGE_TOLERANCE_METERS_PER_SECOND
    consumed = self.consume_set_speed_change
    if consumed and (changed or not sm["starpilotCarState"].accelPressed):
      self.consume_set_speed_change = False

    if not sm["selfdriveState"].enabled:
      self.override_disable_time += DT_MDL
      if self.override_disable_time >= SLC_OVERRIDE_DISABLE_CLEAR_TIME:
        self.clear_override()
      return
    self.override_disable_time = 0.0

    target = self.target_to_use
    target_with_offset = target + self.get_offset(target)
    set_speed = v_cruise + v_cruise_diff
    bidirectional = getattr(self.starpilot_toggles, "redneck_cruise", False)
    if self.set_speed_override > 0:
      if bidirectional:
        self.set_speed_override = max(set_speed, 0.0)
      elif set_speed <= 0 or (target_with_offset > 0 and set_speed <= target_with_offset and
                              (self.source != SOURCE_NONE or changed)):
        self.set_speed_override = 0.0
      else:
        self.set_speed_override = set_speed
    elif (target_with_offset > 0 and set_speed > 0 and not consumed and
          ((bidirectional and changed) or (not bidirectional and raised and set_speed > target_with_offset))):
      self.set_speed_override = set_speed

    self.pedal_override = v_ego + v_ego_diff if sm["carState"].gasPressed and v_ego > target_with_offset > 0 else 0.0
    self.overridden_speed = self.pedal_override or self.set_speed_override

  def update(self, dashboard_speed_limit, now, time_validated, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm,
             *, active=True, display_only=False):
    self.limit_change_started = False
    self.confirmation_button_consumed = False
    self._using_experimental_fallback = False
    mode = "display" if display_only else "active" if active else "off"
    if mode != self._mode:
      self.mapbox.reset()
      self._mode = mode
    if not active and not display_only:
      self.reset_control_state()
      self.mapbox.reset()
      self.source, self.target = SOURCE_NONE, 0.0
      self.map_speed_limit = self.next_speed_limit = self.vision_limit = 0.0
      return

    self._update_map_speed_limit(v_ego, sm)
    vision_limit = self._get_vision_limit(v_ego, sm, display_only)
    source, limit = self._select_limit(dashboard_speed_limit, self.map_speed_limit, vision_limit)
    source, limit = self._apply_mapbox_filler(source, limit, now, time_validated, v_ego, sm)

    if display_only:
      self.reset_control_state()
      self.source, self.target = (source, limit) if limit >= 1 else (SOURCE_NONE, 0.0)
      return

    self._active_control = True
    if source == SOURCE_NONE:
      self._update_limit(source, limit, sm)
      self._apply_fallback(v_cruise, sm["selfdriveState"].enabled)
    else:
      self._update_limit(source, limit, sm)
    self._process_adopt_request(source, limit)
    self._update_override(v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm)