Merge PR #90: Rivian Angle Support

Original PR by TonyJOM (Anthony Orta).

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

selfdrive/car/car_specific.py

@@ -4,6 +4,7 @@ from opendbc.car import DT_CTRL, structs
 from opendbc.car.chrysler.values import RAM_DT
 from opendbc.car.gm.values import CAR as GM_CAR, GMFlags, SDGM_CAR
 from opendbc.car.interfaces import MAX_CTRL_SPEED
+from opendbc.car.rivian.values import RivianFlags
 
 from openpilot.selfdrive.selfdrived.events import Events
 
@@ -65,6 +66,21 @@ class CarSpecificEvents:
     self.gm_low_speed_alert_shown = False
     self.no_steer_warning = False
     self.silent_steer_warning = True
+    self.rivian = self.CP.brand == "rivian"
+    self.rivian_angle_harness = self.rivian and bool(self.CP.flags & RivianFlags.ANGLE_HARNESS)
+    self.rivian_status_frame = 0
+    self.rivian_angle_saturated = False
+    self.rivian_toi_recovery_failed = False
+    self.rivian_status_params = None
+    self.rivian_angle_params = None
+    if self.rivian:
+      try:
+        from openpilot.common.params import Params
+        self.rivian_status_params = Params(memory=True)
+        if self.rivian_angle_harness:
+          self.rivian_angle_params = self.rivian_status_params
+      except Exception:
+        pass
 
   def update(self, CS: car.CarState, CS_prev: car.CarState, CC: car.CarControl):
     extra_gears = BRAND_EXTRA_GEARS.get(self.CP.brand, None)
@@ -186,6 +202,18 @@ class CarSpecificEvents:
     else:
       events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears)
 
+    if self.rivian:
+      self.rivian_status_frame += 1
+      if self.rivian_status_params is not None and self.rivian_status_frame % 5 == 0:
+        self.rivian_toi_recovery_failed = self.rivian_status_params.get_bool("RivianToiRecoveryFailed")
+        if self.rivian_angle_harness:
+          self.rivian_angle_saturated = self.rivian_status_params.get_bool("RivianAngleSaturated")
+      if self.rivian_toi_recovery_failed:
+        events.add(EventName.steerTempUnavailable)
+    if self.rivian_angle_harness:
+      if self.rivian_angle_saturated:
+        events.add(EventName.steerSaturated)
+
     return events
 
   def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears: list | None = None, pcm_enable=True,

selfdrive/car/card.py

@@ -226,7 +226,10 @@ class Car:
 
     self.starpilot_card = StarPilotCard(self.CP, self.FPCP)
 
-    self.sm = self.sm.extend(['starpilotOnroadEvents', 'starpilotPlan', 'starpilotSelfdriveState', 'liveCalibration', 'selfdriveState'])
+    starpilot_services = ['starpilotOnroadEvents', 'starpilotPlan', 'starpilotSelfdriveState', 'liveCalibration', 'selfdriveState']
+    if self.CP.brand == "rivian":
+      starpilot_services.append('liveParameters')
+    self.sm = self.sm.extend(starpilot_services)
     self.pm = self.pm.extend(['starpilotCarState'])
 
   def _inject_favorite_virtual_cruise_events(self, CS: car.CarState) -> None:
@@ -416,6 +419,10 @@ class Car:
       now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
       self._update_redneck_cruise(CS, CC)
       self._update_openpilot_lead_state(CC)
+      if self.CP.brand == "rivian" and self.sm.all_checks(['liveParameters']) and hasattr(self.CI.CC, 'update_live_params'):
+        live_params = self.sm['liveParameters']
+        self.CI.CC.update_live_params(live_params.roll, live_params.angleOffsetDeg,
+                                      live_params.stiffnessFactor, live_params.steerRatio)
       self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos, self.starpilot_toggles)
       self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
 

selfdrive/car/tests/test_rivian_angle_saturation.py

