remove old mapd, sla, scc

selfdrive/car/card.py

@@ -219,7 +219,6 @@ class Car:
     if can_rcv_valid and REPLAY:
       self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
 
-    self.v_cruise_helper.update_speed_limit_assist(self.is_metric, self.sm['longitudinalPlanSP'])
     self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
     if self.sm['carControl'].enabled and not self.CC_prev.enabled:
       # Use CarState w/ buttons from the step selfdrived enables on

selfdrive/car/cruise.py

@@ -53,7 +53,6 @@ class VCruiseHelper(VCruiseHelperSP):
       if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled):
         # if stock cruise is completely disabled, then we can use our own set speed logic
         self._update_v_cruise_non_pcm(CS, _enabled, is_metric)
-        self.update_speed_limit_assist_v_cruise_non_pcm()
         self.v_cruise_cluster_kph = self.v_cruise_kph
         self.update_button_timers(CS, enabled)
       else:
@@ -105,12 +104,6 @@ class VCruiseHelper(VCruiseHelperSP):
     if not self.button_change_states[button_type]["enabled"]:
       return
 
-    # Speed Limit Assist for Non PCM long cars.
-    # True: Disallow set speed changes when user confirmed the target set speed during preActive state
-    # False: Allow set speed changes as SLA is not requesting user confirmation
-    if self.update_speed_limit_assist_pre_active_confirmed(button_type):
-      return
-
     long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta)
     if long_press and self.v_cruise_kph % v_cruise_delta != 0:  # partial interval
       self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta

selfdrive/ui/mici/onroad/hud_renderer.py

@@ -2,7 +2,6 @@ import pyray as rl
 from dataclasses import dataclass
 from openpilot.common.constants import CV
 from openpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar
-from openpilot.selfdrive.ui.mici.onroad.speed_limit_ui import SpeedLimitRenderer
 from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
 from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
@@ -118,7 +117,6 @@ class HudRenderer(Widget):
 
     self._turn_intent = TurnIntent()
     self._torque_bar = TorqueBar()
-    self._speed_limit = SpeedLimitRenderer()
 
     self._txt_wheel: rl.Texture = gui_app.texture('icons_mici/wheel.png', 50, 50)
     self._txt_wheel_critical: rl.Texture = gui_app.texture('icons_mici/wheel_critical.png', 50, 50)
@@ -190,8 +188,6 @@ class HudRenderer(Widget):
       )
       draw_set_speed = alpha >= 1e-2
 
-    self._speed_limit.set_hide_for_hud(draw_set_speed)
-
     if draw_set_speed:
       self._draw_set_speed(rect)
 
@@ -199,8 +195,6 @@ class HudRenderer(Widget):
 
     self._draw_blind_spot_indicators(rect)
 
-    self._speed_limit.render(rect)
-
   def _draw_blind_spot_indicators(self, rect: rl.Rectangle) -> None:
     sm = ui_state.sm
     car_state = sm['carState']

selfdrive/ui/mici/onroad/speed_limit_ui.py

@@ -1,434 +0,0 @@
-import pyray as rl
-import math
-from dataclasses import dataclass
-from enum import IntEnum
-from openpilot.common.constants import CV
-from openpilot.selfdrive.ui.ui_state import ui_state
-from openpilot.system.ui.lib.application import gui_app, FontWeight
-from openpilot.system.ui.lib.multilang import tr
-from openpilot.system.ui.lib.text_measure import measure_text_cached
-from openpilot.system.ui.widgets import Widget
-
-from cereal import log
-EventName = log.OnroadEvent.EventName
-
-METER_TO_KM = 0.001
-METER_TO_FOOT = 3.28084
-METER_TO_MILE = 0.000621371
-UI_FREQ = 20
-SPEED_LIMIT_AHEAD_VALID_FRAME_THRESHOLD = 5
-
-SPEED_LIMIT_SIZE_METRIC = 95
-SPEED_LIMIT_SIZE_IMPERIAL_W = 88
-SPEED_LIMIT_SIZE_IMPERIAL_H = 110
-
-
-class SpeedLimitMode(IntEnum):
-  OFF = 0
-  WARNING = 1
-  ASSIST = 2
-
-
-@dataclass(frozen=True)
-class SpeedLimitColors:
-  white: rl.Color = rl.WHITE
-  black: rl.Color = rl.BLACK
-  red: rl.Color = rl.Color(255, 0, 0, 255)
-  grey: rl.Color = rl.Color(145, 155, 149, 241)
-  green: rl.Color = rl.Color(0, 255, 0, 255)
-  light_grey: rl.Color = rl.Color(200, 200, 200, 255)
-  dark_grey: rl.Color = rl.Color(180, 180, 180, 255)
-  border_grey: rl.Color = rl.Color(77, 77, 77, 255)
-  black_translucent: rl.Color = rl.Color(0, 0, 0, 166)
-  border_translucent: rl.Color = rl.Color(255, 255, 255, 75)
-  yellow: rl.Color = rl.Color(255, 200, 0, 255)
-
-
-COLORS = SpeedLimitColors()
-
-
-class SpeedLimitRenderer(Widget):
-  def __init__(self):
-    super().__init__()
-    self.speed_limit: float = 0.0
-    self.speed_limit_last: float = 0.0
-    self.speed_limit_offset: float = 0.0
-    self.speed_limit_valid: bool = False
-    self.speed_limit_last_valid: bool = False
-    self.speed_limit_final_last: float = 0.0
-    self.speed_limit_source: int = 0
-    self.speed_limit_assist_state: int = 0
-    self.speed_limit_assist_active: bool = False
-    self.speed_limit_mode: SpeedLimitMode = SpeedLimitMode.OFF
-    self.speed_limit_ahead_valid: bool = False
-    self.speed_limit_ahead: float = 0.0
-    self.speed_limit_ahead_distance: float = 0.0
-    self.speed_limit_ahead_distance_prev: float = 0.0
-    self.speed_limit_ahead_valid_frame: int = 0
-    self.speed_limit_assist_frame: int = 0
-    self.long_override: bool = False
-    self.current_speed: float = 0.0
-    self._hide_for_hud: bool = False
-
-    self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
-    self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
-    self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
-
-    from openpilot.common.filter_simple import FirstOrderFilter
-    self._alpha_filter = FirstOrderFilter(0.0, 0.15, 1 / gui_app.target_fps)
-
-  def set_hide_for_hud(self, hide: bool):
-    self._hide_for_hud = hide
-
-  def _update_state(self) -> None:
-    sm = ui_state.sm
-    if sm.recv_frame["carState"] < ui_state.started_frame:
-      return
-
-    speed_conv = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
-
-    if sm.updated["longitudinalPlanSP"]:
-      lp_sp = sm["longitudinalPlanSP"]
-      speed_limit_data = lp_sp.speedLimit
-      resolver = speed_limit_data.resolver
-      assist = speed_limit_data.assist
-
-      self.speed_limit = resolver.speedLimit * speed_conv
-      self.speed_limit_last = resolver.speedLimitLast * speed_conv
-      self.speed_limit_offset = resolver.speedLimitOffset * speed_conv
-      self.speed_limit_valid = resolver.speedLimitValid
-      self.speed_limit_last_valid = resolver.speedLimitLastValid
-      self.speed_limit_final_last = resolver.speedLimitFinalLast * speed_conv
-      self.speed_limit_source = resolver.source
-      self.speed_limit_assist_state = assist.state
-      self.speed_limit_assist_active = assist.active
-
-    if sm.updated["liveMapDataSP"]:
-      lmd = sm["liveMapDataSP"]
-      self.speed_limit_ahead_valid = lmd.speedLimitAheadValid
-      self.speed_limit_ahead = lmd.speedLimitAhead * speed_conv
-      self.speed_limit_ahead_distance = lmd.speedLimitAheadDistance
-
-      if (self.speed_limit_ahead_distance < self.speed_limit_ahead_distance_prev and
-            self.speed_limit_ahead_valid_frame < SPEED_LIMIT_AHEAD_VALID_FRAME_THRESHOLD):
-        self.speed_limit_ahead_valid_frame += 1
-      elif (self.speed_limit_ahead_distance > self.speed_limit_ahead_distance_prev and
-            self.speed_limit_ahead_valid_frame > 0):
-        self.speed_limit_ahead_valid_frame -= 1
-
-      self.speed_limit_ahead_distance_prev = self.speed_limit_ahead_distance
-
-    car_control = sm["carControl"]
-    #self.long_override = car_control.cruiseControl.override_
-
-    car_state = sm["carState"]
-    v_ego_cluster = car_state.vEgoCluster
-    v_ego_cluster_seen = v_ego_cluster != 0.0
-    v_ego = v_ego_cluster if v_ego_cluster_seen else car_state.vEgo
-    self.current_speed = max(0.0, v_ego * speed_conv)
-
-    self.speed_limit_mode = SpeedLimitMode.ASSIST  # TODO: HELPPP
-
-  def _render(self, rect: rl.Rectangle) -> None:
-    sm = ui_state.sm
-
-    should_show = True
-    if sm['onroadEvents']:
-      should_show = False
-
-    ss = sm['selfdriveState']
-    if ss.alertSize != 0:
-      should_show = False
-
-    if self._hide_for_hud:
-      should_show = False
-
-    if self.speed_limit_assist_state == 1:
-      self.speed_limit_assist_frame += 1
-      self.assist_pulse_alpha = 180 + int(40 * math.sin(self.speed_limit_assist_frame / 6))
-      show_speed_limit = True
-    else:
-      self.speed_limit_assist_frame = 0
-      self.assist_pulse_alpha = 255
-      show_speed_limit = self.speed_limit_mode != SpeedLimitMode.OFF
-
-    if not show_speed_limit:
-      should_show = False
-
-    self._alpha_filter.update(1.0 if should_show else 0.0)
-    if self._alpha_filter.x < 0.01:
-      return
-
-    margin_left = 21
-    margin_bottom = 120
-
-    if ui_state.is_metric:
-      sign_width = SPEED_LIMIT_SIZE_METRIC
-      sign_height = SPEED_LIMIT_SIZE_METRIC
-    else:
-      sign_width = SPEED_LIMIT_SIZE_IMPERIAL_W
-      sign_height = SPEED_LIMIT_SIZE_IMPERIAL_H
-
-    x = rect.x + margin_left
-    y = rect.y + rect.height - margin_bottom - sign_height
-
-    sign_rect = rl.Rectangle(x, y, sign_width, sign_height)
-
-    if ui_state.is_metric:
-      self._draw_vienna_sign_compact(sign_rect)
-    else:
-      self._draw_mutcd_sign_compact(sign_rect)
-
-    if self.speed_limit_assist_state != 1:
-      self._draw_upcoming_compact(sign_rect)
-
-  def _pulse_element(self, frame: int) -> bool:
-    if frame % UI_FREQ < (UI_FREQ / 2.5):
-      return False
-    return True
-
-  def _draw_vienna_sign_compact(self, sign_rect: rl.Rectangle) -> None:
-    """Draw compact EU Vienna"""
-    speed_limit_warning_enabled = self.speed_limit_mode >= SpeedLimitMode.WARNING
-    has_speed_limit = self.speed_limit_valid or self.speed_limit_last_valid
-    overspeed = has_speed_limit and round(self.speed_limit_final_last) < round(self.current_speed)
-    speed_limit_str = str(round(self.speed_limit_last)) if has_speed_limit else "---"
-
-    ring_color = COLORS.red
-    speed_color_base = COLORS.black
-    bg_alpha = 230
-
-    if speed_limit_warning_enabled and overspeed:
-      speed_color_base = COLORS.red
-      ring_color = COLORS.red
-    elif not self.speed_limit_valid and self.speed_limit_last_valid:
-      speed_color_base = COLORS.grey
-      ring_color = COLORS.grey
-      bg_alpha = 180
-
-    alpha_multiplier = self._alpha_filter.x
-
-    shadow_offset = 3
-    center = rl.Vector2(
-      sign_rect.x + sign_rect.width / 2 + shadow_offset,
-      sign_rect.y + sign_rect.height / 2 + shadow_offset
-    )
-    radius = sign_rect.width / 2
-    rl.draw_circle_v(center, radius, rl.Color(0, 0, 0, int(80 * alpha_multiplier)))
-
-    center = rl.Vector2(sign_rect.x + sign_rect.width / 2, sign_rect.y + sign_rect.height / 2)
-    rl.draw_circle_v(center, radius, rl.Color(255, 255, 255, int(bg_alpha * alpha_multiplier)))
-
-    if isinstance(ring_color, tuple):
-      ring_r, ring_g, ring_b, _ = ring_color
-    else:
-      ring_r, ring_g, ring_b = ring_color.r, ring_color.g, ring_color.b
-    ring_color_alpha = rl.Color(ring_r, ring_g, ring_b, int(255 * alpha_multiplier))
-    rl.draw_ring(center, radius * 0.82, radius * 0.95, 0, 360, 36, ring_color_alpha)
-
-    font_size = 36 if len(speed_limit_str) >= 3 else 50  # Bigger text
-    text_size = measure_text_cached(self._font_bold, speed_limit_str, font_size)
-    text_pos = rl.Vector2(center.x - text_size.x / 2, center.y - text_size.y / 2)
-
-    if isinstance(speed_color_base, tuple):
-      speed_r, speed_g, speed_b, _ = speed_color_base
-    else:
-      speed_r, speed_g, speed_b = speed_color_base.r, speed_color_base.g, speed_color_base.b
-    speed_color = rl.Color(speed_r, speed_g, speed_b, int(255 * alpha_multiplier))
-    rl.draw_text_ex(self._font_bold, speed_limit_str, text_pos, font_size, 0, speed_color)
-
-    if self.speed_limit_offset != 0 and has_speed_limit:
-      self._draw_offset_compact_circular(sign_rect, center, radius)
-
-  def _draw_mutcd_sign_compact(self, sign_rect: rl.Rectangle) -> None:
-    """Draw compact US MUTCD"""
-    speed_limit_warning_enabled = self.speed_limit_mode >= SpeedLimitMode.WARNING
-    has_speed_limit = self.speed_limit_valid or self.speed_limit_last_valid
-    overspeed = has_speed_limit and round(self.speed_limit_final_last) < round(self.current_speed)
-    speed_limit_str = str(round(self.speed_limit_last)) if has_speed_limit else "---"
-
-    if speed_limit_warning_enabled and overspeed:
-      speed_color_base = COLORS.red
-    elif not self.speed_limit_valid and self.speed_limit_last_valid:
-      speed_color_base = COLORS.grey
-    else:
-      speed_color_base = COLORS.black
-
-    alpha_multiplier = self._alpha_filter.x
-
-    shadow_offset = 3
-    shadow_rect = rl.Rectangle(
-      sign_rect.x + shadow_offset,
-      sign_rect.y + shadow_offset,
-      sign_rect.width,
-      sign_rect.height
-    )
-    rl.draw_rectangle_rounded(shadow_rect, 0.15, 10, rl.Color(0, 0, 0, int(80 * alpha_multiplier)))
-    rl.draw_rectangle_rounded(sign_rect, 0.15, 10, rl.Color(255, 255, 255, int(255 * alpha_multiplier)))
-    rl.draw_rectangle_rounded_lines_ex(sign_rect, 0.15, 10, 3, rl.Color(0, 0, 0, int(255 * alpha_multiplier)))
-
-    speed_text = tr("SPEED")
-    limit_text = tr("LIMIT")
-
-    header_font_size = 28
-    speed_size = measure_text_cached(self._font_semi_bold, speed_text, header_font_size)
-    limit_size = measure_text_cached(self._font_semi_bold, limit_text, header_font_size)
-
-    speed_pos = rl.Vector2(sign_rect.x + (sign_rect.width - speed_size.x) / 2, sign_rect.y + 3)
-    limit_pos = rl.Vector2(sign_rect.x + (sign_rect.width - limit_size.x) / 2, sign_rect.y + 26)
-
-    text_color = rl.Color(0, 0, 0, int(255 * alpha_multiplier))
-    rl.draw_text_ex(self._font_semi_bold, speed_text, speed_pos, header_font_size, 0, text_color)
-    rl.draw_text_ex(self._font_semi_bold, limit_text, limit_pos, header_font_size, 0, text_color)
-
-    number_font_size = 40 if len(speed_limit_str) >= 3 else 60  # Bigger text
-    number_size = measure_text_cached(self._font_bold, speed_limit_str, number_font_size)
-    number_pos = rl.Vector2(
-      sign_rect.x + (sign_rect.width - number_size.x) / 2,
-      sign_rect.y + 45
-    )
-
-    if isinstance(speed_color_base, tuple):
-      r, g, b, _ = speed_color_base
-    else:
-      r, g, b = speed_color_base.r, speed_color_base.g, speed_color_base.b
-    speed_color = rl.Color(r, g, b, int(255 * alpha_multiplier))
-    rl.draw_text_ex(self._font_bold, speed_limit_str, number_pos, number_font_size, 0, speed_color)
-
-    if self.speed_limit_offset != 0 and has_speed_limit:
-      return
-      #self._draw_offset_compact_rectangular(sign_rect)
-
-  def _draw_offset_compact_circular(self, sign_rect: rl.Rectangle, center: rl.Vector2, radius: float) -> None:
-    """Draw compact offset sign"""
-    offset_str = ("+" if self.speed_limit_offset > 0 else "") + str(round(self.speed_limit_offset))
-
-    pill_width = 35
-    pill_height = 18
-    pill_x = center.x - pill_width / 2
-    pill_y = sign_rect.y + sign_rect.height + 5
-
-    pill_rect = rl.Rectangle(pill_x, pill_y, pill_width, pill_height)
-    rl.draw_rectangle_rounded(pill_rect, 0.5, 10, rl.Color(0, 0, 0, int(166 * self._alpha_filter.x)))
-
-    offset_font_size = 13
-    offset_size = measure_text_cached(self._font_semi_bold, offset_str, offset_font_size)
-    offset_pos = rl.Vector2(
-      pill_x + (pill_width - offset_size.x) / 2,
-      pill_y + (pill_height - offset_size.y) / 2
-    )
-    rl.draw_text_ex(self._font_semi_bold, offset_str, offset_pos, offset_font_size, 0,
-                    rl.Color(255, 255, 255, int(255 * self._alpha_filter.x)))
-
-  def _draw_offset_compact_rectangular(self, sign_rect: rl.Rectangle) -> None:
-    """Draw compact offset MUTCD"""
-    offset_str = ("+" if self.speed_limit_offset > 0 else "") + str(round(self.speed_limit_offset))
-
-    box_width = 32
-    box_height = 17
-    box_x = sign_rect.x + (sign_rect.width - box_width) / 2
-    box_y = sign_rect.y + sign_rect.height + 5
-
-    box_rect = rl.Rectangle(box_x, box_y, box_width, box_height)
-    rl.draw_rectangle_rounded(box_rect, 0.3, 10, rl.Color(0, 0, 0, int(166 * self._alpha_filter.x)))
-
-    offset_font_size = 12
-    offset_size = measure_text_cached(self._font_semi_bold, offset_str, offset_font_size)
-    offset_pos = rl.Vector2(
-      box_x + (box_width - offset_size.x) / 2,
-      box_y + (box_height - offset_size.y) / 2
-    )
-    rl.draw_text_ex(self._font_semi_bold, offset_str, offset_pos, offset_font_size, 0,
-                    rl.Color(255, 255, 255, int(255 * self._alpha_filter.x)))
-
-  def _draw_upcoming_compact(self, sign_rect: rl.Rectangle) -> None:
-    """Draw upcoming speed limit indicator to the right"""
-    speed_limit_ahead = (
-          self.speed_limit_ahead_valid and
-          self.speed_limit_ahead > 0 and
-          self.speed_limit_ahead != self.speed_limit and
-          self.speed_limit_ahead_valid_frame > 0 and
-          self.speed_limit_source == 0
-    )
-
-    if not speed_limit_ahead:
-      return
-
-    speed_str = str(round(self.speed_limit_ahead))
-    distance_str = self._format_distance(self.speed_limit_ahead_distance)
-    alpha_multiplier = self._alpha_filter.x
-
-    spacing = 10
-    ahead_size = 60
-    ahead_x = sign_rect.x + sign_rect.width + spacing
-    ahead_y = sign_rect.y + (sign_rect.height - ahead_size) / 2
-    ahead_rect = rl.Rectangle(ahead_x, ahead_y, ahead_size, ahead_size)
-
-    center = rl.Vector2(ahead_rect.x + ahead_rect.width / 2, ahead_rect.y + ahead_rect.height / 2)
-    radius = ahead_size / 2
-    rl.draw_circle_v(center, radius, rl.Color(0, 0, 0, int(160 * alpha_multiplier)))
-    border_color = rl.Color(255, 255, 255, int(75 * alpha_multiplier))
-    rl.draw_ring(center, radius * 0.88, radius * 0.95, 0, 360, 36, border_color)
-
-    speed_font_size = 26
-    speed_size = measure_text_cached(self._font_bold, speed_str, speed_font_size)
-    speed_pos = rl.Vector2(center.x - speed_size.x / 2, center.y - speed_size.y / 2 - 6)
-    rl.draw_text_ex(self._font_bold, speed_str, speed_pos, speed_font_size, 0,
-                    rl.Color(255, 255, 255, int(255 * alpha_multiplier)))
-
-    distance_font_size = 11
-    distance_size = measure_text_cached(self._font_medium, distance_str, distance_font_size)
-    distance_pos = rl.Vector2(center.x - distance_size.x / 2, center.y + 9)
-    rl.draw_text_ex(self._font_medium, distance_str, distance_pos, distance_font_size, 0,
-                    rl.Color(200, 200, 200, int(255 * alpha_multiplier)))
-
-  def _format_distance(self, distance: float) -> str:
-    """Format distance string"""
-    if ui_state.is_metric:
-      if distance < 50:
-        return tr("Near")
-      if distance >= 1000:
-        return f"{distance * METER_TO_KM:.1f}km"
-
-      if distance < 200:
-        rounded = max(10, self._round_to_interval(distance, 10, 5))
-      else:
-        rounded = self._round_to_interval(distance, 100, 50)
-      return f"{rounded}m"
-    else:
-      distance_ft = distance * METER_TO_FOOT
-      if distance_ft < 100:
-        return tr("Near")
-      if distance_ft >= 900:
-        return f"{distance * METER_TO_MILE:.1f}mi"
-
-      if distance_ft < 500:
-        rounded = max(50, self._round_to_interval(distance_ft, 50, 25))
-      else:
-        rounded = self._round_to_interval(distance_ft, 100, 50)
-      return f"{rounded}ft"
-
-  def _round_to_interval(self, distance: float, interval: int, threshold: int) -> int:
-    base = int(distance / interval) * interval
-    return base + interval if (distance - base >= threshold) else base
-
-  def _draw_pre_active_glow(self, sign_rect: rl.Rectangle) -> None:
-    """Draw pulsing glow effect for pre-active state"""
-    bounce_frequency = 2.0 * math.pi / UI_FREQ
-    glow_intensity = 0.5 + 0.5 * abs(math.sin(self.speed_limit_assist_frame * bounce_frequency))
-
-    center = rl.Vector2(sign_rect.x + sign_rect.width / 2, sign_rect.y + sign_rect.height / 2)
-
-    if ui_state.is_metric:
-      radius = sign_rect.width / 2
-      glow_color = rl.Color(0, 255, 0, int(100 * glow_intensity))
-      rl.draw_circle_v(center, radius + 8, glow_color)
-    else:
-      glow_rect = rl.Rectangle(
-        sign_rect.x - 4,
-        sign_rect.y - 4,
-        sign_rect.width + 8,
-        sign_rect.height + 8
-      )
-      glow_color = rl.Color(0, 255, 0, int(100 * glow_intensity))
-      rl.draw_rectangle_rounded(glow_rect, 0.15, 10, glow_color)

