Merge PR #90: Rivian Angle Support

Original PR by TonyJOM (Anthony Orta).

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

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)