diff --git a/cereal/log.capnp b/cereal/log.capnp index b34322caa..4f12e4cdf 100644 --- a/cereal/log.capnp +++ b/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; } } diff --git a/common/realtime.py b/common/realtime.py index 2d72c0a66..d4c22a6d4 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -22,6 +22,7 @@ class Priority: # - modeld = 55 # - camerad = 54 CTRL_LOW = 51 # plannerd & radard + UI = 50 # CORE 3 # - pandad = 55 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index d38f8a04f..b292ffd85 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index 80471ed8d..731a10ed4 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/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 ( diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 699cc2206..c883e7e8d 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/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 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 11ef530e6..03a2b3897 100755 --- a/selfdrive/modeld/modeld.py +++ b/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 diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index f4c4d4fdd..6d530cab4 100644 --- a/selfdrive/selfdrived/events.py +++ b/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"), }, diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 3e57cd448..c8b48609b 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/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: diff --git a/selfdrive/ui/ui.py b/selfdrive/ui/ui.py index dcbba3cc1..8a98ae87a 100644 --- a/selfdrive/ui/ui.py +++ b/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")