selfdrive/ui/onroad/hud_renderer.py

@@ -3,7 +3,6 @@ from dataclasses import dataclass
 from openpilot.common.constants import CV
 from openpilot.selfdrive.ui.onroad.exp_button import ExpButton
 from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
-from openpilot.selfdrive.ui.onroad.speed_limit_ui import SpeedLimitRenderer
 from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.lib.text_measure import measure_text_cached
@@ -66,7 +65,6 @@ class HudRenderer(Widget):
     self.set_speed: float = SET_SPEED_NA
     self.speed: float = 0.0
     self.v_ego_cluster_seen: bool = False
-    self._speed_limit_renderer = SpeedLimitRenderer()
 
     self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
     self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
@@ -118,7 +116,6 @@ class HudRenderer(Widget):
       self._draw_set_speed(rect)
 
     self._draw_current_speed(rect)
-    self._speed_limit_renderer.render(rect)
 
     button_x = rect.x + rect.width - UI_CONFIG.border_size - UI_CONFIG.button_size
     button_y = rect.y + UI_CONFIG.border_size

selfdrive/ui/onroad/speed_limit_ui.py

@@ -1,388 +0,0 @@
-import pyray as rl
-import math
-from dataclasses import dataclass
-from enum import IntEnum
-from openpilot.common.constants import CV
-from openpilot.selfdrive.ui.ui_state import ui_state
-from openpilot.system.ui.lib.application import gui_app, FontWeight
-from openpilot.system.ui.lib.multilang import tr
-from openpilot.system.ui.lib.text_measure import measure_text_cached
-from openpilot.system.ui.widgets import Widget
-
-METER_TO_KM = 0.001
-METER_TO_FOOT = 3.28084
-METER_TO_MILE = 0.000621371
-UI_FREQ = 20
-SPEED_LIMIT_AHEAD_VALID_FRAME_THRESHOLD = 5
-
-SPEED_LIMIT_WIDTH_METRIC = 200
-SPEED_LIMIT_WIDTH_IMPERIAL = 172
-SPEED_LIMIT_HEIGHT = 204
-
-
-class SpeedLimitMode(IntEnum):
-  OFF = 0
-  WARNING = 1
-  ASSIST = 2
-
-
-@dataclass(frozen=True)
-class SpeedLimitColors:
-  white: rl.Color = rl.WHITE
-  black: rl.Color = rl.BLACK
-  red: rl.Color = rl.Color(255, 0, 0, 255)
-  grey: rl.Color = rl.Color(145, 155, 149, 241)
-  green: rl.Color = rl.Color(0, 255, 0, 255)
-  light_grey: rl.Color = rl.Color(200, 200, 200, 255)
-  dark_grey: rl.Color = rl.Color(180, 180, 180, 255)
-  border_grey: rl.Color = rl.Color(77, 77, 77, 255)
-  black_translucent: rl.Color = rl.Color(0, 0, 0, 166)
-  border_translucent: rl.Color = rl.Color(255, 255, 255, 75)
-
-
-COLORS = SpeedLimitColors()
-
-
-class SpeedLimitRenderer(Widget):
-  def __init__(self):
-    super().__init__()
-    self.speed_limit: float = 0.0
-    self.speed_limit_last: float = 0.0
-    self.speed_limit_offset: float = 0.0
-    self.speed_limit_valid: bool = False
-    self.speed_limit_last_valid: bool = False
-    self.speed_limit_final_last: float = 0.0
-    self.speed_limit_source: int = 0
-    self.speed_limit_assist_state: int = 0
-    self.speed_limit_assist_active: bool = False
-    self.speed_limit_mode: SpeedLimitMode = SpeedLimitMode.OFF
-    self.speed_limit_ahead_valid: bool = False
-    self.speed_limit_ahead: float = 0.0
-    self.speed_limit_ahead_distance: float = 0.0
-    self.speed_limit_ahead_distance_prev: float = 0.0
-    self.speed_limit_ahead_valid_frame: int = 0
-    self.speed_limit_assist_frame: int = 0
-    self.long_override: bool = False
-    self.current_speed: float = 0.0
-
-    self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
-    self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
-    self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
-
-  def _update_state(self) -> None:
-    sm = ui_state.sm
-    if sm.recv_frame["carState"] < ui_state.started_frame:
-      return
-
-    speed_conv = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
-
-    if sm.updated["longitudinalPlanSP"]:
-      lp_sp = sm["longitudinalPlanSP"]
-      speed_limit_data = lp_sp.speedLimit
-      resolver = speed_limit_data.resolver
-      assist = speed_limit_data.assist
-
-      self.speed_limit = resolver.speedLimit * speed_conv
-      self.speed_limit_last = resolver.speedLimitLast * speed_conv
-      self.speed_limit_offset = resolver.speedLimitOffset * speed_conv
-      self.speed_limit_valid = resolver.speedLimitValid
-      self.speed_limit_last_valid = resolver.speedLimitLastValid
-      self.speed_limit_final_last = resolver.speedLimitFinalLast * speed_conv
-      self.speed_limit_source = resolver.source
-      self.speed_limit_assist_state = assist.state
-      self.speed_limit_assist_active = assist.active
-
-    if sm.updated["liveMapDataSP"]:
-      lmd = sm["liveMapDataSP"]
-      self.speed_limit_ahead_valid = lmd.speedLimitAheadValid
-      self.speed_limit_ahead = lmd.speedLimitAhead * speed_conv
-      self.speed_limit_ahead_distance = lmd.speedLimitAheadDistance
-
-      if (self.speed_limit_ahead_distance < self.speed_limit_ahead_distance_prev and
-        self.speed_limit_ahead_valid_frame < SPEED_LIMIT_AHEAD_VALID_FRAME_THRESHOLD):
-        self.speed_limit_ahead_valid_frame += 1
-      elif (self.speed_limit_ahead_distance > self.speed_limit_ahead_distance_prev and
-            self.speed_limit_ahead_valid_frame > 0):
-        self.speed_limit_ahead_valid_frame -= 1
-
-      self.speed_limit_ahead_distance_prev = self.speed_limit_ahead_distance
-
-    car_control = sm["carControl"]
-    #self.long_override = car_control.cruiseControl.override_
-
-    car_state = sm["carState"]
-    v_ego_cluster = car_state.vEgoCluster
-    v_ego_cluster_seen = v_ego_cluster != 0.0
-    v_ego = v_ego_cluster if v_ego_cluster_seen else car_state.vEgo
-    self.current_speed = max(0.0, v_ego * speed_conv)
-
-    self.speed_limit_mode = SpeedLimitMode.WARNING  # TODO: HELPPP
-
-  def _render(self, rect: rl.Rectangle) -> None:
-    show_speed_limit = False
-
-    if self.speed_limit_assist_state == 1:  # PRE_ACTIVE state
-      self.speed_limit_assist_frame += 1
-      show_speed_limit = self._pulse_element(self.speed_limit_assist_frame)
-    else:
-      self.speed_limit_assist_frame = 0
-      show_speed_limit = self.speed_limit_mode != SpeedLimitMode.OFF
-
-    if not show_speed_limit:
-      return
-
-    speed_limit_width = SPEED_LIMIT_WIDTH_METRIC if ui_state.is_metric else SPEED_LIMIT_WIDTH_IMPERIAL
-
-    base_x = 280 if ui_state.is_metric else 272
-
-    x = rect.x + base_x + (SPEED_LIMIT_WIDTH_IMPERIAL - speed_limit_width) // 2
-    y = rect.y + 45
-
-    sign_rect = rl.Rectangle(x, y, speed_limit_width, SPEED_LIMIT_HEIGHT)
-
-    # Draw pre-active arrows
-    if self.speed_limit_assist_state == 1:  # PRE_ACTIVE
-      self._draw_pre_active_arrow(sign_rect)
-
-    self._draw_speed_limit_sign(sign_rect)
-
-    # Draw upcoming speed limit if not in PRE_ACTIVE state
-    if self.speed_limit_assist_state != 1:
-      self._draw_upcoming_speed_limit(sign_rect)
-
-  def _pulse_element(self, frame: int) -> bool:
-    if frame % UI_FREQ < (UI_FREQ / 2.5):
-      return False
-    return True
-
-  def _draw_speed_limit_sign(self, sign_rect: rl.Rectangle) -> None:
-    speed_limit_warning_enabled = self.speed_limit_mode >= SpeedLimitMode.WARNING
-    has_speed_limit = self.speed_limit_valid or self.speed_limit_last_valid
-    overspeed = has_speed_limit and round(self.speed_limit_final_last) < round(self.current_speed)
-    speed_limit_str = str(round(self.speed_limit_last)) if has_speed_limit else "---"
-
-    speed_limit_sub_text = ""
-    if self.speed_limit_offset != 0:
-      speed_limit_sub_text = ("" if self.speed_limit_offset > 0 else "-") + str(round(abs(self.speed_limit_offset)))
-
-    speed_color = COLORS.black
-    if speed_limit_warning_enabled and overspeed:
-      speed_color = COLORS.red
-    elif not self.speed_limit_valid and self.speed_limit_last_valid:
-      speed_color = COLORS.grey
-
-    if ui_state.is_metric:
-      self._draw_vienna_sign(sign_rect, speed_limit_str, speed_limit_sub_text, speed_color, has_speed_limit)
-    else:
-      self._draw_mutcd_sign(sign_rect, speed_limit_str, speed_limit_sub_text, speed_color, has_speed_limit)
-
-  def _draw_vienna_sign(self, sign_rect: rl.Rectangle, speed_str: str, sub_text: str,
-                        speed_color: rl.Color, has_speed_limit: bool) -> None:
-    """Draw EU Vienna sign."""
-    padding = 15
-    circle_size = min(sign_rect.width, sign_rect.height) - (padding * 2)
-    circle_x = sign_rect.x + (sign_rect.width - circle_size) / 2
-    circle_y = sign_rect.y + (sign_rect.height - circle_size) / 2
-    center = rl.Vector2(circle_x + circle_size / 2, circle_y + circle_size / 2)
-    radius = circle_size / 2
-
-    rl.draw_circle_v(center, radius, COLORS.white)
-    rl.draw_ring(center, radius * 0.88, radius, 0, 360, 36, COLORS.red)
-
-    font_size = 70 if len(speed_str) >= 3 else 85
-    text_size = measure_text_cached(self._font_bold, speed_str, font_size)
-    text_pos = rl.Vector2(center.x - text_size.x / 2, center.y - text_size.y / 2)
-    rl.draw_text_ex(self._font_bold, speed_str, text_pos, font_size, 0, speed_color)
-
-    # Offset value
-    if sub_text and has_speed_limit:
-      offset_size = circle_size * 0.4
-      overlap = offset_size * 0.25
-      offset_center = rl.Vector2(
-        sign_rect.x + sign_rect.width - offset_size / 1.25 + overlap,
-        sign_rect.y - offset_size / 1.75 + overlap + offset_size / 2
-      )
-      rl.draw_circle_v(offset_center, offset_size / 2, COLORS.black)
-      rl.draw_ring(offset_center, offset_size / 2 - 3, offset_size / 2, 0, 360, 36, COLORS.border_grey)
-
-      sub_font_size = offset_size * (0.5 if ui_state.is_metric else 0.6)
-      if len(sub_text) >= 3:
-        sub_font_size = offset_size * 0.475
-
-      sub_text_size = measure_text_cached(self._font_bold, sub_text, int(sub_font_size))
-      sub_text_pos = rl.Vector2(
-        offset_center.x - sub_text_size.x / 2,
-        offset_center.y - sub_text_size.y / 2
-      )
-      rl.draw_text_ex(self._font_bold, sub_text, sub_text_pos, int(sub_font_size), 0, COLORS.white)
-
-  def _draw_mutcd_sign(self, sign_rect: rl.Rectangle, speed_str: str, sub_text: str,
-                       speed_color: rl.Color, has_speed_limit: bool) -> None:
-    """Draw US/Canada MUTCD sign."""
-    #rl.draw_rectangle_rounded(sign_rect, 0.35, 10, COLORS.white) # this is here for sunny
-    padding = 8
-    inner_rect = rl.Rectangle(
-      sign_rect.x + padding,
-      sign_rect.y + padding,
-      sign_rect.width - (padding * 2),
-      sign_rect.height - (padding * 2)
-    )
-    rl.draw_rectangle_rounded(inner_rect, 0.35, 10, COLORS.white)
-    rl.draw_rectangle_rounded_lines_ex(inner_rect, 0.35, 10, 4, COLORS.black)
-
-    border_width = 4
-    border_rect = rl.Rectangle(
-      inner_rect.x + border_width,
-      inner_rect.y + border_width,
-      inner_rect.width - (border_width * 2),
-      inner_rect.height - (border_width * 2)
-    )
-    speed_text = tr("SPEED")
-    limit_text = tr("LIMIT")
-
-    speed_size = measure_text_cached(self._font_semi_bold, speed_text, 40)
-    limit_size = measure_text_cached(self._font_semi_bold, limit_text, 40)
-
-    speed_pos = rl.Vector2(border_rect.x + (border_rect.width - speed_size.x) / 2, sign_rect.y + 27)
-    limit_pos = rl.Vector2(border_rect.x + (border_rect.width - limit_size.x) / 2, sign_rect.y + 65)
-
-    rl.draw_text_ex(self._font_semi_bold, speed_text, speed_pos, 40, 0, COLORS.black)
-    rl.draw_text_ex(self._font_semi_bold, limit_text, limit_pos, 40, 0, COLORS.black)
-
-    font_size = 70 if len(speed_str) >= 3 else 90
-    speed_value_size = measure_text_cached(self._font_bold, speed_str, font_size)
-    speed_value_pos = rl.Vector2(
-      border_rect.x + (border_rect.width - speed_value_size.x) / 2,
-      sign_rect.y + 100
-    )
-    rl.draw_text_ex(self._font_bold, speed_str, speed_value_pos, font_size, 0, speed_color)
-
-    # Offset value
-    if sub_text and has_speed_limit:
-      offset_box_size = sign_rect.width * 0.4
-      overlap = offset_box_size * 0.25
-      offset_rect = rl.Rectangle(
-        sign_rect.x + sign_rect.width - offset_box_size / 1.5 + overlap,
-        sign_rect.y - offset_box_size / 1.25 + overlap,
-        offset_box_size,
-        offset_box_size
-      )
-
-      corner_radius = offset_box_size * 0.2 / offset_box_size
-      rl.draw_rectangle_rounded(offset_rect, corner_radius, 10, COLORS.black)
-      rl.draw_rectangle_rounded_lines_ex(offset_rect, corner_radius, 10, 6, COLORS.border_grey)
-
-      sub_font_size = offset_box_size * 0.6
-      if len(sub_text) >= 3:
-        sub_font_size = offset_box_size * 0.475
-
-      sub_text_size = measure_text_cached(self._font_bold, sub_text, int(sub_font_size))
-      sub_text_pos = rl.Vector2(
-        offset_rect.x + (offset_rect.width - sub_text_size.x) / 2,
-        offset_rect.y + (offset_rect.height - sub_text_size.y) / 2
-      )
-      rl.draw_text_ex(self._font_bold, sub_text, sub_text_pos, int(sub_font_size), 0, COLORS.white)
-
-  def _draw_upcoming_speed_limit(self, sign_rect: rl.Rectangle) -> None:
-    """Draw upcoming speed limit indicator"""
-    speed_limit_ahead = (
-      self.speed_limit_ahead_valid and
-      self.speed_limit_ahead > 0 and
-      self.speed_limit_ahead != self.speed_limit and
-      self.speed_limit_ahead_valid_frame > 0 and
-      self.speed_limit_source == 0  # source
-    )
-
-    if not speed_limit_ahead:
-      return
-
-    speed_str = str(round(self.speed_limit_ahead))
-    distance_str = self._format_distance(self.speed_limit_ahead_distance)
-
-    ahead_width = 170
-    ahead_height = 160
-    ahead_x = sign_rect.x + (sign_rect.width - ahead_width) / 2
-    ahead_y = sign_rect.y + sign_rect.height + 10
-    ahead_rect = rl.Rectangle(ahead_x, ahead_y, ahead_width, ahead_height)
-
-    rl.draw_rectangle_rounded(ahead_rect, 0.2, 10, rl.Color(0, 0, 0, 180))
-    rl.draw_rectangle_rounded_lines_ex(ahead_rect, 0.2, 10, 3, rl.Color(255, 255, 255, 100))
-
-    ahead_label = tr("AHEAD")
-    ahead_size = measure_text_cached(self._font_semi_bold, ahead_label, 40)
-    ahead_pos = rl.Vector2(ahead_rect.x + (ahead_rect.width - ahead_size.x) / 2, ahead_rect.y + 4)
-    rl.draw_text_ex(self._font_semi_bold, ahead_label, ahead_pos, 40, 0, COLORS.light_grey)
-
-    speed_size = measure_text_cached(self._font_bold, speed_str, 70)
-    speed_pos = rl.Vector2(ahead_rect.x + (ahead_rect.width - speed_size.x) / 2, ahead_rect.y + 38)
-    rl.draw_text_ex(self._font_bold, speed_str, speed_pos, 70, 0, COLORS.white)
-
-    distance_size = measure_text_cached(self._font_medium, distance_str, 40)
-    distance_pos = rl.Vector2(
-      ahead_rect.x + (ahead_rect.width - distance_size.x) / 2,
-      ahead_rect.y + 110
-    )
-    rl.draw_text_ex(self._font_medium, distance_str, distance_pos, 40, 0, COLORS.dark_grey)
-
-  def _format_distance(self, distance: float) -> str:
-    if ui_state.is_metric:
-      if distance < 50:
-        return tr("Near")
-      if distance >= 1000:
-        return f"{distance * METER_TO_KM:.1f}" + tr("km")
-
-      # Round to appropriate
-      if distance < 200:
-        rounded = max(10, self._round_to_interval(distance, 10, 5))
-      else:
-        rounded = self._round_to_interval(distance, 100, 50)
-      return str(rounded) + tr("m")
-    else:
-      distance_ft = distance * METER_TO_FOOT
-      if distance_ft < 100:
-        return tr("Near")
-      if distance_ft >= 900:
-        return f"{distance * METER_TO_MILE:.1f}" + tr("mi")
-
-      if distance_ft < 500:
-        rounded = max(50, self._round_to_interval(distance_ft, 50, 25))
-      else:
-        rounded = self._round_to_interval(distance_ft, 100, 50)
-      return str(rounded) + tr("ft")
-
-  def _round_to_interval(self, distance: float, interval: int, threshold: int) -> int:
-    base = int(distance / interval) * interval
-    return base + interval if (distance - base >= threshold) else base
-
-  def _draw_pre_active_arrow(self, sign_rect: rl.Rectangle) -> None:
-    sign_margin = 12
-    arrow_spacing = sign_margin * 1.4
-    arrow_x = sign_rect.x + sign_rect.width + arrow_spacing
-
-    set_speed = 0
-    speed_limit_final = round(self.speed_limit_final_last)
-
-    bounce_frequency = 2.0 * math.pi / UI_FREQ
-    bounce_offset = 20 * math.sin(self.speed_limit_assist_frame * bounce_frequency)
-
-    arrow_size = 45
-    center_y = sign_rect.y + sign_rect.height / 2
-
-    if set_speed < speed_limit_final:
-      arrow_y = center_y - arrow_size + bounce_offset
-      self._draw_triangle_up(arrow_x, arrow_y, arrow_size, COLORS.green)
-    elif set_speed > speed_limit_final:
-      arrow_y = center_y - bounce_offset
-      self._draw_triangle_down(arrow_x, arrow_y, arrow_size, COLORS.green)
-
-  def _draw_triangle_up(self, x: float, y: float, size: float, color: rl.Color) -> None:
-    v1 = rl.Vector2(x + size / 2, y)
-    v2 = rl.Vector2(x, y + size)
-    v3 = rl.Vector2(x + size, y + size)
-    rl.draw_triangle(v1, v3, v2, color)
-
-  def _draw_triangle_down(self, x: float, y: float, size: float, color: rl.Color) -> None:
-    v1 = rl.Vector2(x, y)
-    v2 = rl.Vector2(x + size, y)
-    v3 = rl.Vector2(x + size / 2, y + size)
-    rl.draw_triangle(v1, v2, v3, color)

