Merge PR #90: Rivian Angle Support

Original PR by TonyJOM (Anthony Orta).

Co-authored-by: TonyJOM <anthonyorta20@icloud.com>

selfdrive/ui/mici/onroad/hud_renderer.py

@@ -4,6 +4,7 @@ import pyray as rl
 from dataclasses import dataclass
 from openpilot.common.constants import CV
 from openpilot.selfdrive.ui.onroad.starpilot.torque_bar import TorqueBar
+from openpilot.selfdrive.ui.onroad.starpilot.rivian_lateral_mode import rivian_lateral_mode
 from openpilot.selfdrive.ui.mici.onroad.speed_limit_utils import resolve_display_speed_limit_ms
 from openpilot.selfdrive.ui.onroad.starpilot.navigation_card import NavigationCardRenderer
 from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
@@ -162,6 +163,7 @@ class HudRenderer(Widget):
 
     self._wheel_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps)
     self._wheel_y_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps)
+    self._wheel_tint: rl.Color | None = None
 
     self._set_speed_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
     self._egpu_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
@@ -185,10 +187,13 @@ class HudRenderer(Widget):
       self.is_cruise_set = False
       self.set_speed = SET_SPEED_NA
       self.speed = 0.0
+      self._wheel_tint = None
       return
 
     controls_state = sm['controlsState']
     car_state = sm['carState']