@@ -0,0 +1,68 @@
+import importlib.util
+from pathlib import Path
+import sys
+from types import ModuleType, SimpleNamespace
+
+
+MODULE_PATH = Path(__file__).resolve().parents[1] / "car_specific.py"
+
+
+class FakeEvents:
+  def __init__(self):
+    self.names = []
+
+  def add(self, event):
+    self.names.append(event)
+
+
+def load_car_specific(monkeypatch):
+  messaging = ModuleType("cereal.messaging")
+  messaging.SubMaster = object
+  monkeypatch.setitem(sys.modules, "cereal.messaging", messaging)
+
+  events = ModuleType("openpilot.selfdrive.selfdrived.events")
+  events.Events = FakeEvents
+  monkeypatch.setitem(sys.modules, "openpilot.selfdrive.selfdrived.events", events)
+
+  spec = importlib.util.spec_from_file_location("rivian_car_specific_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 test_angle_saturation_raises_stock_take_control_event(monkeypatch):
+  module = load_car_specific(monkeypatch)
+  handler = module.CarSpecificEvents(SimpleNamespace(brand="rivian", flags=1))
+  handler.rivian_status_params = SimpleNamespace(get_bool=lambda key: key == "RivianAngleSaturated")
+  handler.rivian_angle_params = handler.rivian_status_params
+  handler.create_common_events = lambda *args, **kwargs: FakeEvents()
+
+  events = None
+  for _ in range(5):
+    events = handler.update(SimpleNamespace(), SimpleNamespace(), SimpleNamespace())
+
+  assert module.EventName.steerSaturated in events.names
+
+
+def test_saturation_bridge_is_inert_without_angle_harness(monkeypatch):
+  module = load_car_specific(monkeypatch)
+  handler = module.CarSpecificEvents(SimpleNamespace(brand="rivian", flags=0))
+  handler.create_common_events = lambda *args, **kwargs: FakeEvents()
+
+  events = handler.update(SimpleNamespace(), SimpleNamespace(), SimpleNamespace())
+
+  assert module.EventName.steerSaturated not in events.names
+
+
+def test_toi_recovery_timeout_raises_temporary_steering_event(monkeypatch):
+  module = load_car_specific(monkeypatch)
+  handler = module.CarSpecificEvents(SimpleNamespace(brand="rivian", flags=0))
+  handler.rivian_status_params = SimpleNamespace(get_bool=lambda key: key == "RivianToiRecoveryFailed")
+  handler.create_common_events = lambda *args, **kwargs: FakeEvents()
+
+  events = None
+  for _ in range(5):
+    events = handler.update(SimpleNamespace(), SimpleNamespace(), SimpleNamespace())
+
+  assert module.EventName.steerTempUnavailable in events.names

selfdrive/controls/controlsd.py

@@ -26,7 +26,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
 )
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.car.cruise_state import should_cancel_stock_cruise
-from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
+from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS, get_car_lateral_smooth_seconds
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
 from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles
@@ -35,6 +35,7 @@ from openpilot.starpilot.controls.lib.neural_network_feedforward import LatContr
 State = log.SelfdriveState.OpenpilotState
 LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
+LateralControlMode = car.CarControl.Actuators.LateralControlMode
 
 ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
 
@@ -216,6 +217,19 @@ def get_plan_reach(model_v2) -> float:
   return xs[-1] if len(xs) else 0.0
 
 
+def get_control_lateral_smooth_seconds(brand: str, v_ego: float, vehicle_smooth_seconds: float) -> float:
+  if brand != "rivian":
+    return LAT_SMOOTH_SECONDS
+  return get_car_lateral_smooth_seconds(brand, v_ego, vehicle_smooth_seconds)
+
+
+def turn_lead_allowed(brand: str, lateral_control_mode: car.CarControl.Actuators.LateralControlMode) -> bool:
+  # Torque steering mechanically damps the turn-lead fade. A direct angle
+  # controller follows the resulting lead/catch-up cycle literally, which can
+  # reverse the wheel command several times during one turn initiation.
+  return brand != "rivian" or lateral_control_mode != LateralControlMode.angle
+
+
 # Turn-initiation lead. The model's action and the fixed 4/7 m probes are anchored in
 # METERS, so the seconds of warning they give shrinks with speed — at 12 mph a corner
 # enters the 7 m window only ~1.3 s out, too late to wind the wheel, which is why every