selfdrive/ui/sunnypilot/ui_state.py

@@ -29,7 +29,7 @@ class UIStateSP:
     self.params = Params()
     self.sm_services_ext = [
       "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
-      "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay"
+      "gpsLocation", "liveTorqueParameters", "carStateSP", "carParamsSP", "liveDelay"
     ]
 
     self.sunnylink_state = SunnylinkState()

sunnypilot/mapd/__init__.py

@@ -1,5 +0,0 @@
-import os
-from openpilot.common.basedir import BASEDIR
-
-MAPD_BIN_DIR = os.path.join(BASEDIR, 'third_party/mapd_pfeiferj')
-MAPD_PATH = os.path.join(MAPD_BIN_DIR, 'mapd')

sunnypilot/mapd/live_map_data/__init__.py

@@ -1,22 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from openpilot.common.swaglog import cloudlog
-
-LOOK_AHEAD_HORIZON_TIME = 15.  # s. Time horizon for look ahead of turn speed sections to provide on liveMapDataSP msg.
-_DEBUG = False
-_CLOUDLOG_DEBUG = False
-ROAD_NAME_TIMEOUT = 30  # secs
-R = 6373000.0  # approximate radius of earth in mts
-QUERY_RADIUS = 3000  # mts. Radius to use on OSM data queries.
-QUERY_RADIUS_OFFLINE = 2250  # mts. Radius to use on offline OSM data queries.
-
-
-def get_debug(msg, log_to_cloud=True):
-  if _CLOUDLOG_DEBUG and log_to_cloud:
-    cloudlog.debug(msg)
-  if _DEBUG:
-    print(msg)

sunnypilot/mapd/live_map_data/base_map_data.py

@@ -1,65 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from abc import abstractmethod, ABC
-
-import cereal.messaging as messaging
-from openpilot.common.params import Params
-from openpilot.common.constants import CV
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot.navd.helpers import coordinate_from_param
-
-MAX_SPEED_LIMIT = V_CRUISE_UNSET * CV.KPH_TO_MS
-
-
-class BaseMapData(ABC):
-  def __init__(self):
-    self.params = Params()
-
-    self.sm = messaging.SubMaster(['liveLocationKalman'])
-    self.pm = messaging.PubMaster(['liveMapDataSP'])
-
-    self.localizer_valid = False
-    self.last_bearing = None
-    self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params)
-
-  @abstractmethod
-  def update_location(self) -> None:
-    pass
-
-  @abstractmethod
-  def get_current_speed_limit(self) -> float:
-    pass
-
-  @abstractmethod
-  def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
-    pass
-
-  @abstractmethod
-  def get_current_road_name(self) -> str:
-    pass
-
-  def publish(self) -> None:
-    speed_limit = self.get_current_speed_limit()
-    next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
-
-    mapd_sp_send = messaging.new_message('liveMapDataSP')
-    mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK
-    live_map_data = mapd_sp_send.liveMapDataSP
-
-    live_map_data.speedLimitValid = bool(MAX_SPEED_LIMIT > speed_limit > 0)
-    live_map_data.speedLimit = speed_limit
-    live_map_data.speedLimitAheadValid = bool(MAX_SPEED_LIMIT > next_speed_limit > 0)
-    live_map_data.speedLimitAhead = next_speed_limit
-    live_map_data.speedLimitAheadDistance = next_speed_limit_distance
-    live_map_data.roadName = self.get_current_road_name()
-
-    self.pm.send('liveMapDataSP', mapd_sp_send)
-
-  def tick(self) -> None:
-    self.sm.update(0)
-    self.update_location()
-    self.publish()

sunnypilot/mapd/live_map_data/debug.py

