h8ters

cereal/log.capnp

@@ -130,8 +130,13 @@ struct OnroadEvent @0xc4fa6047f024e718 {
     userBookmark @95;
     excessiveActuation @96;
     audioFeedback @97;
+    bigModelLoading @100;
+    bigModelFailed @102;
 
     soundsUnavailableDEPRECATED @47;
+    stockLkasDEPRECATED @98;
+    lateralManeuverDEPRECATED @99;
+    bigModelReadyDEPRECATED @101;
   }
 }
 

common/realtime.py

@@ -22,6 +22,7 @@ class Priority:
   # - modeld = 55
   # - camerad = 54
   CTRL_LOW = 51 # plannerd & radard
+  UI = 50
 
   # CORE 3
   # - pandad = 55

selfdrive/controls/lib/longitudinal_planner.py

@@ -24,6 +24,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   get_force_stop_handoff_distance,
   is_gm_silverado_early_follow_lead,
   is_toyota_rav4_tss2_post_departure_tune,
+  get_toyota_rav4_tss2_early_lead_cap,
   get_toyota_sienna_post_departure_restop_cap,
   get_untracked_slow_lead_decel_scale,
 )
@@ -2321,11 +2322,17 @@ class LongitudinalPlanner:
       output_a_target = min(output_a_target, approach_lift_cap)
 
     close_lead_caps = []
+    rav4_early_lead_caps = []
     tracked_vision_approach_caps = []
     vision_low_speed_stop_active = False
     vision_brake_cap_active = False
     if lead_control_active:
       for lead in (self.lead_one, self.lead_two):
+        rav4_early_lead_cap = get_toyota_rav4_tss2_early_lead_cap(
+          self.CP, lead, v_ego, output_accel_min,
+        )
+        if rav4_early_lead_cap is not None:
+          rav4_early_lead_caps.append(rav4_early_lead_cap)
         cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min)
         if cap is not None:
           close_lead_caps.append(cap)
@@ -2725,6 +2732,11 @@ class LongitudinalPlanner:
       self.a_desired = min(self.a_desired, close_final_guard_cap)
       output_a_target = min(output_a_target, close_final_guard_cap)
 
+    if rav4_early_lead_caps:
+      rav4_early_lead_cap = min(rav4_early_lead_caps)
+      self.a_desired = min(self.a_desired, rav4_early_lead_cap)
+      output_a_target = min(output_a_target, rav4_early_lead_cap)
+
     if close_release_hold_cap is not None:
       self.a_desired = min(self.a_desired, close_release_hold_cap)
       output_a_target = min(output_a_target, close_release_hold_cap)

selfdrive/controls/lib/longitudinal_vehicle_tunes.py

@@ -18,6 +18,15 @@ TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_MODEL_PROB = 0.95
 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_LATERAL_OFFSET = 1.75
 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_BRAKE = 0.18
 TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_BRAKE = 0.32
+TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_EGO_SPEED = 12.0
+TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_MODEL_PROB = 0.85
+TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_LATERAL_OFFSET = 1.2
+TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_DISTANCE = 45.0
+TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DISTANCE = 105.0
+TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_CLOSING_SPEED = 4.0
+TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE = 0.8
+TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_BRAKE = 2.0
+TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DECEL = 0.5
 TOYOTA_CAMRY_TSS2_FORCE_STOP_HANDOFF_M = 4.5
 DEFAULT_FORCE_STOP_HANDOFF_M = 6.0
 
@@ -30,6 +39,49 @@ def is_toyota_rav4_tss2_post_departure_tune(CP):
   )
 
 