@@ -577,7 +591,9 @@ class Controls:
     # bend is not a turn. The model-oppose veto is defense-in-depth for the fade-in
     # edge: a model actively steering against the blinker is correcting something the
     # lead must not fight (see the constants comment for the 2026-07-19 failures).
-    if (CC.latActive and blinker_dir != 0.0 and
+    lateral_control_mode = self.sm['carOutput'].actuatorsOutput.lateralControlMode
+    if (turn_lead_allowed(self.CP.brand, lateral_control_mode) and
+        CC.latActive and blinker_dir != 0.0 and
         model_v2.meta.laneChangeState == LaneChangeState.off and
         TURN_LEAD_MIN_SPEED <= CS.vEgo < TURN_LEAD_MAX_SPEED and
         new_desired_curvature * blinker_dir > -TURN_LEAD_MODEL_OPPOSE):
@@ -650,7 +666,8 @@ class Controls:
 
     self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll,
                                                                jerk_factor)
-    lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
+    lat_smooth_seconds = get_control_lateral_smooth_seconds(self.CP.brand, CS.vEgo, self.CP.lateralSmoothSeconds)
+    lat_delay = self.sm["liveDelay"].lateralDelay + lat_smooth_seconds
 
     actuators.curvature = self.desired_curvature
     steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,

selfdrive/controls/tests/test_turn_lead.py

@@ -0,0 +1,30 @@
+from cereal import car
+
+import pytest
+
+from openpilot.selfdrive.controls.controlsd import get_control_lateral_smooth_seconds, turn_lead_allowed
+
+
+LateralControlMode = car.CarControl.Actuators.LateralControlMode
+
+
+def test_turn_lead_is_suppressed_only_during_applied_angle_control():
+  assert not turn_lead_allowed("rivian", LateralControlMode.angle)
+  assert turn_lead_allowed("rivian", LateralControlMode.torque)
+  assert turn_lead_allowed("rivian", LateralControlMode.torqueRecovering)
+  assert turn_lead_allowed("rivian", LateralControlMode.inactive)
+  assert turn_lead_allowed("ford", LateralControlMode.angle)
+
+
+@pytest.mark.parametrize("v_ego", [0.0, 5.0, 30.0])
+def test_non_rivian_control_smoothing_matches_starpilot(v_ego):
+  assert get_control_lateral_smooth_seconds("toyota", v_ego, 0.0) == 0.1
+
+
+@pytest.mark.parametrize(("v_ego", "expected"), [
+  (0.0, 0.4),
+  (5.0, 0.2),
+  (30.0, 0.0),
+])
+def test_rivian_control_smoothing_remains_speed_scheduled(v_ego, expected):
+  assert get_control_lateral_smooth_seconds("rivian", v_ego, 0.4) == pytest.approx(expected)

selfdrive/modeld/modeld.py

@@ -67,6 +67,17 @@ MIN_LAT_CONTROL_SPEED = 0.3
 BIG_MODEL_TIMEOUT = 60
 BIG_MODEL_LOAD_WAIT_TIMEOUT_MS = 30000
 BIG_MODEL_RUN_WAIT_TIMEOUT_MS = 3000
+LAT_SMOOTH_BP = [2.0, 8.0]
+
+
+def get_lateral_smooth_seconds(v_ego: float, maximum: float = 0.0) -> float:
+  return float(np.interp(v_ego, LAT_SMOOTH_BP, [maximum, 0.0]))
+
+
+def get_car_lateral_smooth_seconds(brand: str, v_ego: float, maximum: float) -> float:
+  if brand == "rivian":
+    return get_lateral_smooth_seconds(v_ego, maximum)
+  return maximum
 
 
 def _get_param_str(params: Params, key: str, default: str = "") -> str:
@@ -686,13 +697,15 @@ def main(demo=False):
       meta_extra = meta_main
 
     sm.update(0)
-    lat_smooth_seconds = _model_smooth_seconds(params, "LatSmoothSeconds", LAT_SMOOTH_SECONDS)
     long_smooth_seconds = _model_smooth_seconds(params, "LongSmoothSeconds", LONG_SMOOTH_SECONDS)
     long_delay = CP.longitudinalActuatorDelay + long_smooth_seconds
     desire = DH.desire
     is_rhd = sm["driverMonitoringState"].isRHD
     frame_id = sm["roadCameraState"].frameId
     v_ego = max(sm["carState"].vEgo, 0.)