@@ -1,56 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-# DISCLAIMER: This code is intended principally for development and debugging purposes.
-# Although it provides a standalone entry point to the program, users should refer
-# to the actual implementations for consumption. Usage outside of development scenarios
-# is not advised and could lead to unpredictable results.
-
-import threading
-import traceback
-
-from cereal import messaging
-from openpilot.common.gps import get_gps_location_service
-from openpilot.common.params import Params
-from openpilot.common.realtime import config_realtime_process
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver
-from openpilot.sunnypilot.mapd.live_map_data import get_debug
-
-
-def excepthook(args):
-  get_debug(f'MapD: Threading exception:\n{args}')
-  traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback)
-
-
-def live_map_data_sp_thread():
-  config_realtime_process([0, 1, 2, 3], 5)
-
-  params = Params()
-  gps_location_service = get_gps_location_service(params)
-
-  while True:
-    live_map_data_sp_thread_debug(gps_location_service)
-
-
-def live_map_data_sp_thread_debug(gps_location_service):
-  _sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', 'carStateSP', gps_location_service])
-  _sub_master.update()
-
-  v_ego = _sub_master['carState'].vEgo
-  _resolver = SpeedLimitResolver()
-  _resolver.policy = Policy.car_state_priority
-  _resolver.update(v_ego, _sub_master)
-  print(_resolver.speed_limit, _resolver.distance, _resolver.source)
-
-
-def main():
-  threading.excepthook = excepthook
-  live_map_data_sp_thread()
-
-
-if __name__ == "__main__":
-  main()

sunnypilot/mapd/live_map_data/osm_map_data.py

@@ -1,61 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import json
-import math
-import platform
-
-from cereal import log
-from openpilot.common.params import Params
-from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
-from openpilot.sunnypilot.navd.helpers import Coordinate
-
-
-class OsmMapData(BaseMapData):
-  def __init__(self):
-    super().__init__()
-    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
-
-  def update_location(self) -> None:
-    location = self.sm['liveLocationKalman']
-    self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
-
-    if self.localizer_valid:
-      self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
-      self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
-
-    if self.last_position is None:
-      return
-
-    params = {
-      "latitude": self.last_position.latitude,
-      "longitude": self.last_position.longitude,
-    }
-
-    if self.last_bearing is not None:
-      params['bearing'] = self.last_bearing
-
-    self.mem_params.put("LastGPSPosition", json.dumps(params))
-
-  def get_current_speed_limit(self) -> float:
-    return float(self.mem_params.get("MapSpeedLimit") or 0.0)
-
-  def get_current_road_name(self) -> str:
-    return str(self.mem_params.get("RoadName") or "")
-
-  def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
-    next_speed_limit_section_str = self.mem_params.get("NextMapSpeedLimit")
-    next_speed_limit_section = next_speed_limit_section_str if next_speed_limit_section_str else {}
-    next_speed_limit = next_speed_limit_section.get('speedlimit', 0.0)
-    next_speed_limit_latitude = next_speed_limit_section.get('latitude')
-    next_speed_limit_longitude = next_speed_limit_section.get('longitude')
-    next_speed_limit_distance = 0.0
-
-    if next_speed_limit_latitude and next_speed_limit_longitude:
-      next_speed_limit_coordinates = Coordinate(next_speed_limit_latitude, next_speed_limit_longitude)
-      next_speed_limit_distance = (self.last_position or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates)
-
-    return next_speed_limit, next_speed_limit_distance

sunnypilot/mapd/live_map_data/standalone.py

@@ -1,42 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-# DISCLAIMER: This code is intended principally for development and debugging purposes.
-# Although it provides a standalone entry point to the program, users should refer
-# to the actual implementations for consumption. Usage outside of development scenarios
-# is not advised and could lead to unpredictable results.
-
-import threading
-import traceback
-
-from openpilot.common.realtime import Ratekeeper, config_realtime_process
-from openpilot.sunnypilot.mapd.live_map_data import get_debug
-from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
-
-
-def excepthook(args):
-  get_debug(f'MapD: Threading exception:\n{args}')
-  traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback)
-
-
-def live_map_data_sp_thread():
-  config_realtime_process([0, 1, 2, 3], 5)
-
-  live_map_sp = OsmMapData()
-  rk = Ratekeeper(1, print_delay_threshold=None)
-
-  while True:
-    live_map_sp.tick()
-    rk.keep_time()
-
-
-def main():
-  threading.excepthook = excepthook
-  live_map_data_sp_thread()
-
-
-if __name__ == "__main__":
-  main()

sunnypilot/mapd/mapd_installer.py

@@ -1,154 +0,0 @@
-#!/usr/bin/env python3
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import logging
-import os
-import stat
-import time
-import traceback
-import requests
-from pathlib import Path
-from urllib.request import urlopen
-
-from cereal import messaging
-from openpilot.common.params import Params
-from openpilot.system.hardware.hw import Paths
-from openpilot.common.spinner import Spinner
-from openpilot.system.version import is_prebuilt
-from openpilot.sunnypilot.mapd import MAPD_PATH, MAPD_BIN_DIR
-import openpilot.system.sentry as sentry
-
-VERSION = "v1.12.0"
-URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd"
-
-
-def update_installed_version(version: str, params: Params = None) -> None:
-  if params is None:
-    params = Params()
-
-  params.put("MapdVersion", version)
-
-
-class MapdInstallManager:
-  def __init__(self, spinner_ref: Spinner):
-    self._spinner = spinner_ref
-    self._params = Params()
-
-  def download(self) -> None:
-    self.ensure_directories_exist()
-    self._download_file()
-    update_installed_version(VERSION, self._params)
-
-  def check_and_download(self) -> None:
-    if self.download_needed():
-      self.download()
-
-  def download_needed(self) -> bool:
-    return not os.path.exists(MAPD_PATH) or self.get_installed_version() != VERSION
-
-  @staticmethod
-  def ensure_directories_exist() -> None:
-    if not os.path.exists(Paths.mapd_root()):
-      os.makedirs(Paths.mapd_root())
-    if not os.path.exists(MAPD_BIN_DIR):
-      os.makedirs(MAPD_BIN_DIR)
-
-  @staticmethod
-  def _safe_write_and_set_executable(file_path: Path, content: bytes) -> None:
-    with open(file_path, 'wb') as output:
-      output.write(content)
-      output.flush()
-      os.fsync(output.fileno())
-    current_permissions = stat.S_IMODE(os.lstat(file_path).st_mode)
-    os.chmod(file_path, current_permissions | stat.S_IEXEC)
-
-  def _download_file(self, num_retries=5) -> None:
-    temp_file = Path(MAPD_PATH + ".tmp")
-    download_timeout = 60
-    for cnt in range(num_retries):
-      try:
-        response = requests.get(URL, stream=True, timeout=download_timeout)
-        response.raise_for_status()
-        self._safe_write_and_set_executable(temp_file, response.content)
-        # No exceptions encountered. Safe to replace original file.
-        temp_file.replace(MAPD_PATH)
-        return
-      except requests.exceptions.ReadTimeout:
-        self._spinner.update(f"ReadTimeout caught. Timeout is [{download_timeout}]. Retrying download... [{cnt}]")
-        time.sleep(0.5)
-      except requests.exceptions.RequestException as e:
-        self._spinner.update(f"RequestException caught: {e}. Retrying download... [{cnt}]")
-        time.sleep(0.5)
-
-    # Delete temp file if the process was not successful.
-    if temp_file.exists():
-      temp_file.unlink()
-    logging.error("Failed to download file after all retries")
-
-  def get_installed_version(self) -> str:
-    return str(self._params.get("MapdVersion") or "")
-
-  def wait_for_internet_connection(self, return_on_failure: bool = False) -> bool:
-    max_retries = 10
-    for retries in range(max_retries + 1):
-      self._spinner.update(f"Waiting for internet connection... [{retries}/{max_retries}]")
-      time.sleep(2)
-      try:
-        _ = urlopen('https://sentry.io', timeout=10)
-        return True
-      except Exception as e:
-        print(f'Wait for internet failed: {e}')
-        if return_on_failure and retries == max_retries:
-          return False
-
-    return False
-
-  def non_prebuilt_install(self) -> None:
-    sm = messaging.SubMaster(['deviceState'])
-    metered = sm['deviceState'].networkMetered
-
-    if metered:
-      self._spinner.update("Can't proceed with mapd install since network is metered!")
-      time.sleep(5)
-      return
-
-    try:
-      self.ensure_directories_exist()
-      if not self.download_needed():
-        self._spinner.update("Mapd is good!")
-        time.sleep(0.1)
-        return
-
-      if self.wait_for_internet_connection(return_on_failure=True):
-        self._spinner.update(f"Downloading pfeiferj's mapd [{self.get_installed_version()}] => [{VERSION}].")
-        time.sleep(0.1)
-        self.check_and_download()
-      self._spinner.close()
-
-    except Exception:
-      for i in range(6):
-        self._spinner.update("Failed to download OSM maps won't work until properly downloaded!" +
-                             "Try again manually rebooting. " +
-                             f"Boot will continue in {5 - i}s...")
-        time.sleep(1)
-
-      sentry.init(sentry.SentryProject.SELFDRIVE)
-      traceback.print_exc()
-      sentry.capture_exception()
-
-
-if __name__ == "__main__":
-  spinner = Spinner()
-  install_manager = MapdInstallManager(spinner)
-  install_manager.ensure_directories_exist()
-  if is_prebuilt():
-    debug_msg = f"[DEBUG] This is prebuilt, no mapd install required. VERSION: [{VERSION}], Param [{install_manager.get_installed_version()}]"
-    spinner.update(debug_msg)
-    update_installed_version(VERSION)
-  else:
-    spinner.update(f"Checking if mapd is installed and valid. Prebuilt [{is_prebuilt()}]")
-    install_manager.non_prebuilt_install()

sunnypilot/mapd/mapd_manager.py

@@ -1,144 +0,0 @@
-#!/usr/bin/env python3
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import json
-import platform
-import os
-import glob
-import shutil
-from datetime import datetime
-
-from openpilot.common.params import Params
-from openpilot.common.realtime import Ratekeeper, config_realtime_process
-from openpilot.common.swaglog import cloudlog
-from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
-from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
-from openpilot.system.hardware.hw import Paths
-from openpilot.sunnypilot.mapd import MAPD_PATH
-from openpilot.sunnypilot.mapd.mapd_installer import VERSION, update_installed_version
-
-# PFEIFER - MAPD {{
-params = Params()
-mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else params
-# }} PFEIFER - MAPD
-
-
-def get_files_for_cleanup() -> list[str]:
-  paths = [
-    f"{Paths.mapd_root()}/db",
-    f"{Paths.mapd_root()}/v*"
-  ]
-  files_to_remove = []
-  for path in paths:
-    if os.path.exists(path):
-      files = glob.glob(path + '/**', recursive=True)
-      files_to_remove.extend(files)
-  # check for version and mapd files
-  if not os.path.isfile(MAPD_PATH):
-    files_to_remove.append(MAPD_PATH)
-  return files_to_remove
-
-
-def cleanup_old_osm_data(files_to_remove: list[str]) -> None:
-  for file in files_to_remove:
-    # Remove trailing slash if path is file
-    if file.endswith('/') and os.path.isfile(file[:-1]):
-      file = file[:-1]
-    # Try to remove as file or symbolic link first
-    if os.path.islink(file) or os.path.isfile(file):
-      os.remove(file)
-    elif os.path.isdir(file):  # If it's a directory
-      shutil.rmtree(file, ignore_errors=False)
-
-
-def request_refresh_osm_location_data(nations: list[str], states: list[str] | None = None) -> None:
-  params.put("OsmDownloadedDate", str(datetime.now().timestamp()))
-  params.put_bool("OsmDbUpdatesCheck", False)
-
-  osm_download_locations = {
-    "nations": nations,
-    "states": states or []
-  }
-
-  print(f"Downloading maps for {json.dumps(osm_download_locations)}")
-  mem_params.put("OSMDownloadLocations", osm_download_locations)
-
-
-def filter_nations_and_states(nations: list[str], states: list[str] | None = None) -> tuple[list[str], list[str]]:
-  """Filters and prepares nation and state data for OSM map download.
-
-  If the nation is 'US' and a specific state is provided, the nation 'US' is removed from the list.
-  If the nation is 'US' and the state is 'All', the 'All' is removed from the list.
-  The idea behind these filters is that if a specific state in the US is provided,
-  there's no need to download map data for the entire US. Conversely,
-  if the state is unspecified (i.e., 'All'), we intend to download map data for the whole US,
-  and 'All' isn't a valid state name, so it's removed.
-
-  Parameters:
-  nations (list): A list of nations for which the map data is to be downloaded.
-  states (list, optional): A list of states for which the map data is to be downloaded. Defaults to None.
-
-  Returns:
-  tuple: Two lists. The first list is filtered nations and the second list is filtered states.
-  """
-
-  if "US" in nations and states and not any(x.lower() == "all" for x in states):
-    # If a specific state in the US is provided, remove 'US' from nations
-    nations.remove("US")
-  elif "US" in nations and states and any(x.lower() == "all" for x in states):
-    # If 'All' is provided as a state (case invariant), remove those instances from states
-    states = [x for x in states if x.lower() != "all"]
-  elif "US" not in nations and states and any(x.lower() == "all" for x in states):
-    states.remove("All")
-  return nations, states or []
-
-
-def update_osm_db() -> None:
-  if params.get_bool("OsmDbUpdatesCheck"):
-    cleanup_old_osm_data(get_files_for_cleanup())
-    country = params.get("OsmLocationName", return_default=True)
-    state = params.get("OsmStateName", return_default=True)
-    filtered_nations, filtered_states = filter_nations_and_states([country], [state])
-    request_refresh_osm_location_data(filtered_nations, filtered_states)
-
-  if not mem_params.get("OSMDownloadBounds"):
-    mem_params.put("OSMDownloadBounds", "")
-
-  if not mem_params.get("LastGPSPosition"):
-    mem_params.put("LastGPSPosition", "{}")
-
-
-def main_thread():
-  update_installed_version(VERSION, params)
-  config_realtime_process([0, 1, 2, 3], 5)
-
-  rk = Ratekeeper(1, print_delay_threshold=None)
-  live_map_sp = OsmMapData()
-
-  # Create folder needed for OSM
-  try:
-    os.mkdir(Paths.mapd_root())
-  except FileExistsError:
-    pass
-  except PermissionError:
-    cloudlog.exception(f"mapd: failed to make {Paths.mapd_root()}")
-
-  while True:
-    show_alert = get_files_for_cleanup() and params.get_bool("OsmLocal")
-    set_offroad_alert("Offroad_OSMUpdateRequired", show_alert, "This alert will be cleared when new maps are downloaded.")
-
-    update_osm_db()
-    live_map_sp.tick()
-    rk.keep_time()
-
-
-def main():
-  main_thread()
-
-
-if __name__ == "__main__":
-  main()

sunnypilot/mapd/tests/mapd_hash

@@ -1 +0,0 @@
-fdb3b49ee19956e6ce09fdc3373cbba557f1263b2180e9f344c1d4053852284b

sunnypilot/mapd/tests/test_mapd_version.py

@@ -1,19 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from openpilot.sunnypilot import get_file_hash
-from openpilot.sunnypilot.mapd import MAPD_PATH
-from openpilot.sunnypilot.mapd.update_version import MAPD_HASH_PATH
-
-
-class TestMapdVersion:
-  def test_compare_versions(self):
-    mapd_hash = get_file_hash(MAPD_PATH)
-
-    with open(MAPD_HASH_PATH) as f:
-      current_hash = f.read().strip()
-
-    assert current_hash == mapd_hash, "Run sunnypilot/mapd/update_version.py to update the current mapd version and hash"

sunnypilot/mapd/update_version.py