+    rivian_lateral_mode.update()
+    self._wheel_tint = rivian_lateral_mode.wheel_tint
 
     v_cruise_cluster = car_state.vCruiseCluster
     set_speed = (
@@ -380,7 +385,8 @@ class HudRenderer(Widget):
     origin = (wheel_txt.width / 2, wheel_txt.height / 2)
 
     # color and draw
-    color = rl.Color(255, 255, 255, int(self._wheel_alpha_filter.x))
+    base_color = self._wheel_tint if self._wheel_tint is not None and not self._show_wheel_critical else rl.Color(255, 255, 255, 255)
+    color = rl.Color(base_color.r, base_color.g, base_color.b, int(self._wheel_alpha_filter.x))
     rl.draw_texture_pro(wheel_txt, src_rect, dest_rect, origin, rotation, color)
 
     if self._show_wheel_critical:

selfdrive/ui/onroad/exp_button.py

@@ -25,6 +25,7 @@ class ExpButton(Widget):
 
     self._white_color: rl.Color = rl.Color(255, 255, 255, 255)
     self._black_bg: rl.Color = rl.Color(0, 0, 0, 166)
+    self.wheel_tint: rl.Color | None = None
     self._txt_wheel: rl.Texture = gui_app.texture('icons/chffr_wheel.png', icon_size, icon_size)
     self._txt_exp: rl.Texture = gui_app.texture('icons/experimental.png', icon_size, icon_size)
     self._rect = rl.Rectangle(0, 0, button_size, button_size)
@@ -97,17 +98,31 @@ class ExpButton(Widget):
 
     self._white_color.a = 180 if self.is_pressed or not self._engageable else 255
 
-    texture = self._txt_exp if self._held_or_actual_mode() else self._txt_wheel
+    exp_mode = self._held_or_actual_mode()
+    texture = self._txt_exp if exp_mode else self._txt_wheel
+    color = self._white_color
+    tint = None
+    if self.wheel_tint is not None:
+      tint = rl.Color(self.wheel_tint.r, self.wheel_tint.g, self.wheel_tint.b, self._white_color.a)
+
     rl.draw_circle(center_x, center_y, self._rect.width / 2, self._bg_color)
+    if tint is not None:
+      if exp_mode:
+        # The experimental icon is already colored, so show the lateral mode
+        # around it instead of obscuring the icon with a texture tint.
+        radius = self._rect.width / 2
+        rl.draw_ring(rl.Vector2(center_x, center_y), radius - 8, radius, 0, 360, 0, tint)
+      else:
+        color = tint
 
     rotating_wheel = ui_state.starpilot_toggles.get("rotating_wheel", False) or self._params.get_bool("RotatingWheel")
     if texture == self._txt_wheel and rotating_wheel:
       source_rect = rl.Rectangle(0, 0, texture.width, texture.height)
       dest_rect = rl.Rectangle(center_x, center_y, texture.width, texture.height)
       origin = rl.Vector2(texture.width / 2, texture.height / 2)
-      rl.draw_texture_pro(texture, source_rect, dest_rect, origin, -self._steer_angle_filter.x, self._white_color)
+      rl.draw_texture_pro(texture, source_rect, dest_rect, origin, -self._steer_angle_filter.x, color)
     else:
-      rl.draw_texture_ex(texture, rl.Vector2(center_x - texture.width / 2, center_y - texture.height / 2), 0.0, 1.0, self._white_color)
+      rl.draw_texture_ex(texture, rl.Vector2(center_x - texture.width / 2, center_y - texture.height / 2), 0.0, 1.0, color)
 
   def _held_or_actual_mode(self):
     now = time.monotonic()

selfdrive/ui/onroad/starpilot/rivian_lateral_mode.py

@@ -0,0 +1,55 @@
+import pyray as rl
+
+from cereal import car
+from openpilot.selfdrive.ui.ui_state import ui_state
+
+ANGLE_COLOR = rl.Color(0x3A, 0xDB, 0x6D, 255)
+TORQUE_COLOR = rl.Color(0x4D, 0x9D, 0xFF, 255)
+DRIVER_OVERRIDE_COLOR = rl.Color(255, 255, 255, 255)
+LateralControlMode = car.CarControl.Actuators.LateralControlMode
+
+
+class RivianLateralMode:
+  """Display Rivian's active lateral channel and driver steering input."""
+
+  def __init__(self):
+    self.mode: str | None = None
+    self.driver_override = False
+    self._frame = -1
+
+  def update(self) -> None:
+    sm = ui_state.sm
+    if sm.frame == self._frame:
+      return
+    self._frame = sm.frame
+
+    CP = ui_state.CP
+    rivian = CP is not None and CP.brand == "rivian"
+    car_state_received = sm.recv_frame["carState"] >= ui_state.started_frame
+    car_control_received = sm.recv_frame["carControl"] >= ui_state.started_frame
+    if not rivian or not car_state_received or not car_control_received or not sm["carControl"].latActive:
+      self.mode = None
+      self.driver_override = False
+      return
+
+    self.driver_override = sm["carState"].steeringPressed
+    lateral_mode = sm["carOutput"].actuatorsOutput.lateralControlMode
+    if lateral_mode == LateralControlMode.angle:
+      self.mode = "angle"
+    elif lateral_mode in (LateralControlMode.torque, LateralControlMode.torqueRecovering):
+      self.mode = "torque"
+    else:
+      self.mode = None
+
+  @property
+  def wheel_tint(self) -> "rl.Color | None":
+    if self.driver_override:
+      return DRIVER_OVERRIDE_COLOR
+    if self.mode == "angle":
+      return ANGLE_COLOR
+    if self.mode == "torque":
+      return TORQUE_COLOR
+    return None
+
+
+rivian_lateral_mode = RivianLateralMode()

selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py

@@ -5,6 +5,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.starpilot_border import render_behi
 from openpilot.selfdrive.ui.onroad.starpilot.path import render_adjacent_lanes, render_path_edges
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.selfdrive.ui.onroad.starpilot.torque_bar import TorqueBar
+from openpilot.selfdrive.ui.onroad.starpilot.rivian_lateral_mode import rivian_lateral_mode
 from openpilot.selfdrive.ui.onroad.starpilot.widget_layout_manager import WidgetLayoutManager
 from openpilot.selfdrive.ui.onroad.starpilot.widgets import (
   SetSpeedWidget, SpeedLimitWidget, PedalIconsWidget,
@@ -76,6 +77,10 @@ class StarPilotOnroadView(AugmentedRoadView):
     self._child(self._driver_monitor_widget)
     self._child(self._stopped_timer_widget)
 
+  def _update_state(self) -> None:
+    rivian_lateral_mode.update()
+    self._hud_renderer._exp_button.wheel_tint = rivian_lateral_mode.wheel_tint
+
   def _render(self, rect: rl.Rectangle):
     border_width = self._get_border_width()
     border_color = get_screen_edge_color(ui_state)

selfdrive/ui/onroad/starpilot/torque_bar.py

@@ -7,6 +7,7 @@ import numpy as np
 import pyray as rl
 from opendbc.car import ACCELERATION_DUE_TO_GRAVITY
 from openpilot.selfdrive.ui.lib.starpilot_visuals import blend_colors
+from openpilot.selfdrive.ui.onroad.starpilot.rivian_lateral_mode import rivian_lateral_mode
 from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
 from openpilot.system.ui.lib.application import gui_app
 from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
@@ -162,8 +163,13 @@ class TorqueBar(Widget):
     if self._demo:
       return
 
-    # torque line
-    if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState':
+    rivian_lateral_mode.update()
+    # Angle-controlled cars, including Rivian's hybrid controller while its
+    # angle channel is active, use a lateral-acceleration estimate for the bar.
+    # The shared Rivian mode detector keys off the actual CAN torque so the bar
+    # follows live angle/torque handoffs rather than the controller request.
+    if (ui_state.sm['controlsState'].lateralControlState.which() == 'angleState' or
+        rivian_lateral_mode.mode == "angle"):
       controls_state = ui_state.sm['controlsState']
       car_state = ui_state.sm['carState']
       live_parameters = ui_state.sm['liveParameters']

selfdrive/ui/tests/test_rivian_lateral_mode.py

@@ -0,0 +1,295 @@
+import importlib.util
+from enum import IntFlag
+from pathlib import Path
+import sys
+from types import ModuleType, SimpleNamespace
+
+import pytest
+
+from cereal import car
+
+
+MODULE_PATH = Path(__file__).resolve().parents[1] / "onroad" / "starpilot" / "rivian_lateral_mode.py"
+EXP_BUTTON_PATH = Path(__file__).resolve().parents[1] / "onroad" / "exp_button.py"
+
+
+LateralControlMode = car.CarControl.Actuators.LateralControlMode
+
+
+class FakeColor:
+  def __init__(self, r, g, b, a):
+    self.r = r
+    self.g = g
+    self.b = b
+    self.a = a
+
+
+class FakeRectangle:
+  def __init__(self, x, y, width, height):
+    self.x = x
+    self.y = y
+    self.width = width
+    self.height = height
+
+
+class FakeTexture:
+  def __init__(self, name, width, height):
+    self.name = name
+    self.width = width
+    self.height = height
+
+
+class FakeSubMaster(dict):
+  def __init__(self, *, lateral_mode, steering_pressed=False, lat_active=True):
+    super().__init__({
+      "carControl": SimpleNamespace(latActive=lat_active),
+      "carState": SimpleNamespace(steeringPressed=steering_pressed),
+      "carOutput": SimpleNamespace(
+        actuatorsOutput=SimpleNamespace(lateralControlMode=lateral_mode),
+      ),
+    })
+    self.frame = 1
+    self.recv_frame = {"carControl": 1, "carState": 1}
+
+
+def load_lateral_mode(monkeypatch, *, brand="rivian", angle_harness=True, longitudinal_harness=False,
+                      steering_pressed=False, lat_active=True, lateral_mode=LateralControlMode.inactive):
+  class RivianFlags(IntFlag):
+    ANGLE_HARNESS = 1
+    LONGITUDINAL_HARNESS = 2
+
+  fake_pyray = ModuleType("pyray")
+  fake_pyray.Color = lambda *args: args
+  monkeypatch.setitem(sys.modules, "pyray", fake_pyray)
+
+  values_module = ModuleType("opendbc.car.rivian.values")
+  values_module.RivianFlags = RivianFlags
+  monkeypatch.setitem(sys.modules, "opendbc.car.rivian.values", values_module)
+
+  flags = RivianFlags(0)
+  if angle_harness:
+    flags |= RivianFlags.ANGLE_HARNESS
+  if longitudinal_harness:
+    flags |= RivianFlags.LONGITUDINAL_HARNESS
+
+  ui_state = SimpleNamespace(
+    CP=SimpleNamespace(brand=brand, flags=flags),
+    sm=FakeSubMaster(lateral_mode=lateral_mode, steering_pressed=steering_pressed, lat_active=lat_active),
+    started_frame=0,
+  )
+  ui_state_module = ModuleType("openpilot.selfdrive.ui.ui_state")
+  ui_state_module.ui_state = ui_state
+  monkeypatch.setitem(sys.modules, "openpilot.selfdrive.ui.ui_state", ui_state_module)
+
+  spec = importlib.util.spec_from_file_location("rivian_lateral_mode_under_test", MODULE_PATH)
+  assert spec is not None and spec.loader is not None
+  module = importlib.util.module_from_spec(spec)
+  spec.loader.exec_module(module)
+  return module
+
+
+def load_exp_button(monkeypatch):
+  draws = {"textures": [], "rings": []}
+  fake_pyray = ModuleType("pyray")
+  fake_pyray.Color = FakeColor
+  fake_pyray.Rectangle = FakeRectangle
+  fake_pyray.Texture = FakeTexture
+  fake_pyray.Vector2 = lambda x, y: SimpleNamespace(x=x, y=y)
+  fake_pyray.draw_circle = lambda *args: None
+  fake_pyray.draw_ring = lambda *args: draws["rings"].append(args)
+  fake_pyray.draw_texture_ex = lambda *args: draws["textures"].append(args)
+  fake_pyray.draw_texture_pro = lambda *args: draws["textures"].append(args)
+  monkeypatch.setitem(sys.modules, "pyray", fake_pyray)
+
+  params_module = ModuleType("openpilot.common.params")
+  params_module.Params = type("Params", (), {"get_bool": lambda self, *args, **kwargs: False})
+  monkeypatch.setitem(sys.modules, "openpilot.common.params", params_module)
+
+  fake_ui_state = SimpleNamespace(
+    ui_params=SimpleNamespace(get_bool=lambda *args, **kwargs: False),
+    sm={
+      "selfdriveState": SimpleNamespace(experimentalMode=False, engageable=True, enabled=False),
+      "carState": SimpleNamespace(steeringAngleDeg=0.0),
+    },
+    starpilot_toggles={},
+    always_on_lateral_active=False,
+    conditional_status=0,
+    switchback_mode_enabled=False,
+    traffic_mode_enabled=False,
+    params_memory=SimpleNamespace(),
+    has_longitudinal_control=False,
+  )
+  ui_state_module = ModuleType("openpilot.selfdrive.ui.ui_state")
+  ui_state_module.ui_state = fake_ui_state
+  monkeypatch.setitem(sys.modules, "openpilot.selfdrive.ui.ui_state", ui_state_module)
+
+  class FakeGuiApp:
+    target_fps = 60
+
+    @staticmethod
+    def texture(name, width, height):
+      return FakeTexture(name, width, height)
+
+  application_module = ModuleType("openpilot.system.ui.lib.application")
+  application_module.gui_app = FakeGuiApp()
+  monkeypatch.setitem(sys.modules, "openpilot.system.ui.lib.application", application_module)
+
+  class FakeWidget:
+    def __init__(self):
+      self.is_pressed = False
+
+    def set_visible(self, visible):
+      self._visible = visible
+
+    def _handle_mouse_release(self, _):
+      pass
+
+  widgets_module = ModuleType("openpilot.system.ui.widgets")
+  widgets_module.Widget = FakeWidget
+  monkeypatch.setitem(sys.modules, "openpilot.system.ui.widgets", widgets_module)
+
+  class FakeFilter:
+    def __init__(self, x, *args):
+      self.x = x
+
+    def update(self, x):
+      self.x = x
+      return x
+
+  filter_module = ModuleType("openpilot.common.filter_simple")
+  filter_module.FirstOrderFilter = FakeFilter
+  monkeypatch.setitem(sys.modules, "openpilot.common.filter_simple", filter_module)
+
+  experimental_module = ModuleType("openpilot.starpilot.common.experimental_state")
+  experimental_module.CEStatus = {"OFF": 0}
+  experimental_module.next_manual_ce_status = lambda *args: 0
+  experimental_module.sync_manual_ce_state = lambda *args: None
+  monkeypatch.setitem(sys.modules, "openpilot.starpilot.common.experimental_state", experimental_module)
+
+  spec = importlib.util.spec_from_file_location("exp_button_under_test", EXP_BUTTON_PATH)
+  assert spec is not None and spec.loader is not None
+  module = importlib.util.module_from_spec(spec)
+  spec.loader.exec_module(module)
+  return module, draws
+
+
+def test_angle_mode_uses_controller_report(monkeypatch):
+  module = load_lateral_mode(monkeypatch, lateral_mode=LateralControlMode.angle)
+  mode = module.RivianLateralMode()
+
+  mode.update()
+
+  assert mode.mode == "angle"
+  assert mode.wheel_tint == module.ANGLE_COLOR
+
+
+def test_zero_torque_at_standstill_stays_in_reported_torque_mode(monkeypatch):
+  module = load_lateral_mode(monkeypatch, lateral_mode=LateralControlMode.torque)
+  mode = module.RivianLateralMode()
+
+  mode.update()
+
+  assert mode.mode == "torque"
+  assert mode.wheel_tint == module.TORQUE_COLOR
+
+
+def test_torque_recovery_stays_blue(monkeypatch):
+  module = load_lateral_mode(monkeypatch, lateral_mode=LateralControlMode.torqueRecovering)
+  mode = module.RivianLateralMode()
+
+  mode.update()
+
+  assert mode.mode == "torque"
+  assert mode.wheel_tint == module.TORQUE_COLOR
+
+
+def test_basic_harness_rivian_uses_torque_color(monkeypatch):
+  module = load_lateral_mode(monkeypatch, angle_harness=False, lateral_mode=LateralControlMode.torque)
+  mode = module.RivianLateralMode()
+
+  mode.update()
+
+  assert mode.mode == "torque"
+  assert mode.wheel_tint == module.TORQUE_COLOR
+
+
+def test_longitudinal_harness_rivian_uses_torque_color(monkeypatch):
+  module = load_lateral_mode(monkeypatch, angle_harness=False, longitudinal_harness=True,
+                             lateral_mode=LateralControlMode.torque)
+  mode = module.RivianLateralMode()
+
+  mode.update()
+
+  assert mode.mode == "torque"
+  assert mode.wheel_tint == module.TORQUE_COLOR
+
+
+@pytest.mark.parametrize(("angle_harness", "longitudinal_harness", "lateral_mode"), [
+  (False, False, LateralControlMode.torque),
+  (False, True, LateralControlMode.torque),
+  (True, True, LateralControlMode.angle),
+  (True, True, LateralControlMode.torque),
+  (True, True, LateralControlMode.torqueRecovering),
+])
+def test_driver_steering_is_white_in_every_configuration(monkeypatch, angle_harness, longitudinal_harness, lateral_mode):
+  module = load_lateral_mode(monkeypatch, angle_harness=angle_harness, longitudinal_harness=longitudinal_harness,
+                             steering_pressed=True, lateral_mode=lateral_mode)
+  mode = module.RivianLateralMode()
+
+  mode.update()
+
+  expected_mode = "angle" if lateral_mode == LateralControlMode.angle else "torque"
+  assert mode.mode == expected_mode
+  assert mode.driver_override
+  assert mode.wheel_tint == module.DRIVER_OVERRIDE_COLOR
+
+
+def test_releasing_wheel_restores_active_mode_color(monkeypatch):
+  module = load_lateral_mode(monkeypatch, steering_pressed=True, lateral_mode=LateralControlMode.angle)
+  mode = module.RivianLateralMode()
+  mode.update()
+  assert mode.wheel_tint == module.DRIVER_OVERRIDE_COLOR
+
+  module.ui_state.sm["carState"].steeringPressed = False
+  module.ui_state.sm.frame += 1
+  mode.update()
+
+  assert not mode.driver_override
+  assert mode.wheel_tint == module.ANGLE_COLOR
+
+
+def test_non_rivian_is_not_classified(monkeypatch):
+  module = load_lateral_mode(monkeypatch, brand="toyota", steering_pressed=True,
+                             lateral_mode=LateralControlMode.torque)
+  mode = module.RivianLateralMode()
+
+  mode.update()
+
+  assert mode.mode is None
+  assert not mode.driver_override
+  assert mode.wheel_tint is None
+
+
+def test_inactive_lateral_is_not_classified(monkeypatch):
+  module = load_lateral_mode(monkeypatch, steering_pressed=True, lat_active=False,
+                             lateral_mode=LateralControlMode.torque)
+  mode = module.RivianLateralMode()
+
+  mode.update()
+
+  assert mode.mode is None
+  assert not mode.driver_override
+  assert mode.wheel_tint is None
+
+
+def test_non_mici_wheel_icon_uses_rivian_tint(monkeypatch):
+  module, draws = load_exp_button(monkeypatch)
+  button = module.ExpButton(192, 144)
+  button.wheel_tint = FakeColor(0x4D, 0x9D, 0xFF, 255)
+  button._update_state()
+
+  button._render(FakeRectangle(0, 0, 192, 192))
+
+  assert len(draws["textures"]) == 1
+  texture_color = draws["textures"][0][-1]
+  assert (texture_color.r, texture_color.g, texture_color.b, texture_color.a) == (0x4D, 0x9D, 0xFF, 255)