+def get_toyota_rav4_tss2_early_lead_cap(CP, lead, v_ego, accel_min):
+  """Start a mild RAV4 coast/brake response before a hard lead approach."""
+  if (
+    not is_toyota_rav4_tss2_post_departure_tune(CP) or
+    lead is None or not bool(getattr(lead, "status", False)) or
+    bool(getattr(lead, "radar", False)) or
+    float(v_ego) < TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_EGO_SPEED or
+    float(getattr(lead, "modelProb", 0.0)) < TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_MODEL_PROB or
+    abs(float(getattr(lead, "yRel", 0.0))) > TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_LATERAL_OFFSET
+  ):
+    return None
+
+  distance = float(getattr(lead, "dRel", float("inf")))
+  lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0)
+  closing_speed = float(v_ego) - lead_speed
+  lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
+  if (
+    not TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_DISTANCE <= distance <= TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DISTANCE or
+    closing_speed < TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_CLOSING_SPEED or
+    lead_brake < TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE
+  ):
+    return None
+
+  distance_factor = np.clip(
+    (TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DISTANCE - distance) /
+    (TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DISTANCE - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_DISTANCE),
+    0.0, 1.0,
+  )
+  closing_factor = np.clip((closing_speed - 4.0) / 6.0, 0.0, 1.0)
+  brake_factor = np.clip(
+    (lead_brake - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE) /
+    (TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_BRAKE - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE),
+    0.0, 1.0,
+  )
+  confidence_factor = np.clip(
+    (float(getattr(lead, "modelProb", 0.0)) - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_MODEL_PROB) / 0.13,
+    0.0, 1.0,
+  )
+  decel = 0.10 + 0.20 * distance_factor + 0.10 * closing_factor + 0.10 * brake_factor
+  decel *= 0.75 + 0.25 * confidence_factor
+  return max(float(accel_min), -min(TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DECEL, decel))
+
+
 def get_far_follow_output_slew_rates(CP):
   if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_HRV_3G":
     return (

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -20,6 +20,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import Longi
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
 from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   get_follow_prebrake_min_headway,
+  get_toyota_rav4_tss2_early_lead_cap,
   get_toyota_sienna_post_departure_restop_cap,
   is_gm_silverado_early_follow_lead,
   is_toyota_rav4_tss2_post_departure_tune,
@@ -2780,6 +2781,26 @@ def test_rav4_tss2_variants_use_the_car_specific_post_departure_tune():
   assert not is_toyota_rav4_tss2_post_departure_tune(other_cp)
 
 
+def test_rav4_tss2_early_lead_cap_starts_a_mild_response():
+  CP = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
+  lead = make_lead(status=True, d_rel=100.0, v_lead=13.0, a_lead=-1.1, model_prob=0.9)
+
+  cap = get_toyota_rav4_tss2_early_lead_cap(CP, lead, 21.0, -3.5)
+
+  assert cap is not None
+  assert -0.5 <= cap < 0.0
+
+
+def test_rav4_tss2_early_lead_cap_does_not_change_other_paths():
+  rav4 = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
+  other = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2022)
+  lead = make_lead(status=True, d_rel=100.0, v_lead=13.0, a_lead=-1.1, model_prob=0.9)
+  radar_lead = make_lead(status=True, d_rel=100.0, v_lead=13.0, a_lead=-1.1, radar=True, model_prob=1.0)
+
+  assert get_toyota_rav4_tss2_early_lead_cap(other, lead, 21.0, -3.5) is None
+  assert get_toyota_rav4_tss2_early_lead_cap(rav4, radar_lead, 21.0, -3.5) is None
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
 def test_force_stop_handoff_sets_output_should_stop_before_zero_vcruise(model_version):
   v_ego = 1.25

selfdrive/modeld/modeld.py

@@ -492,6 +492,7 @@ def main(demo=False):
   params.put_bool("UsbGpuActive", False)
   params.put_bool("UsbGpuLoading", external_gpu_requested)
   if external_gpu_requested:
+    os.environ["HCQDEV_WAIT_TIMEOUT_MS"] = "3000"
     from tinygrad.helpers import DEV
     device_config = tinygrad_dev_config(True, TICI)
     DEV.value = device_config

selfdrive/selfdrived/events.py

@@ -523,6 +523,17 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
                                        "Ensure road ahead is clear"),
   },
 
+  EventName.bigModelLoading: {
+    ET.NO_ENTRY: NoEntryAlert("Big Model Loading"),
+  },
+
+  EventName.bigModelFailed: {
+    ET.SOFT_DISABLE: soft_disable_alert("Big Model Failed"),
+    ET.PERMANENT: NormalPermanentAlert("Big Model Failed",
+                                       "Restart the car to retry,\nsmall model is still available",
+                                       duration=20.),
+  },
+
   EventName.selfdriveInitializing: {
     ET.NO_ENTRY: NoEntryAlert("System Initializing"),
   },