+    lat_smooth_default = CP.lateralSmoothSeconds if CP.brand == "rivian" else LAT_SMOOTH_SECONDS
+    lat_smooth_maximum = _model_smooth_seconds(params, "LatSmoothSeconds", lat_smooth_default)
+    lat_smooth_seconds = get_car_lateral_smooth_seconds(CP.brand, v_ego, lat_smooth_maximum)
     lat_delay = sm["liveDelay"].lateralDelay + lat_smooth_seconds
     lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32)
     if sm.frame % 60 == 0:

selfdrive/modeld/tests/test_lateral_smoothing.py

@@ -0,0 +1,34 @@
+import pytest
+
+from openpilot.selfdrive.modeld.modeld import get_car_lateral_smooth_seconds, get_lateral_smooth_seconds
+
+
+@pytest.mark.parametrize(("v_ego", "expected"), [
+  (0.0, 0.4),
+  (2.0, 0.4),
+  (5.0, 0.2),
+  (8.0, 0.0),
+  (30.0, 0.0),
+])
+def test_lateral_smoothing_tapers_with_speed(v_ego, expected):
+  assert get_lateral_smooth_seconds(v_ego, 0.4) == pytest.approx(expected)
+
+
+@pytest.mark.parametrize("v_ego", [0.0, 5.0, 30.0])
+def test_default_lateral_smoothing_is_disabled(v_ego):
+  assert get_lateral_smooth_seconds(v_ego) == 0.0
+
+
+@pytest.mark.parametrize("v_ego", [0.0, 5.0, 30.0])
+def test_non_rivian_cars_keep_configured_starpilot_smoothing(v_ego):
+  assert get_car_lateral_smooth_seconds("toyota", v_ego, 0.4) == 0.4
+
+
+@pytest.mark.parametrize(("v_ego", "maximum", "expected"), [
+  (0.0, 0.4, 0.4),
+  (5.0, 0.4, 0.2),
+  (30.0, 0.4, 0.0),
+  (0.0, 0.0, 0.0),
+])
+def test_rivian_uses_configured_smoothing(v_ego, maximum, expected):
+  assert get_car_lateral_smooth_seconds("rivian", v_ego, maximum) == pytest.approx(expected)

selfdrive/pandad/pandad.py

@@ -11,6 +11,7 @@ from openpilot.common.basedir import BASEDIR
 from openpilot.common.params import Params, UnknownKeyName
 from openpilot.system.hardware import HARDWARE
 from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.pandad.rivian_long_flasher import prepare_rivian_bridge
 
 
 def get_selected_firmware_name(app_fn: str, remote_start: bool, hkg_remote_start: bool, ignore_ignition_line: bool) -> str:
@@ -159,6 +160,13 @@ def main() -> None:
 
       cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}")
 
+      # Update and reserve the Rivian harness bridge before managing internal Pandas.
+      bridge_serials = prepare_rivian_bridge(panda_serials)
+      panda_serials = [serial for serial in panda_serials if serial not in bridge_serials]
+      if len(panda_serials) == 0:
+        no_internal_panda_count += 1
+        continue
+
       # Flash pandas
       pandas: list[Panda] = []
       remote_start = get_remote_start_boots_comma(params)

selfdrive/pandad/rivian_long_flasher.py