@@ -1,97 +0,0 @@
-#!/usr/bin/env python3
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import argparse
-import os
-import re
-
-from openpilot.sunnypilot import get_file_hash
-from openpilot.common.basedir import BASEDIR
-from openpilot.sunnypilot.mapd import MAPD_PATH
-
-MAPD_HASH_PATH = os.path.join(BASEDIR, "sunnypilot", "mapd", "tests", "mapd_hash")
-MAPD_VERSION_PATH = os.path.join(BASEDIR, "sunnypilot", "mapd", "mapd_installer.py")
-
-
-def update_mapd_hash():
-  mapd_hash = get_file_hash(MAPD_PATH)
-
-  with open(MAPD_HASH_PATH, "w") as f:
-    f.write(mapd_hash)
-
-  print(f"Generated and updated new mapd hash to {MAPD_HASH_PATH}")
-
-
-def get_current_mapd_version(path: str) -> str:
-  print("[GET CURRENT MAPD VERSION]")
-  with open(path) as f:
-    for line in f:
-      if line.strip().startswith("VERSION"):
-        # Match VERSION = 'v1.11.0' or VERSION="v1.11.0" (with optional spaces)
-        match = re.search(r'VERSION\s*=\s*[\'"]([^\'"]+)[\'"]', line)
-        if match:
-          ver = match.group(1)
-          print(f'Current mapd version: "{ver}"')
-          return ver
-        else:
-          print("[ERROR] VERSION line found but no quoted value detected.")
-          return ""
-  print("[ERROR] VERSION not found in file!")
-  return ""
-
-
-def update_mapd_version(ver: str, path: str):
-  print("[CHANGE CURRENT MAPD VERSION]")
-
-  with open(path) as f:
-    lines = f.readlines()
-
-  found = False
-  new_lines = []
-  for line in lines:
-    if not found and line.startswith("VERSION ="):
-      new_lines.append(f'VERSION = "{ver}"\n')
-      found = True
-      new_lines.extend(lines[lines.index(line) + 1:])
-      break
-    else:
-      new_lines.append(line)
-
-  if not found:
-    print("[ERROR] VERSION line not found! Aborting without writing.")
-    return
-
-  with open(path, "w") as f:
-    f.writelines(new_lines)
-
-  print(f'New mapd version: "{ver}"')
-  print("[DONE]")
-
-
-if __name__ == "__main__":
-  parser = argparse.ArgumentParser(description="Update mapd version and hash")
-  parser.add_argument("--new_ver", type=str, help="New mapd version")
-  args = parser.parse_args()
-
-  if not args.new_ver:
-    print("Warning: No new mapd version provided. Use --new_ver to specify")
-    print("Example:")
-    print("  python sunnypilot/mapd/update_version.py --new_ver \"v1.12.0\"")
-    print("Current mapd version and hash will not be updated! (aborted)")
-    exit(0)
-
-  current_ver = get_current_mapd_version(MAPD_VERSION_PATH)
-  new_ver = f"{args.new_ver}"
-  if current_ver == new_ver:
-    print(f'Proposed mapd version: "{new_ver}"')
-    confirm = input("Proposed mapd version is the same as the current mapd version. Confirm? (y/n): ").upper().strip()
-    if confirm != "Y":
-      print("Current mapd version and hash will not be updated! (aborted)")
-      exit(0)
-
-  update_mapd_version(new_ver, MAPD_VERSION_PATH)
-  update_mapd_hash()

sunnypilot/selfdrive/car/cruise_ext.py