selfdrive/selfdrived/selfdrived.py

@@ -223,6 +223,11 @@ class SelfdriveD:
     self.events_prev = []
     self.logged_comm_issue = None
     self.not_running_prev = None
+    self.big_model_loading = False
+    self.big_model_attempted = False
+    self.big_model_active = False
+    self.big_model_failed = False
+    self.big_model_ready_t = 0.
     self.experimental_mode = False
     self.ecu_disable_failed = False
     self.ecu_disable_failed_checked = not (
@@ -347,6 +352,27 @@ class SelfdriveD:
       self.events.add(EventName.joystickDebug)
       self.startup_event = None
 
+    loading = self.params.get_bool("UsbGpuLoading")
+    if loading:
+      self.big_model_attempted = True
+    if self.big_model_loading and not loading:
+      self.big_model_ready_t = time.monotonic()
+    self.big_model_loading = loading
+    if loading:
+      self.events.add(EventName.bigModelLoading)
+
+    big_active = self.params.get("UsbGpuActive")
+    model_unavailable = self.big_model_active and self.sm.seen['modelV2'] and not self.sm.alive['modelV2']
+    big_failed = self.big_model_attempted and not loading and (big_active is False or model_unavailable)
+    if big_failed and not self.big_model_failed:
+      self.events.add(EventName.bigModelFailed)
+    self.big_model_failed = big_failed
+
+    if big_active:
+      self.big_model_active = True
+    if not self.enabled and not model_unavailable:
+      self.big_model_active = False
+
     if self.sm.recv_frame['lateralManeuverPlan'] > 0:
       self.starpilot_events.add(StarPilotEventName.lateralManeuver)
       self.startup_event = None
@@ -590,6 +616,9 @@ class SelfdriveD:
     # All events here should at least have NO_ENTRY and SOFT_DISABLE.
     num_events = len(self.events)
 
+    if self.big_model_active and big_failed:
+      self.events.add(EventName.bigModelFailed)
+
     not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning}
     if self.sm.recv_frame['managerState'] and len(not_running):
       if not_running != self.not_running_prev:
@@ -623,7 +652,8 @@ class SelfdriveD:
                          (contains_event_type(self.events, self.starpilot_events, ET.SOFT_DISABLE) or
                           contains_event_type(self.events, self.starpilot_events, ET.IMMEDIATE_DISABLE))
     no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
-    if not self.sm.all_checks() and no_system_errors:
+    big_model_settling = self.big_model_loading or time.monotonic() < self.big_model_ready_t + 5.
+    if not self.sm.all_checks() and no_system_errors and not big_model_settling:
       if not self.sm.all_alive():
         self.events.add(EventName.commIssue)
       elif not self.sm.all_freq_ok():
@@ -642,7 +672,7 @@ class SelfdriveD:
     else:
       self.logged_comm_issue = None
 
-    if not self.CP.notCar:
+    if not self.CP.notCar and not big_model_settling:
       if not self.sm['livePose'].posenetOK:
         self.events.add(EventName.posenetInvalid)
       if not self.sm['livePose'].inputsOK:

selfdrive/ui/ui.py

@@ -3,7 +3,7 @@ import os
 import time
 
 from openpilot.system.hardware import TICI
-from openpilot.common.realtime import config_realtime_process, set_core_affinity
+from openpilot.common.realtime import Priority, config_realtime_process, set_core_affinity
 from openpilot.common.watchdog import kick_watchdog
 from openpilot.system.ui.lib.application import gui_app
 from openpilot.selfdrive.ui.stall_monitor import UIStallMonitor
@@ -44,7 +44,7 @@ def _stall_context() -> dict[str, object]:
 
 def main():
   cores = {5, }
-  config_realtime_process(0, 51)
+  config_realtime_process(0, Priority.UI)
 
   stall_monitor = UIStallMonitor("raylib_ui")
   stall_monitor.progress("ui.before_init_window")