@@ -0,0 +1,127 @@
+#!/usr/bin/env python3
+"""Firmware management for the Rivian Gen 1 harness bridge."""
+
+import os
+from itertools import accumulate
+
+from cereal import car
+from panda import Panda
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+
+FW_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), "rivian_long_fw.bin.signed")
+SECTOR_SIZES = [0x4000] * 4 + [0x10000] + [0x20000] * 11
+
+
+def _is_rivian() -> bool:
+  params = Params()
+
+  for key in ("CarParamsPersistent", "CarParamsCache", "CarParamsPrevRoute"):
+    cp_bytes = params.get(key)
+    if cp_bytes is None:
+      continue
+    try:
+      with car.CarParams.from_bytes(cp_bytes) as CP:
+        if CP.brand == "rivian":
+          return True
+    except Exception:
+      cloudlog.exception(f"Unable to read {key} while identifying Rivian bridge")
+
+  return False
+
+
+def is_rivian_vehicle() -> bool:
+  return _is_rivian()
+
+
+def _flash_static(handle, code: bytes) -> None:
+  assert Panda.flasher_present(handle)
+  last_sector = next((i + 1 for i, value in enumerate(accumulate(SECTOR_SIZES[1:])) if value > len(code)), -1)
+  assert 1 <= last_sector < 7, "Invalid Rivian bridge firmware size"
+
+  handle.controlWrite(Panda.REQUEST_IN, 0xB1, 0, 0, b'')
+  for sector in range(1, last_sector + 1):
+    handle.controlWrite(Panda.REQUEST_IN, 0xB2, sector, 0, b'')
+  for offset in range(0, len(code), 0x10):
+    handle.bulkWrite(2, code[offset:offset + 0x10])
+  try:
+    handle.controlWrite(Panda.REQUEST_IN, 0xD8, 0, 0, b'', expect_disconnect=True)
+  except Exception:
+    pass
+
+
+def _flash_panda(panda: Panda) -> None:
+  expected_signature = Panda.get_signature_from_firmware(FW_PATH)
+  if not panda.bootstub and panda.get_signature() == expected_signature:
+    cloudlog.info(f"Rivian bridge {panda.get_usb_serial()} already up to date")
+    return
+
+  cloudlog.info(f"Flashing Rivian Extreme harness bridge {panda.get_usb_serial()}")
+  with open(FW_PATH, "rb") as firmware:
+    code = firmware.read()
+
+  if not panda.bootstub:
+    # Old F4 firmware cannot use Panda.reset(); enter its bootstub directly.
+    try:
+      panda._handle.controlWrite(Panda.REQUEST_IN, 0xD1, 1, 0, b'', timeout=15000, expect_disconnect=True)
+    except Exception:
+      pass
+    panda.close()
+    panda.reconnect()
+
+  _flash_static(panda._handle, code)
+  panda.reconnect()
+  cloudlog.info(f"Successfully flashed Rivian Extreme harness bridge {panda.get_usb_serial()}")
+
+
+def is_rivian_bridge_panda(panda: Panda, rivian: bool | None = None) -> bool:
+  if panda.is_internal() or panda.get_type() != Panda.HW_TYPE_BLACK:
+    return False
+  if rivian is None:
+    rivian = _is_rivian()
+  if rivian or panda.bootstub:
+    return rivian
+  try:
+    expected_signature = Panda.get_signature_from_firmware(FW_PATH)
+    return panda.get_signature() == expected_signature
+  except Exception:
+    return False
+
+
+def prepare_rivian_bridge(panda_serials: list[str]) -> set[str]:
+  """Identify bridge serials which must not be passed to normal Panda management."""
+  firmware_available = os.path.isfile(FW_PATH)
+  if not firmware_available:
+    cloudlog.error(f"Rivian bridge firmware not found at {FW_PATH}")
+
+  rivian = _is_rivian()
+  usb_serials = set(Panda.usb_list())
+  bridge_serials: set[str] = set()
+
+  for serial in panda_serials:
+    if serial not in usb_serials:
+      continue
+    panda = None
+    try:
+      panda = Panda(serial)
+      if panda.is_internal() or panda.get_type() != Panda.HW_TYPE_BLACK:
+        continue
+
+      bridge_confirmed = is_rivian_bridge_panda(panda, rivian)
+      if not bridge_confirmed:
+        continue
+
+      bridge_serials.add(serial)
+      if rivian and firmware_available:
+        _flash_panda(panda)
+    except Exception:
+      cloudlog.exception(f"Failed to prepare Rivian Extreme harness bridge {serial}")
+    finally:
+      if panda is not None:
+        panda.close()
+
+  return bridge_serials
+
+
+if __name__ == '__main__':
+  prepare_rivian_bridge(Panda.list())

selfdrive/pandad/tests/test_rivian_long_flasher.py