@@ -6,16 +6,12 @@ See the LICENSE.md file in the root directory for more details.
 """
 import numpy as np
 
-from cereal import car, custom
+from cereal import car
 from opendbc.car import structs
-from openpilot.common.constants import CV
 from openpilot.common.params import Params
 from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import ACTIVE_STATES as SLA_ACTIVE_STATES
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target
 
 ButtonType = car.CarState.ButtonEvent.Type
-SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
 
 CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
                        ButtonType.setCruise: 0, ButtonType.resumeCruise: 0,
@@ -54,16 +50,6 @@ class VCruiseHelperSP:
 
     self.enable_button_timers = CRUISE_BUTTON_TIMER
 
-    # Speed Limit Assist
-    self.sla_state = SpeedLimitAssistState.disabled
-    self.prev_sla_state = SpeedLimitAssistState.disabled
-    self.has_speed_limit = False
-    self.speed_limit_final_last = 0.
-    self.speed_limit_final_last_kph = 0.
-    self.prev_speed_limit_final_last_kph = 0.
-    self.req_plus = False
-    self.req_minus = False
-
   def read_custom_set_speed_params(self) -> None:
     self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
     self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
@@ -106,33 +92,3 @@ class VCruiseHelperSP:
       return enabled and self.enabled_prev
 
     return enabled
-
-  def update_speed_limit_assist(self, is_metric, LP_SP: custom.LongitudinalPlanSP) -> None:
-    resolver = LP_SP.speedLimit.resolver
-    self.has_speed_limit = resolver.speedLimitValid or resolver.speedLimitLastValid
-    self.speed_limit_final_last = LP_SP.speedLimit.resolver.speedLimitFinalLast
-    self.speed_limit_final_last_kph = self.speed_limit_final_last * CV.MS_TO_KPH
-    self.sla_state = LP_SP.speedLimit.assist.state
-    self.req_plus, self.req_minus = compare_cluster_target(self.v_cruise_cluster_kph * CV.KPH_TO_MS,
-                                                           self.speed_limit_final_last, is_metric)
-
-  @property
-  def update_speed_limit_final_last_changed(self) -> bool:
-    return self.has_speed_limit and bool(self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph)
-
-  def update_speed_limit_assist_pre_active_confirmed(self, button_type: car.CarState.ButtonEvent.Type) -> bool:
-    if self.sla_state == SpeedLimitAssistState.preActive or self.prev_sla_state == SpeedLimitAssistState.preActive:
-      if button_type == ButtonType.decelCruise and self.req_minus:
-        return True
-      if button_type == ButtonType.accelCruise and self.req_plus:
-        return True
-
-    return False
-
-  def update_speed_limit_assist_v_cruise_non_pcm(self) -> None:
-    if self.sla_state in SLA_ACTIVE_STATES and (self.prev_sla_state not in SLA_ACTIVE_STATES or
-                                                self.update_speed_limit_final_last_changed):
-      self.v_cruise_kph = np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
-
-    self.prev_sla_state = self.sla_state
-    self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph

sunnypilot/selfdrive/car/interfaces.py

@@ -11,7 +11,6 @@ from opendbc.car.interfaces import CarInterfaceBase
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import get_nn_model_path
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import set_speed_limit_assist_availability
 
 import openpilot.system.sentry as sentry
 
@@ -86,10 +85,6 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS
     cloudlog.warning("openpilot Longitudinal Control and ICBM not available, cleaning up params")
     params.remove("DynamicExperimentalControl")
     params.remove("CustomAccIncrementsEnabled")
-    params.remove("SmartCruiseControlVision")
-    params.remove("SmartCruiseControlMap")
-
-  set_speed_limit_assist_availability(CP, CP_SP, params)
 
 
 def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:

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

@@ -1,9 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from openpilot.common.constants import CV
-
-MIN_V = 20 * CV.KPH_TO_MS  # Do not operate under 20 km/h

sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py

@@ -1,261 +0,0 @@
-import json
-import math
-import platform
-
-from cereal import custom
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.navd.helpers import coordinate_from_param, Coordinate
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
-
-MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
-
-ACTIVE_STATES = (MapState.turning, )
-ENABLED_STATES = (MapState.enabled, MapState.overriding, *ACTIVE_STATES)
-
-R = 6373000.0  # approximate radius of earth in meters
-TO_RADIANS = math.pi / 180
-TO_DEGREES = 180 / math.pi
-TARGET_JERK = -0.6  # m/s^3 There's some jounce limits that are not consistent so we're fudging this some
-TARGET_ACCEL = -1.2  # m/s^2 should match up with the long planner limit
-TARGET_OFFSET = 1.0  # seconds - This controls how soon before the curve you reach the target velocity. It also helps
-                     # reach the target velocity when inaccuracies in the distance modeling logic would cause overshoot.
-                     # The value is multiplied against the target velocity to determine the additional distance. This is
-                     # done to keep the distance calculations consistent but results in the offset actually being less
-                     # time than specified depending on how much of a speed differential there is between v_ego and the
-                     # target velocity.
-
-
-def velocities_from_param(param: str, params: Params):
-  if params is None:
-    params = Params()
-
-  json_str = params.get(param)
-  if json_str is None:
-    return None
-
-  velocities = json.loads(json_str)
-
-  return velocities
-
-
-def calculate_accel(t, target_jerk, a_ego):
-  return a_ego + target_jerk * t
-
-
-def calculate_velocity(t, target_jerk, a_ego, v_ego):
-  return v_ego + a_ego * t + target_jerk/2 * (t ** 2)
-
-
-def calculate_distance(t, target_jerk, a_ego, v_ego):
-  return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3)
-
-
-# points should be in radians
-# output is meters
-def distance_to_point(ax, ay, bx, by):
-  a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2)
-  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
-
-  return R * c  # in meters
-
-
-class SmartCruiseControlMap:
-  v_target: float = 0
-  a_target: float = 0.
-  v_ego: float = 0.
-  a_ego: float = 0.
-  output_v_target: float = V_CRUISE_UNSET
-  output_a_target: float = 0.
-
-  def __init__(self):
-    self.params = Params()
-    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
-    self.enabled = self.params.get_bool("SmartCruiseControlMap")
-    self.long_enabled = False
-    self.long_override = False
-    self.is_enabled = False
-    self.is_active = False
-    self.state = MapState.disabled
-    self.v_cruise = 0
-    self.target_lat = 0.0
-    self.target_lon = 0.0
-    self.frame = -1
-
-    self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
-    self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
-
-  def get_v_target_from_control(self) -> float:
-    if self.is_active:
-      return max(self.v_target, MIN_V)
-
-    return V_CRUISE_UNSET
-
-  def get_a_target_from_control(self) -> float:
-    return self.a_ego
-
-  def update_params(self):
-    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.enabled = self.params.get_bool("SmartCruiseControlMap")
-
-  def update_calculations(self) -> None:
-    self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
-    lat = self.last_position.latitude
-    lon = self.last_position.longitude
-
-    self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
-
-    if self.last_position is None or self.target_velocities is None:
-      return
-
-    min_dist = 1000
-    min_idx = 0
-    distances = []
-
-    # find our location in the path
-    for i in range(len(self.target_velocities)):
-      target_velocity = self.target_velocities[i]
-      tlat = target_velocity["latitude"]
-      tlon = target_velocity["longitude"]
-      d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS)
-      distances.append(d)
-      if d < min_dist:
-        min_dist = d
-        min_idx = i
-
-    # only look at values from our current position forward
-    forward_points = self.target_velocities[min_idx:]
-    forward_distances = distances[min_idx:]
-
-    # find velocities that we are within the distance we need to adjust for
-    valid_velocities = []
-    for i in range(len(forward_points)):
-      target_velocity = forward_points[i]
-      tlat = target_velocity["latitude"]
-      tlon = target_velocity["longitude"]
-      tv = target_velocity["velocity"]
-      if tv > self.v_ego:
-        continue
-
-      d = forward_distances[i]
-
-      a_diff = (self.a_ego - TARGET_ACCEL)
-      accel_t = abs(a_diff / TARGET_JERK)
-      min_accel_v = calculate_velocity(accel_t, TARGET_JERK, self.a_ego, self.v_ego)
-
-      max_d = 0
-      if tv > min_accel_v:
-        # calculate time needed based on target jerk
-        a = 0.5 * TARGET_JERK
-        b = self.a_ego
-        c = self.v_ego - tv
-        t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
-        t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
-        if not isinstance(t_a, complex) and t_a > 0:
-          t = t_a
-        else:
-          t = t_b
-        if isinstance(t, complex):
-          continue
-
-        max_d = max_d + calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
-      else:
-        t = accel_t
-        max_d = calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
-
-        # calculate additional time needed based on target accel
-        t = abs((min_accel_v - tv) / TARGET_ACCEL)
-        max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v)
-
-      if d < max_d + tv * TARGET_OFFSET:
-        valid_velocities.append((float(tv), tlat, tlon))
-
-    # Find the smallest velocity we need to adjust for
-    min_v = 100.0
-    target_lat = 0.0
-    target_lon = 0.0
-    for tv, lat, lon in valid_velocities:
-      if tv < min_v:
-        min_v = tv
-        target_lat = lat
-        target_lon = lon
-
-    if self.v_target < min_v and not (self.target_lat == 0 and self.target_lon == 0):
-      for i in range(len(forward_points)):
-        target_velocity = forward_points[i]
-        tlat = target_velocity["latitude"]
-        tlon = target_velocity["longitude"]
-        tv = target_velocity["velocity"]
-        if tv > self.v_ego:
-          continue
-
-        if tlat == self.target_lat and tlon == self.target_lon and tv == self.v_target:
-          return
-
-      # not found so let's reset
-      self.v_target = 0.0
-      self.target_lat = 0.0
-      self.target_lon = 0.0
-
-    self.v_target = min_v
-    self.target_lat = target_lat
-    self.target_lon = target_lon
-
-  def _update_state_machine(self) -> tuple[bool, bool]:
-    # ENABLED, TURNING
-    if self.state != MapState.disabled:
-      if not self.long_enabled or not self.enabled:
-        self.state = MapState.disabled
-      elif self.long_override:
-        self.state = MapState.overriding
-
-      else:
-        # ENABLED
-        if self.state == MapState.enabled:
-          if self.v_cruise > self.v_target != 0:
-            self.state = MapState.turning
-
-        # TURNING
-        elif self.state == MapState.turning:
-          if self.v_cruise <= self.v_target or self.v_target == 0:
-            self.state = MapState.enabled
-
-        # OVERRIDING
-        elif self.state == MapState.overriding:
-          if not self.long_override:
-            if self.v_cruise > self.v_target != 0:
-              self.state = MapState.turning
-            else:
-              self.state = MapState.enabled
-
-    # DISABLED
-    elif self.state == MapState.disabled:
-      if self.long_enabled and self.enabled:
-        if self.long_override:
-          self.state = MapState.overriding
-        else:
-          self.state = MapState.enabled
-
-    enabled = self.state in ENABLED_STATES
-    active = self.state in ACTIVE_STATES
-
-    return enabled, active
-
-  def update(self, long_enabled: bool, long_override: bool, v_ego, a_ego, v_cruise) -> None:
-    self.long_enabled = long_enabled
-    self.long_override = long_override
-    self.v_ego = v_ego
-    self.a_ego = a_ego
-    self.v_cruise = v_cruise
-
-    self.update_params()
-    self.update_calculations()
-
-    self.is_enabled, self.is_active = self._update_state_machine()
-
-    self.output_v_target = self.get_v_target_from_control()
-    self.output_a_target = self.get_a_target_from_control()
-
-    self.frame += 1

sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py

@@ -1,19 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import cereal.messaging as messaging
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
-
-
-class SmartCruiseControl:
-  def __init__(self):
-    self.vision = SmartCruiseControlVision()
-    self.map = SmartCruiseControlMap()
-
-  def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None:
-    self.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise)
-    self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)

sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py

@@ -1,58 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import platform
-
-from cereal import custom
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
-
-MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
-
-
-class TestSmartCruiseControlMap:
-
-  def setup_method(self):
-    self.params = Params()
-    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
-    self.reset_params()
-    self.scc_m = SmartCruiseControlMap()
-
-  def reset_params(self):
-    self.params.put_bool("SmartCruiseControlMap", True)
-
-    # TODO-SP: mock data from gpsLocation
-    self.params.put("LastGPSPosition", "{}")
-    self.params.put("MapTargetVelocities", "{}")
-
-  def test_initial_state(self):
-    assert self.scc_m.state == VisionState.disabled
-    assert not self.scc_m.is_active
-    assert self.scc_m.output_v_target == V_CRUISE_UNSET
-    assert self.scc_m.output_a_target == 0.
-
-  def test_system_disabled(self):
-    self.params.put_bool("SmartCruiseControlMap", False)
-    self.scc_m.enabled = self.params.get_bool("SmartCruiseControlMap")
-
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_m.update(True, False, 0., 0., 0.)
-    assert self.scc_m.state == VisionState.disabled
-    assert not self.scc_m.is_active
-
-  def test_disabled(self):
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_m.update(False, False, 0., 0., 0.)
-    assert self.scc_m.state == VisionState.disabled
-
-  def test_transition_disabled_to_enabled(self):
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_m.update(True, False, 0., 0., 0.)
-    assert self.scc_m.state == VisionState.enabled
-
-  # TODO-SP: mock data from modelV2 to test other states

sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py

@@ -1,104 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import numpy as np
-
-import cereal.messaging as messaging
-from cereal import custom, log
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
-
-VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
-
-
-def generate_modelV2():
-  model = messaging.new_message('modelV2')
-  position = log.XYZTData.new_message()
-  speed = 30
-  position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)]
-  model.modelV2.position = position
-  orientation = log.XYZTData.new_message()
-  curvature = 0.05
-  orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS]
-  orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
-  model.modelV2.orientation = orientation
-  orientationRate = log.XYZTData.new_message()
-  orientationRate.z = [float(z) for z in ModelConstants.T_IDXS]
-  model.modelV2.orientationRate = orientationRate
-  velocity = log.XYZTData.new_message()
-  velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
-  velocity.x[0] = float(speed)  # always start at current speed
-  model.modelV2.velocity = velocity
-  acceleration = log.XYZTData.new_message()
-  acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
-  acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)]
-  model.modelV2.acceleration = acceleration
-
-  return model
-
-
-def generate_carState():
-  car_state = messaging.new_message('carState')
-  speed = 30
-  v_cruise = 50
-  car_state.carState.vEgo = float(speed)
-  car_state.carState.standstill = False
-  car_state.carState.vCruise = float(v_cruise * 3.6)
-
-  return car_state
-
-
-def generate_controlsState():
-  controls_state = messaging.new_message('controlsState')
-  controls_state.controlsState.curvature = 0.05
-
-  return controls_state
-
-
-class TestSmartCruiseControlVision:
-
-  def setup_method(self):
-    self.params = Params()
-    self.reset_params()
-    self.scc_v = SmartCruiseControlVision()
-
-    mdl = generate_modelV2()
-    cs = generate_carState()
-    controls_state = generate_controlsState()
-    self.sm = {'modelV2': mdl.modelV2, 'carState': cs.carState, 'controlsState': controls_state.controlsState}
-
-  def reset_params(self):
-    self.params.put_bool("SmartCruiseControlVision", True)
-
-  def test_initial_state(self):
-    assert self.scc_v.state == VisionState.disabled
-    assert not self.scc_v.is_active
-    assert self.scc_v.output_v_target == V_CRUISE_UNSET
-    assert self.scc_v.output_a_target == 0.
-
-  def test_system_disabled(self):
-    self.params.put_bool("SmartCruiseControlVision", False)
-    self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision")
-
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_v.update(self.sm, True, False, 0., 0., 0.)
-    assert self.scc_v.state == VisionState.disabled
-    assert not self.scc_v.is_active
-
-  def test_disabled(self):
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_v.update(self.sm, False, False, 0., 0., 0.)
-    assert self.scc_v.state == VisionState.disabled
-
-  def test_transition_disabled_to_enabled(self):
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_v.update(self.sm, True, False, 0., 0., 0.)
-    assert self.scc_v.state == VisionState.enabled
-
-  # TODO-SP: mock modelV2 data to test other states

sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py

@@ -1,203 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import numpy as np
-
-import cereal.messaging as messaging
-from cereal import custom
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
-
-VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
-
-ACTIVE_STATES = (VisionState.entering, VisionState.turning, VisionState.leaving)
-ENABLED_STATES = (VisionState.enabled, VisionState.overriding, *ACTIVE_STATES)
-
-_ENTERING_PRED_LAT_ACC_TH = 1.3  # Predicted Lat Acc threshold to trigger entering turn state.
-_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1  # Predicted Lat Acc threshold to abort entering state if speed drops.
-
-_TURNING_LAT_ACC_TH = 1.6  # Lat Acc threshold to trigger turning state.
-
-_LEAVING_LAT_ACC_TH = 1.3  # Lat Acc threshold to trigger leaving turn state.
-_FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger the end of the turn cycle.
-
-_A_LAT_REG_MAX = 2.  # Maximum lateral acceleration
-
-_NO_OVERSHOOT_TIME_HORIZON = 4.  # s. Time to use for velocity desired based on a_target when not overshooting.
-
-# Lookup table for the minimum smooth deceleration during the ENTERING state
-# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
-_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.]  # min decel value allowed on ENTERING state
-_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.]  # absolute value of lat acc ahead
-
-# Lookup table for the acceleration for the TURNING state
-# depending on the current lateral acceleration of the vehicle.
-_TURNING_ACC_V = [0.5, 0., -0.4]  # acc value
-_TURNING_ACC_BP = [1.5, 2.3, 3.]  # absolute value of current lat acc
-
-_LEAVING_ACC = 0.5  # Conformable acceleration to regain speed while leaving a turn.
-
-
-class SmartCruiseControlVision:
-  v_target: float = 0
-  a_target: float = 0.
-  v_ego: float = 0.
-  a_ego: float = 0.
-  output_v_target: float = V_CRUISE_UNSET
-  output_a_target: float = 0.
-
-  def __init__(self):
-    self.params = Params()
-    self.frame = -1
-    self.long_enabled = False
-    self.long_override = False
-    self.is_enabled = False
-    self.is_active = False
-    self.enabled = self.params.get_bool("SmartCruiseControlVision")
-    self.v_cruise_setpoint = 0.
-
-    self.state = VisionState.disabled
-    self.current_lat_acc = 0.
-    self.max_pred_lat_acc = 0.
-
-  def get_a_target_from_control(self) -> float:
-    return self.a_target
-
-  def get_v_target_from_control(self) -> float:
-    if self.is_active:
-      return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
-
-    return V_CRUISE_UNSET
-
-  def _update_params(self) -> None:
-    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.enabled = self.params.get_bool("SmartCruiseControlVision")
-
-  def _update_calculations(self, sm: messaging.SubMaster) -> None:
-    if not self.long_enabled:
-      return
-    else:
-      rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
-      vel_plan = np.array(sm['modelV2'].velocity.x)
-
-      self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
-
-      # get the maximum lat accel from the model
-      predicted_lat_accels = rate_plan * vel_plan
-      self.max_pred_lat_acc = np.amax(predicted_lat_accels)
-
-      # get the maximum curve based on the current velocity
-      v_ego = max(self.v_ego, 0.1)  # ensure a value greater than 0 for calculations
-      max_curve = self.max_pred_lat_acc / (v_ego**2)
-
-      # Get the target velocity for the maximum curve
-      self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
-
-  def _update_state_machine(self) -> tuple[bool, bool]:
-    # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
-    if self.state != VisionState.disabled:
-      # longitudinal and feature disable always have priority in a non-disabled state
-      if not self.long_enabled or not self.enabled:
-        self.state = VisionState.disabled
-      elif self.long_override:
-        self.state = VisionState.overriding
-
-      else:
-        # ENABLED
-        if self.state == VisionState.enabled:
-          # Do not enter a turn control cycle if the speed is low.
-          if self.v_ego <= MIN_V:
-            pass
-          # If significant lateral acceleration is predicted ahead, then move to Entering turn state.
-          elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
-            self.state = VisionState.entering
-
-        # OVERRIDING
-        elif self.state == VisionState.overriding:
-          if not self.long_override:
-            self.state = VisionState.enabled
-
-        # ENTERING
-        elif self.state == VisionState.entering:
-          # Transition to Turning if current lateral acceleration is over the threshold.
-          if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
-            self.state = VisionState.turning
-          # Abort if the predicted lateral acceleration drops
-          elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
-            self.state = VisionState.enabled
-
-        # TURNING
-        elif self.state == VisionState.turning:
-          # Transition to Leaving if current lateral acceleration drops below a threshold.
-          if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
-            self.state = VisionState.leaving
-
-        # LEAVING
-        elif self.state == VisionState.leaving:
-          # Transition back to Turning if current lateral acceleration goes back over the threshold.
-          if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
-            self.state = VisionState.turning
-          # Finish if current lateral acceleration goes below a threshold.
-          elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
-            self.state = VisionState.enabled
-
-    # DISABLED
-    elif self.state == VisionState.disabled:
-      if self.long_enabled and self.enabled:
-        if self.long_override:
-          self.state = VisionState.overriding
-        else:
-          self.state = VisionState.enabled
-
-    enabled = self.state in ENABLED_STATES
-    active = self.state in ACTIVE_STATES
-
-    return enabled, active
-
-  def _update_solution(self) -> float:
-    # DISABLED, ENABLED, OVERRIDING
-    if self.state not in ACTIVE_STATES:
-      # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
-      # the smooth deceleration.
-      a_target = self.a_ego
-    # ENTERING
-    elif self.state == VisionState.entering:
-      # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
-      a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
-    # TURNING
-    elif self.state == VisionState.turning:
-      # When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
-      a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
-    # LEAVING
-    elif self.state == VisionState.leaving:
-      # When leaving, we provide a comfortable acceleration to regain speed.
-      a_target = _LEAVING_ACC
-    else:
-      raise NotImplementedError(f"SCC-V state not supported: {self.state}")
-
-    return a_target
-
-  def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
-             v_cruise_setpoint: float) -> None:
-    self.long_enabled = long_enabled
-    self.long_override = long_override
-    self.v_ego = v_ego
-    self.a_ego = a_ego
-    self.v_cruise_setpoint = v_cruise_setpoint
-
-    self._update_params()
-    self._update_calculations(sm)
-
-    self.is_enabled, self.is_active = self._update_state_machine()
-    self.a_target = self._update_solution()
-
-    self.output_v_target = self.get_v_target_from_control()
-    self.output_a_target = self.get_a_target_from_control()
-
-    self.frame += 1

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

@@ -1,19 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-LIMIT_ADAPT_ACC = -1.  # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
-LIMIT_MAX_MAP_DATA_AGE = 10.  # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
-
-# Speed Limit Assist constants
-PCM_LONG_REQUIRED_MAX_SET_SPEED = {
-  True: (33.3333, 36.1111),  # km/h, (120, 130)
-  False: (31.2928, 35.7632),  # mph, (70, 80)
-}
-
-CONFIRM_SPEED_THRESHOLD = {
-  True: 80,   # km/h
-  False: 50,  # mph
-}

sunnypilot/selfdrive/controls/lib/speed_limit/common.py

@@ -1,29 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-from openpilot.sunnypilot import IntEnumBase
-
-
-class Policy(IntEnumBase):
-  car_state_only = 0
-  map_data_only = 1
-  car_state_priority = 2
-  map_data_priority = 3
-  combined = 4
-
-
-class OffsetType(IntEnumBase):
-  off = 0
-  fixed = 1
-  percentage = 2
-
-
-class Mode(IntEnumBase):
-  off = 0
-  information = 1
-  warning = 2
-  assist = 3

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

@@ -1,44 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-from cereal import custom, car
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode as SpeedLimitMode
-
-
-def compare_cluster_target(v_cruise_cluster: float, target_set_speed: float, is_metric: bool) -> tuple[bool, bool]:
-  speed_conv = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH
-  v_cruise_cluster_conv = round(v_cruise_cluster * speed_conv)
-  target_set_speed_conv = round(target_set_speed * speed_conv)
-
-  req_plus = v_cruise_cluster_conv < target_set_speed_conv
-  req_minus = v_cruise_cluster_conv > target_set_speed_conv
-
-  return req_plus, req_minus
-
-
-def set_speed_limit_assist_availability(CP: car.CarParams, CP_SP: custom.CarParamsSP, params: Params = None) -> bool:
-  if params is None:
-    params = Params()
-
-  is_release = params.get_bool("IsReleaseSpBranch")
-  disallow_in_release = CP.brand == "tesla" and is_release
-  always_disallow = CP.brand == "rivian"
-  allowed = True
-
-  if disallow_in_release or always_disallow:
-    allowed = False
-
-  if not CP.openpilotLongitudinalControl and CP_SP.pcmCruiseSpeed:
-    allowed = False
-
-  if not allowed:
-    if params.get("SpeedLimitMode", return_default=True) == SpeedLimitMode.assist:
-      params.put("SpeedLimitMode", int(SpeedLimitMode.warning))
-
-  return allowed

sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py

@@ -1,414 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import time
-
-from cereal import custom, car
-from openpilot.common.params import Params
-from openpilot.common.constants import CV
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target, set_speed_limit_assist_availability
-
-ButtonType = car.CarState.ButtonEvent.Type
-EventNameSP = custom.OnroadEventSP.EventName
-SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
-SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
-
-ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
-ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES)
-
-DISABLED_GUARD_PERIOD = 0.5  # secs.
-# secs. Time to wait after activation before considering temp deactivation signal.
-PRE_ACTIVE_GUARD_PERIOD = {
-  True: 15,
-  False: 5,
-}
-SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1  # secs. Time to wait after speed limit change before switching to preActive.
-
-LIMIT_MIN_ACC = -1.5  # m/s^2 Maximum deceleration allowed for limit controllers to provide.
-LIMIT_MAX_ACC = 1.0   # m/s^2 Maximum acceleration allowed for limit controllers to provide while active.
-LIMIT_MIN_SPEED = 8.33  # m/s, Minimum speed limit to provide as solution on limit controllers.
-LIMIT_SPEED_OFFSET_TH = -1.  # m/s Maximum offset between speed limit and current speed for adapting state.
-V_CRUISE_UNSET = 255.
-
-CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise)
-CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise)
-CRUISE_BUTTON_CONFIRM_HOLD = 0.5  # secs.
-
-
-class SpeedLimitAssist:
-  _speed_limit_final_last: float
-  _distance: float
-  v_ego: float
-  a_ego: float
-  v_offset: float
-
-  def __init__(self, CP: car.CarParams, CP_SP: custom.CarParamsSP):
-    self.params = Params()
-    self.CP = CP
-    self.CP_SP = CP_SP
-    self.frame = -1
-    self.long_engaged_timer = 0
-    self.pre_active_timer = 0
-    self.is_metric = self.params.get_bool("IsMetric")
-    set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
-    self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
-    self.long_enabled = False
-    self.long_enabled_prev = False
-    self.is_enabled = False
-    self.is_active = False
-    self.output_v_target = V_CRUISE_UNSET
-    self.output_a_target = 0.
-    self.v_ego = 0.
-    self.a_ego = 0.
-    self.v_offset = 0.
-    self.target_set_speed_conv = 0
-    self.prev_target_set_speed_conv = 0
-    self.v_cruise_cluster = 0.
-    self.v_cruise_cluster_prev = 0.
-    self.v_cruise_cluster_conv = 0
-    self.prev_v_cruise_cluster_conv = 0
-    self._has_speed_limit = False
-    self._speed_limit = 0.
-    self._speed_limit_final_last = 0.
-    self.speed_limit_prev = 0.
-    self.speed_limit_final_last_conv = 0
-    self.prev_speed_limit_final_last_conv = 0
-    self._distance = 0.
-    self.state = SpeedLimitAssistState.disabled
-    self._state_prev = SpeedLimitAssistState.disabled
-    self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
-
-    self._plus_hold = 0.
-    self._minus_hold = 0.
-    self._last_carstate_ts = 0.
-
-    # TODO-SP: SLA's own output_a_target for planner
-    # Solution functions mapped to respective states
-    self.acceleration_solutions = {
-      SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target,
-      SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target,
-      SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target,
-      SpeedLimitAssistState.pending: self.get_current_acceleration_as_target,
-      SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration,
-      SpeedLimitAssistState.active: self.get_active_state_target_acceleration,
-    }
-
-  @property
-  def speed_limit_changed(self) -> bool:
-    return self._has_speed_limit and bool(self._speed_limit != self.speed_limit_prev)
-
-  @property
-  def v_cruise_cluster_changed(self) -> bool:
-    return bool(self.v_cruise_cluster_conv != self.prev_v_cruise_cluster_conv)
-
-  @property
-  def target_set_speed_confirmed(self) -> bool:
-    return bool(self.v_cruise_cluster_conv == self.target_set_speed_conv)
-
-  @property
-  def v_cruise_cluster_below_confirm_speed_threshold(self) -> bool:
-    return bool(self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
-
-  def update_active_event(self, events_sp: EventsSP) -> None:
-    if self.v_cruise_cluster_below_confirm_speed_threshold:
-      events_sp.add(EventNameSP.speedLimitChanged)
-    else:
-      events_sp.add(EventNameSP.speedLimitActive)
-
-  def get_v_target_from_control(self) -> float:
-    if self._has_speed_limit:
-      if self.pcm_op_long and self.is_enabled:
-        return self._speed_limit_final_last
-      if not self.pcm_op_long and self.is_active:
-        return self._speed_limit_final_last
-
-    # Fallback
-    return V_CRUISE_UNSET
-
-  # TODO-SP: SLA's own output_a_target for planner
-  def get_a_target_from_control(self) -> float:
-    return self.a_ego
-
-  def update_params(self) -> None:
-    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.is_metric = self.params.get_bool("IsMetric")
-      set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
-      self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
-
-  def update_car_state(self, CS: car.CarState) -> None:
-    now = time.monotonic()
-    self._last_carstate_ts = now
-
-    for b in CS.buttonEvents:
-      if not b.pressed:
-        if b.type in CRUISE_BUTTONS_PLUS:
-          self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
-        elif b.type in CRUISE_BUTTONS_MINUS:
-          self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
-
-  def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool:
-    now = time.monotonic()
-    if req_plus and now <= self._plus_hold:
-      self._plus_hold = 0.
-      return True
-    elif req_minus and now <= self._minus_hold:
-      self._minus_hold = 0.
-      return True
-
-    # expired
-    if now > self._plus_hold:
-      self._plus_hold = 0.
-    if now > self._minus_hold:
-      self._minus_hold = 0.
-    return False
-
-  def update_calculations(self, v_cruise_cluster: float) -> None:
-    speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
-    self.v_cruise_cluster = v_cruise_cluster
-
-    # Update current velocity offset (error)
-    self.v_offset = self._speed_limit_final_last - self.v_ego
-
-    self.speed_limit_final_last_conv = round(self._speed_limit_final_last * speed_conv)
-    self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv)
-
-    cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric]
-    pcm_long_required_max = cst_low if self._has_speed_limit and self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric] else \
-                            cst_high
-    pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
-
-    self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_last_conv
-
-  @property
-  def apply_confirm_speed_threshold(self) -> bool:
-    # below CST: always require user confirmation
-    if self.v_cruise_cluster_below_confirm_speed_threshold:
-      return True
-
-    # at/above CST:
-    # - new speed limit >= CST: auto change
-    # - new speed limit < CST: user confirmation required
-    return bool(self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
-
-  def get_current_acceleration_as_target(self) -> float:
-    return self.a_ego
-
-  def get_adapting_state_target_acceleration(self) -> float:
-    if self._distance > 0:
-      return (self._speed_limit_final_last ** 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:
-    return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
-
-  def _update_confirmed_state(self):
-    if self._has_speed_limit:
-      if self.v_offset < LIMIT_SPEED_OFFSET_TH:
-        self.state = SpeedLimitAssistState.adapting
-      else:
-        self.state = SpeedLimitAssistState.active
-    else:
-      self.state = SpeedLimitAssistState.pending
-
-  def _update_non_pcm_long_confirmed_state(self) -> bool:
-    if self.target_set_speed_confirmed:
-      return True
-
-    if self.state != SpeedLimitAssistState.preActive:
-      return False
-
-    req_plus, req_minus = compare_cluster_target(self.v_cruise_cluster, self._speed_limit_final_last, self.is_metric)
-
-    return self._get_button_release(req_plus, req_minus)
-
-  def update_state_machine_pcm_op_long(self):
-    self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
-    self.pre_active_timer = max(0, self.pre_active_timer - 1)
-
-    # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
-    if self.state != SpeedLimitAssistState.disabled:
-      if not self.long_enabled or not self.enabled:
-        self.state = SpeedLimitAssistState.disabled
-
-      else:
-        # ACTIVE
-        if self.state == SpeedLimitAssistState.active:
-          if self.v_cruise_cluster_changed:
-            self.state = SpeedLimitAssistState.inactive
-          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-          elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH:
-            self.state = SpeedLimitAssistState.adapting
-
-        # ADAPTING
-        elif self.state == SpeedLimitAssistState.adapting:
-          if self.v_cruise_cluster_changed:
-            self.state = SpeedLimitAssistState.inactive
-          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-          elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
-            self.state = SpeedLimitAssistState.active
-
-        # PENDING
-        elif self.state == SpeedLimitAssistState.pending:
-          if self.target_set_speed_confirmed:
-            self._update_confirmed_state()
-          elif self.speed_limit_changed:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-
-        # PRE_ACTIVE
-        elif self.state == SpeedLimitAssistState.preActive:
-          if self.target_set_speed_confirmed:
-            self._update_confirmed_state()
-          elif self.pre_active_timer <= 0:
-            # Timeout - session ended
-            self.state = SpeedLimitAssistState.inactive
-
-        # INACTIVE
-        elif self.state == SpeedLimitAssistState.inactive:
-          pass
-
-    # DISABLED
-    elif self.state == SpeedLimitAssistState.disabled:
-      if self.long_enabled and self.enabled:
-        # start or reset preActive timer if initially enabled or manual set speed change detected
-        if not self.long_enabled_prev or self.v_cruise_cluster_changed:
-          self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
-
-        elif self.long_engaged_timer <= 0:
-          if self.target_set_speed_confirmed:
-            self._update_confirmed_state()
-          elif self._has_speed_limit:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-          else:
-            self.state = SpeedLimitAssistState.pending
-
-    enabled = self.state in ENABLED_STATES
-    active = self.state in ACTIVE_STATES
-
-    return enabled, active
-
-  def update_state_machine_non_pcm_long(self):
-    self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
-    self.pre_active_timer = max(0, self.pre_active_timer - 1)
-
-    # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
-    if self.state != SpeedLimitAssistState.disabled:
-      if not self.long_enabled or not self.enabled:
-        self.state = SpeedLimitAssistState.disabled
-
-      else:
-        # ACTIVE
-        if self.state == SpeedLimitAssistState.active:
-          if self.v_cruise_cluster_changed:
-            self.state = SpeedLimitAssistState.inactive
-
-          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-
-        # PRE_ACTIVE
-        elif self.state == SpeedLimitAssistState.preActive:
-          if self._update_non_pcm_long_confirmed_state():
-            self.state = SpeedLimitAssistState.active
-          elif self.pre_active_timer <= 0:
-            # Timeout - session ended
-            self.state = SpeedLimitAssistState.inactive
-
-        # INACTIVE
-        elif self.state == SpeedLimitAssistState.inactive:
-          if self.speed_limit_changed:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-          elif self._update_non_pcm_long_confirmed_state():
-            self.state = SpeedLimitAssistState.active
-
-    # DISABLED
-    elif self.state == SpeedLimitAssistState.disabled:
-      if self.long_enabled and self.enabled:
-        # start or reset preActive timer if initially enabled or manual set speed change detected
-        if not self.long_enabled_prev or self.v_cruise_cluster_changed:
-          self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
-
-        elif self.long_engaged_timer <= 0:
-          if self._update_non_pcm_long_confirmed_state():
-            self.state = SpeedLimitAssistState.active
-          elif self._has_speed_limit:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-          else:
-            self.state = SpeedLimitAssistState.inactive
-
-    enabled = self.state in ENABLED_STATES
-    active = self.state in ACTIVE_STATES
-
-    return enabled, active
-
-  def update_events(self, events_sp: EventsSP) -> None:
-    if self.state == SpeedLimitAssistState.preActive:
-      events_sp.add(EventNameSP.speedLimitPreActive)
-
-    if self.state == SpeedLimitAssistState.pending and self._state_prev != SpeedLimitAssistState.pending:
-      events_sp.add(EventNameSP.speedLimitPending)
-
-    if self.is_active:
-      if self._state_prev not in ACTIVE_STATES:
-        self.update_active_event(events_sp)
-
-      # only notify if we acquire a valid speed limit
-      # do not check has_speed_limit here
-      elif self._speed_limit != self.speed_limit_prev:
-        if self.speed_limit_prev <= 0:
-          self.update_active_event(events_sp)
-        elif self.speed_limit_prev > 0 and self._speed_limit > 0:
-          self.update_active_event(events_sp)
-
-  def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float,
-             speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None:
-    self.long_enabled = long_enabled
-    self.v_ego = v_ego
-    self.a_ego = a_ego
-
-    self._has_speed_limit = has_speed_limit
-    self._speed_limit = speed_limit
-    self._speed_limit_final_last = speed_limit_final_last
-    self._distance = distance
-
-    self.update_params()
-    self.update_calculations(v_cruise_cluster)
-
-    self._state_prev = self.state
-    if self.pcm_op_long:
-      self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long()
-    else:
-      self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long()
-
-    self.update_events(events_sp)
-
-    # Update change tracking variables
-    self.speed_limit_prev = self._speed_limit
-    self.v_cruise_cluster_prev = self.v_cruise_cluster
-    self.long_enabled_prev = self.long_enabled
-    self.prev_target_set_speed_conv = self.target_set_speed_conv
-    self.prev_v_cruise_cluster_conv = self.v_cruise_cluster_conv
-    self.prev_speed_limit_final_last_conv = self.speed_limit_final_last_conv
-
-    self.output_v_target = self.get_v_target_from_control()
-    self.output_a_target = self.get_a_target_from_control()
-
-    self.frame += 1

sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py

@@ -1,190 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import time
-
-import cereal.messaging as messaging
-from cereal import custom
-from openpilot.common.constants import CV
-from openpilot.common.gps import get_gps_location_service
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD, get_sanitize_int_param
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy, OffsetType
-
-SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
-
-ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values())
-
-
-class SpeedLimitResolver:
-  limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
-  distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
-  v_ego: float
-  speed_limit: float
-  speed_limit_last: float
-  speed_limit_final: float
-  speed_limit_final_last: float
-  distance: float
-  source: custom.LongitudinalPlanSP.SpeedLimit.Source
-  speed_limit_offset: float
-
-  def __init__(self):
-    self.params = Params()
-    self.frame = -1
-
-    self._gps_location_service = get_gps_location_service(self.params)
-    self.limit_solutions = {}  # Store for speed limit solutions from different sources
-    self.distance_solutions = {}  # Store for distance to current speed limit start for different sources
-
-    self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
-    self.policy = get_sanitize_int_param(
-      "SpeedLimitPolicy",
-      Policy.min().value,
-      Policy.max().value,
-      self.params
-    )
-    self._policy_to_sources_map = {
-      Policy.car_state_only: [SpeedLimitSource.car],
-      Policy.map_data_only: [SpeedLimitSource.map],
-      Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map],
-      Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car],
-      Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map],
-    }
-    self.source = SpeedLimitSource.none
-    for source in ALL_SOURCES:
-      self._reset_limit_sources(source)
-
-    self.is_metric = self.params.get_bool("IsMetric")
-    self.offset_type = get_sanitize_int_param(
-      "SpeedLimitOffsetType",
-      OffsetType.min().value,
-      OffsetType.max().value,
-      self.params
-    )
-    self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
-
-    self.speed_limit = 0.
-    self.speed_limit_last = 0.
-    self.speed_limit_final = 0.
-    self.speed_limit_final_last = 0.
-    self.speed_limit_offset = 0.
-
-  def update_speed_limit_states(self) -> None:
-    self.speed_limit_final = self.speed_limit + self.speed_limit_offset
-
-    if self.speed_limit > 0.:
-      self.speed_limit_last = self.speed_limit
-      self.speed_limit_final_last = self.speed_limit_final
-
-  @property
-  def speed_limit_valid(self) -> bool:
-    return self.speed_limit > 0.
-
-  @property
-  def speed_limit_last_valid(self) -> bool:
-    return self.speed_limit_last > 0.
-
-  def update_params(self):
-    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
-      self.is_metric = self.params.get_bool("IsMetric")
-      self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True)
-      self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
-
-  def _get_speed_limit_offset(self) -> float:
-    if self.offset_type == OffsetType.off:
-      return 0
-    elif self.offset_type == OffsetType.fixed:
-      return float(self.offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS))
-    elif self.offset_type == OffsetType.percentage:
-      return float(self.offset_value * 0.01 * self.speed_limit)
-    else:
-      raise NotImplementedError("Offset not supported")
-
-  def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimit.Source) -> None:
-    self.limit_solutions[source] = 0.
-    self.distance_solutions[source] = 0.
-
-  def _get_from_car_state(self, sm: messaging.SubMaster) -> None:
-    self._reset_limit_sources(SpeedLimitSource.car)
-    self.limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit
-    self.distance_solutions[SpeedLimitSource.car] = 0.
-
-  def _get_from_map_data(self, sm: messaging.SubMaster) -> None:
-    self._reset_limit_sources(SpeedLimitSource.map)
-    self._process_map_data(sm)
-
-  def _process_map_data(self, sm: messaging.SubMaster) -> None:
-    gps_data = sm[self._gps_location_service]
-    map_data = sm['liveMapDataSP']
-
-    gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3
-    if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
-      return
-
-    speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
-    next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
-
-    self._calculate_map_data_limits(sm, speed_limit, next_speed_limit)
-
-  def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None:
-    gps_data = sm[self._gps_location_service]
-    map_data = sm['liveMapDataSP']
-
-    distance_since_fix = self.v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3)
-    distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
-
-    self.limit_solutions[SpeedLimitSource.map] = speed_limit
-    self.distance_solutions[SpeedLimitSource.map] = 0.
-
-    # FIXME-SP: this is not working as expected
-    if 0. < next_speed_limit < self.v_ego:
-      adapt_time = (next_speed_limit - self.v_ego) / LIMIT_ADAPT_ACC
-      adapt_distance = self.v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2
-
-      if distance_to_speed_limit_ahead <= adapt_distance:
-        self.limit_solutions[SpeedLimitSource.map] = next_speed_limit
-        self.distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead
-
-  def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimit.Source:
-    sources_for_policy = self._policy_to_sources_map[self.policy]
-
-    if self.policy != Policy.combined:
-      # They are ordered in the order of preference, so we pick the first that's non-zero
-      for source in sources_for_policy:
-        if self.limit_solutions[source] > 0.:
-          return source
-      return SpeedLimitSource.none
-
-    sources_with_limits = [(s, limit) for s, limit in [(s, self.limit_solutions[s]) for s in sources_for_policy] if limit > 0.]
-    if sources_with_limits:
-      return min(sources_with_limits, key=lambda x: x[1])[0]
-
-    return SpeedLimitSource.none
-
-  def _resolve_limit_sources(self, sm: messaging.SubMaster) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimit.Source]:
-    """Get limit solutions from each data source"""
-    self._get_from_car_state(sm)
-    self._get_from_map_data(sm)
-
-    source = self._get_source_solution_according_to_policy()
-    speed_limit = self.limit_solutions[source] if source else 0.
-    distance = self.distance_solutions[source] if source else 0.
-
-    return speed_limit, distance, source
-
-  def update(self, v_ego: float, sm: messaging.SubMaster) -> None:
-    self.v_ego = v_ego
-    self.update_params()
-
-    self.speed_limit, self.distance, self.source = self._resolve_limit_sources(sm)
-    self.speed_limit_offset = self._get_speed_limit_offset()
-
-    self.update_speed_limit_states()
-
-    self.frame += 1

sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py

@@ -1,278 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-import pytest
-
-from cereal import custom
-from opendbc.car.car_helpers import interfaces
-from opendbc.car.rivian.values import CAR as RIVIAN
-from opendbc.car.tesla.values import CAR as TESLA
-from opendbc.car.toyota.values import CAR as TOYOTA
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \
-  PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES
-from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
-
-SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
-
-ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
-
-SPEED_LIMITS = {
-  'residential': 25 * CV.MPH_TO_MS,  # 25 mph
-  'city': 35 * CV.MPH_TO_MS,         # 35 mph
-  'highway': 65 * CV.MPH_TO_MS,      # 65 mph
-  'freeway': 80 * CV.MPH_TO_MS,      # 80 mph
-}
-
-DEFAULT_CAR = TOYOTA.TOYOTA_RAV4_TSS2
-
-
-@pytest.fixture
-def car_name(request):
-  return getattr(request, "param", DEFAULT_CAR)
-
-
-@pytest.fixture(autouse=True)
-def set_car_name_on_instance(request, car_name):
-  instance = getattr(request, "instance", None)
-  if instance:
-    instance.car_name = car_name
-
-
-class TestSpeedLimitAssist:
-
-  def setup_method(self, method):
-    self.params = Params()
-    self.reset_custom_params()
-    self.events_sp = EventsSP()
-    CI = self._setup_platform(self.car_name)
-    self.sla = SpeedLimitAssist(CI.CP, CI.CP_SP)
-    self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)
-    self.pcm_long_max_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric][1]  # use 80 MPH for now
-    self.speed_conv = CV.MS_TO_KPH if self.sla.is_metric else CV.MS_TO_MPH
-
-  def teardown_method(self, method):
-    self.reset_state()
-
-  def _setup_platform(self, car_name):
-    CarInterface = interfaces[car_name]
-    CP = CarInterface.get_non_essential_params(car_name)
-    CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
-    CI = CarInterface(CP, CP_SP)
-    CI.CP.openpilotLongitudinalControl = True  # always assume it's openpilot longitudinal
-    sunnypilot_interfaces.setup_interfaces(CI, self.params)
-    return CI
-
-  def reset_custom_params(self):
-    self.params.put("IsReleaseSpBranch", True)
-    self.params.put("SpeedLimitMode", int(Mode.assist))
-    self.params.put_bool("IsMetric", False)
-    self.params.put("SpeedLimitOffsetType", 0)
-    self.params.put("SpeedLimitValueOffset", 0)
-
-  def reset_state(self):
-    self.sla.state = SpeedLimitAssistState.disabled
-    self.sla.frame = -1
-    self.sla.last_op_engaged_frame = 0
-    self.sla.op_engaged = False
-    self.sla.op_engaged_prev = False
-    self.sla._speed_limit = 0.
-    self.sla.speed_limit_prev = 0.
-    self.sla.last_valid_speed_limit_offsetted = 0.
-    self.sla._distance = 0.
-    self.events_sp.clear()
-
-  def initialize_active_state(self, initialize_v_cruise):
-    self.sla.state = SpeedLimitAssistState.active
-    self.sla.v_cruise_cluster = initialize_v_cruise
-    self.sla.v_cruise_cluster_prev = initialize_v_cruise
-    self.sla.prev_v_cruise_cluster_conv = round(initialize_v_cruise * self.speed_conv)
-
-  def test_initial_state(self):
-    assert self.sla.state == SpeedLimitAssistState.disabled
-    assert not self.sla.is_enabled
-    assert not self.sla.is_active
-    assert V_CRUISE_UNSET == self.sla.get_v_target_from_control()
-
-  @pytest.mark.parametrize("car_name", [RIVIAN.RIVIAN_R1_GEN1, TESLA.TESLA_MODEL_Y], indirect=True)
-  def test_disallowed_brands(self, car_name):
-    """
-      Speed Limit Assist is disabled for the following brands and conditions:
-      - All Tesla and is a release branch;
-      - All Rivian
-    """
-    assert not self.sla.enabled
-
-    # stay disallowed even when the param may have changed from somewhere else
-    self.params.put("SpeedLimitMode", int(Mode.assist))
-    for _ in range(int(PARAMS_UPDATE_PERIOD / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'],
-                      SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert not self.sla.enabled
-
-  def test_disabled(self):
-    self.params.put("SpeedLimitMode", int(Mode.off))
-    for _ in range(int(10. / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.disabled
-
-  def test_transition_disabled_to_preactive(self):
-    for _ in range(int(3. / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.preActive
-    assert self.sla.is_enabled and not self.sla.is_active
-
-  def test_transition_disabled_to_pending_no_speed_limit_not_max_initial_set_speed(self):
-    for _ in range(int(3. / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['highway'], 0, SPEED_LIMITS['city'], 0, 0, False, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.pending
-    assert self.sla.is_enabled and not self.sla.is_active
-
-  def test_preactive_to_active_with_max_speed_confirmation(self):
-    self.sla.state = SpeedLimitAssistState.preActive
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
-                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.active
-    assert self.sla.is_enabled and self.sla.is_active
-    assert self.sla.output_v_target == SPEED_LIMITS['highway']
-
-  def test_preactive_timeout_to_inactive(self):
-    self.sla.state = SpeedLimitAssistState.preActive
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-
-    for _ in range(int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.inactive
-
-  def test_preactive_to_pending_no_speed_limit(self):
-    self.sla.state = SpeedLimitAssistState.preActive
-    self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed, 0, 0, False, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.pending
-    assert self.sla.is_enabled and not self.sla.is_active
-
-  def test_pending_to_active_when_speed_limit_available(self):
-    self.sla.state = SpeedLimitAssistState.pending
-    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
-    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
-
-    self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed,
-                    SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.active
-
-  def test_pending_to_adapting_when_below_speed_limit(self):
-    self.sla.state = SpeedLimitAssistState.pending
-    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
-    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
-
-    self.sla.update(True, False, SPEED_LIMITS['highway'] + 5, 0, self.pcm_long_max_set_speed,
-                    SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.adapting
-    assert self.sla.is_enabled and self.sla.is_active
-
-  def test_active_to_adapting_transition(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-
-    self.sla.update(True, False, SPEED_LIMITS['highway'] + 2, 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
-                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.adapting
-
-  def test_adapting_to_active_transition(self):
-    self.sla.state = SpeedLimitAssistState.adapting
-    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
-    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
-
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
-                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.active
-
-  def test_manual_cruise_change_detection(self):
-    self.sla.state = SpeedLimitAssistState.active
-    expected_cruise = SPEED_LIMITS['highway']
-    self.sla.v_cruise_cluster_prev = expected_cruise
-
-    different_cruise = SPEED_LIMITS['highway'] + 5
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.inactive
-
-  # TODO-SP: test lower CST cases
-  def test_rapid_speed_limit_changes(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-    speed_limits = [SPEED_LIMITS['highway'], SPEED_LIMITS['freeway']]
-
-    for _, speed_limit in enumerate(speed_limits):
-      self.sla.update(True, False, speed_limit, 0, self.pcm_long_max_set_speed, speed_limit, speed_limit, True, 0, self.events_sp)
-    assert self.sla.state in ACTIVE_STATES
-
-  def test_invalid_speed_limits_handling(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-
-    invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
-
-    for invalid_limit in invalid_limits:
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, invalid_limit, SPEED_LIMITS['city'], True, 0, self.events_sp)
-      assert isinstance(self.sla.output_v_target, (int, float))
-      assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0
-
-  def test_stale_data_handling(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-    old_speed_limit = SPEED_LIMITS['city']
-
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, 0, old_speed_limit, True, 0, self.events_sp)
-    assert self.sla.state in ACTIVE_STATES
-    assert self.sla.output_v_target == old_speed_limit
-
-  def test_distance_based_adapting(self):
-    self.sla.state = SpeedLimitAssistState.adapting
-    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
-    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
-
-    distance = 100.0
-    current_speed = SPEED_LIMITS['freeway']
-    target_speed = SPEED_LIMITS['highway']
-
-    self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, distance, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.adapting
-    assert self.sla.output_v_target == target_speed  # TODO-SP: assert expected accel, need to enable self.acceleration_solutions
-
-  def test_long_disengaged_to_disabled(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-
-    self.sla.update(False, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'],
-                    SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.disabled
-    assert self.sla.output_v_target == V_CRUISE_UNSET
-
-  def test_maintain_states_with_no_changes(self):
-    """Test that states are maintained when no significant changes occur"""
-    test_states = [
-      SpeedLimitAssistState.preActive,
-      SpeedLimitAssistState.pending,
-      SpeedLimitAssistState.active,
-      SpeedLimitAssistState.adapting
-    ]
-
-    for state in test_states:
-      self.sla.state = state
-      self.sla.op_engaged = True
-
-      initial_state = state
-
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-
-      assert self.sla.state in ALL_STATES  # Sanity check
-
-      if initial_state == SpeedLimitAssistState.preActive:
-        assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
-      elif initial_state in ACTIVE_STATES:
-        assert self.sla.state in ACTIVE_STATES

sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py

@@ -1,144 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import random
-import time
-
-import pytest
-from pytest_mock import MockerFixture
-
-from cereal import custom
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE
-
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy
-
-SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
-
-
-def create_mock(properties, mocker: MockerFixture):
-  mock = mocker.MagicMock()
-  for _property, value in properties.items():
-    setattr(mock, _property, value)
-  return mock
-
-
-def setup_sm_mock(mocker: MockerFixture):
-  cruise_speed_limit = random.uniform(0, 120)
-  live_map_data_limit = random.uniform(0, 120)
-
-  car_state = create_mock({
-    'gasPressed': False,
-    'brakePressed': False,
-    'standstill': False,
-  }, mocker)
-  car_state_sp = create_mock({
-    'speedLimit': cruise_speed_limit,
-  }, mocker)
-  live_map_data = create_mock({
-    'speedLimit': live_map_data_limit,
-    'speedLimitValid': True,
-    'speedLimitAhead': 0.,
-    'speedLimitAheadValid': 0.,
-    'speedLimitAheadDistance': 0.,
-  }, mocker)
-  gps_data = create_mock({
-    'unixTimestampMillis': time.monotonic() * 1e3,
-  }, mocker)
-  sm_mock = mocker.MagicMock()
-  sm_mock.__getitem__.side_effect = lambda key: {
-    'carState': car_state,
-    'liveMapDataSP': live_map_data,
-    'carStateSP': car_state_sp,
-    'gpsLocation': gps_data,
-  }[key]
-  return sm_mock
-
-
-parametrized_policies = pytest.mark.parametrize(
-  "policy, sm_key, function_key", [
-    (Policy.car_state_only, 'carStateSP', SpeedLimitSource.car),
-    (Policy.car_state_priority, 'carStateSP', SpeedLimitSource.car),
-    (Policy.map_data_only, 'liveMapDataSP', SpeedLimitSource.map),
-    (Policy.map_data_priority, 'liveMapDataSP', SpeedLimitSource.map),
-  ],
-  ids=lambda val: val.name if hasattr(val, 'name') else str(val)
-)
-
-
-@pytest.mark.parametrize("resolver_class", [SpeedLimitResolver])
-class TestSpeedLimitResolverValidation:
-
-  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
-  def test_initial_state(self, resolver_class, policy):
-    resolver = resolver_class()
-    resolver.policy = policy
-    for source in ALL_SOURCES:
-      if source in resolver.limit_solutions:
-        assert resolver.limit_solutions[source] == 0.
-        assert resolver.distance_solutions[source] == 0.
-
-  @parametrized_policies
-  def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
-    resolver = resolver_class()
-    resolver.policy = policy
-    sm_mock = setup_sm_mock(mocker)
-    source_speed_limit = sm_mock[sm_key].speedLimit
-
-    # Assert the resolver
-    resolver.update(source_speed_limit, sm_mock)
-    assert resolver.speed_limit == source_speed_limit
-    assert resolver.source == ALL_SOURCES[function_key]
-
-  def test_resolver_combined(self, resolver_class, mocker: MockerFixture):
-    resolver = resolver_class()
-    resolver.policy = Policy.combined
-    sm_mock = setup_sm_mock(mocker)
-    socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map}
-    minimum_key, minimum_speed_limit = min(
-      ((key, sm_mock[key].speedLimit) for key in
-       socket_to_source.keys()), key=lambda x: x[1])
-
-    # Assert the resolver
-    resolver.update(minimum_speed_limit, sm_mock)
-    assert resolver.speed_limit == minimum_speed_limit
-    assert resolver.source == socket_to_source[minimum_key]
-
-  @parametrized_policies
-  def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
-    resolver = resolver_class()
-    resolver.policy = policy
-    sm_mock = setup_sm_mock(mocker)
-    source_speed_limit = sm_mock[sm_key].speedLimit
-
-    # Assert the parsing
-    resolver.update(source_speed_limit, sm_mock)
-    assert resolver.limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit
-    assert resolver.distance_solutions[ALL_SOURCES[function_key]] == 0.
-
-  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
-  def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture):
-    v_ego = 50
-    resolver = resolver_class()
-    resolver.policy = policy
-
-    sm_mock = setup_sm_mock(mocker)
-    resolver.update(v_ego, sm_mock)
-
-    # After resolution
-    assert resolver.speed_limit is not None
-    assert resolver.distance is not None
-    assert resolver.source is not None
-
-  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
-  def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture):
-    resolver = resolver_class()
-    resolver.policy = policy
-    sm_mock = mocker.MagicMock()
-    sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
-    resolver._get_from_map_data(sm_mock)
-    assert resolver.limit_solutions[SpeedLimitSource.map] == 0.
-    assert resolver.distance_solutions[SpeedLimitSource.map] == 0.

sunnypilot/selfdrive/selfdrived/events.py

@@ -1,9 +1,7 @@
-import cereal.messaging as messaging
 from cereal import log, car, custom
-from openpilot.common.constants import CV
+
 from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
   NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
 
 
 AlertSize = log.SelfdriveState.AlertSize
@@ -18,43 +16,6 @@ EventNameSP = custom.OnroadEventSP.EventName
 EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()}
 
 
-def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
-  speedLimit = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimit
-  speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
-  message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit'
-  return Alert(
-    message,
-    "",
-    AlertStatus.normal, AlertSize.small,
-    Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.)
-
-
-def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
-  speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
-  speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast
-  speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv)
-  alert_1_str = ""
-  alert_2_str = ""
-  alert_size = AlertSize.none
-
-  if CP.openpilotLongitudinalControl and CP.pcmCruise:
-    # PCM long
-    cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[metric]
-    pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high
-    pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
-    speed_unit = "km/h" if metric else "mph"
-
-    alert_1_str = "Speed Limit Assist: Activation Required"
-    alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate"
-    alert_size = AlertSize.mid
-
-  return Alert(
-    alert_1_str,
-    alert_2_str,
-    AlertStatus.normal, alert_size,
-    Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleLow, .1)
-
-
 class EventsSP(EventsBase):
   def __init__(self):
     super().__init__()
@@ -191,34 +152,6 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
   },
 
-  EventNameSP.speedLimitActive: {
-    ET.WARNING: Alert(
-      "Automatically adjusting to the posted speed limit",
-      "",
-      AlertStatus.normal, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
-  },
-
-  EventNameSP.speedLimitChanged: {
-    ET.WARNING: Alert(
-      "Set speed changed",
-      "",
-      AlertStatus.normal, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
-  },
-
-  EventNameSP.speedLimitPreActive: {
-    ET.WARNING: speed_limit_pre_active_alert,
-  },
-
-  EventNameSP.speedLimitPending: {
-    ET.WARNING: Alert(
-      "Automatically adjusting to the last speed limit",
-      "",
-      AlertStatus.normal, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
-  },
-
   EventNameSP.e2eChime: {
     ET.PERMANENT: Alert(
       "",

third_party/mapd_pfeiferj/README.md

@@ -1,2 +0,0 @@
-# MAPD implementation by pfeiferj
-https://github.com/pfeiferj/openpilot-mapd/releases/