@@ -0,0 +1,80 @@
+import importlib.util
+from pathlib import Path
+from unittest.mock import MagicMock, patch  # noqa: TID251 - mocks are required to guarantee no hardware access
+
+
+MODULE_PATH = Path(__file__).parents[1] / "rivian_long_flasher.py"
+MODULE_SPEC = importlib.util.spec_from_file_location("rivian_long_flasher", MODULE_PATH)
+assert MODULE_SPEC is not None and MODULE_SPEC.loader is not None
+flasher = importlib.util.module_from_spec(MODULE_SPEC)
+MODULE_SPEC.loader.exec_module(flasher)
+
+
+def _external_black_panda(signature: bytes = b"current", bootstub: bool = False):
+  panda = MagicMock()
+  panda.is_internal.return_value = False
+  panda.get_type.return_value = b"\x03"
+  panda.get_signature.return_value = signature
+  panda.bootstub = bootstub
+  return panda
+
+
+def test_current_rivian_bridge_uses_bridge_flash_path():
+  panda = _external_black_panda(signature=b"expected")
+  with patch.object(flasher, "_is_rivian", return_value=True), \
+       patch.object(flasher.os.path, "isfile", return_value=True), \
+       patch.object(flasher, "Panda", wraps=flasher.Panda) as panda_class, \
+       patch.object(flasher, "_flash_panda") as flash_panda:
+    panda_class.return_value = panda
+    panda_class.HW_TYPE_BLACK = b"\x03"
+    panda_class.usb_list.return_value = ["bridge"]
+    panda_class.get_signature_from_firmware.return_value = b"expected"
+
+    assert flasher.prepare_rivian_bridge(["internal", "bridge"]) == {"bridge"}
+    flash_panda.assert_called_once_with(panda)
+    panda.close.assert_called_once()
+
+
+def test_outdated_rivian_bridge_uses_bridge_flash_path():
+  panda = _external_black_panda(signature=b"unexpected")
+  with patch.object(flasher, "_is_rivian", return_value=True), \
+       patch.object(flasher.os.path, "isfile", return_value=True), \
+       patch.object(flasher, "Panda", wraps=flasher.Panda) as panda_class, \
+       patch.object(flasher, "_flash_panda") as flash_panda:
+    panda_class.return_value = panda
+    panda_class.HW_TYPE_BLACK = b"\x03"
+    panda_class.usb_list.return_value = ["bridge"]
+    panda_class.get_signature_from_firmware.return_value = b"expected"
+
+    assert flasher.prepare_rivian_bridge(["internal", "bridge"]) == {"bridge"}
+    flash_panda.assert_called_once_with(panda)
+
+
+def test_non_rivian_matching_bridge_is_reserved_without_flashing():
+  panda = _external_black_panda(signature=b"expected")
+  with patch.object(flasher, "_is_rivian", return_value=False), \
+       patch.object(flasher.os.path, "isfile", return_value=True), \
+       patch.object(flasher, "Panda", wraps=flasher.Panda) as panda_class, \
+       patch.object(flasher, "_flash_panda") as flash_panda:
+    panda_class.return_value = panda
+    panda_class.HW_TYPE_BLACK = b"\x03"
+    panda_class.usb_list.return_value = ["bridge"]
+    panda_class.get_signature_from_firmware.return_value = b"expected"
+
+    assert flasher.prepare_rivian_bridge(["internal", "bridge"]) == {"bridge"}
+    flash_panda.assert_not_called()
+
+
+def test_non_rivian_external_black_panda_is_not_misidentified():
+  panda = _external_black_panda(signature=b"unexpected")
+  with patch.object(flasher, "_is_rivian", return_value=False), \
+       patch.object(flasher.os.path, "isfile", return_value=True), \
+       patch.object(flasher, "Panda", wraps=flasher.Panda) as panda_class, \
+       patch.object(flasher, "_flash_panda") as flash_panda:
+    panda_class.return_value = panda
+    panda_class.HW_TYPE_BLACK = b"\x03"
+    panda_class.usb_list.return_value = ["external"]
+    panda_class.get_signature_from_firmware.return_value = b"expected"
+
+    assert flasher.prepare_rivian_bridge(["internal", "external"]) == set()
+    flash_panda.assert_not_called()

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)