diff --git a/common/libcommon.a b/common/libcommon.a index 86c37c594..2a6e809fc 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index f92a78f8b..122afae05 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -132,7 +132,9 @@ inline static std::unordered_map keys = { {"SentryModeCapture", {CLEAR_ON_MANAGER_START, BOOL}}, {"SentryModeLastEvent", {PERSISTENT, JSON, "{}", "{}"}}, {"SentryModeNtfyUrl", {PERSISTENT, STRING}}, + {"SentryModeSensitivity", {PERSISTENT, FLOAT, "0.04", "0.04", 0, SETTINGS_SIMPLE}}, {"SentryModeStatus", {CLEAR_ON_MANAGER_START | DONT_LOG, JSON}}, + {"SentryModeWarningTime", {PERSISTENT, FLOAT, "1.0", "1.0", 0, SETTINGS_SIMPLE}}, {"SentryModeWebhook", {PERSISTENT, STRING}}, {"SecOCKey", {PERSISTENT | DONT_LOG, STRING}}, {"ShowDebugInfo", {PERSISTENT, BOOL}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index 3dff25520..dfe2034b2 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4a0eb9eae..32b58cbde 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -27,6 +27,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( is_gm_silverado_early_follow_lead, is_toyota_rav4_tss2_post_departure_tune, get_toyota_rav4_tss2_early_lead_cap, + is_toyota_rav4_tss2_radar_follow_lead, get_toyota_sienna_post_departure_restop_cap, get_untracked_slow_lead_decel_scale, ) @@ -128,6 +129,7 @@ VISION_LEAD_APPROACH_DEFICIT_BUFFER_GAIN = 0.20 VISION_LEAD_APPROACH_BRAKING_DEFICIT_MIN = 0.75 VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE = 0.45 VISION_LEAD_APPROACH_BRAKING_FULL_LEAD_BRAKE = 1.20 +PLANNER_SAFETY_WARNING_INTERVAL = 5.0 VISION_LEAD_APPROACH_BRAKING_FLOOR_MIN_DECEL = 1.30 VISION_LEAD_APPROACH_BRAKING_FLOOR_MAX_DECEL = 1.75 VISION_LEAD_APPROACH_CONFIRM_TIME = 0.25 @@ -458,7 +460,9 @@ VISION_CLOSE_RELEASE_HOLD_MIN_LEAD_DELTA = -0.1 VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA = 1.5 VISION_CLOSE_RELEASE_HOLD_MIN_BRAKE = 0.18 VISION_CLOSE_RELEASE_HOLD_MAX_BRAKE = 0.40 -MANUAL_STOP_RESUME_OVERRIDE_TIME = 3.0 +# Give a driver-initiated launch enough time to clear a stale model stop/light +# prediction before the stop request is allowed to reassert. +MANUAL_STOP_RESUME_OVERRIDE_TIME = 6.0 MANUAL_STOP_RESUME_OVERRIDE_MAX_SPEED = 2.0 def get_planner_v_ego(CP, car_state): @@ -573,6 +577,7 @@ class LongitudinalPlanner: self._uncert_last = 0.0 self._uncert_last_t = None self._panic_bypass_log_t = 0.0 + self._safety_warning_log_t = 0.0 self.effective_t_follow = None self.vision_low_speed_stop_hold_until = 0.0 self.vision_lead_approach_confirm_t = 0.0 @@ -1234,7 +1239,10 @@ class LongitudinalPlanner: starpilot_car_state = sm["starpilotCarState"] except KeyError: starpilot_car_state = None - accel_pressed = bool(getattr(starpilot_car_state, "accelPressed", False)) + accel_pressed = bool( + getattr(starpilot_car_state, "accelPressed", False) or + getattr(sm["carState"], "gasPressed", False) + ) model_should_stop = bool(getattr(sm["modelV2"].action, "shouldStop", False)) standstill = bool(getattr(sm["carState"], "standstill", False)) forcing_stop = bool(getattr(sm["starpilotPlan"], "forcingStop", False)) @@ -1980,9 +1988,14 @@ class LongitudinalPlanner: not experimental_mode and any(is_gm_silverado_early_follow_lead(self.CP, lead, scene_v_ego) for lead in (self.lead_one, self.lead_two)) ) + rav4_radar_follow = ( + not experimental_mode and + any(is_toyota_rav4_tss2_radar_follow_lead(self.CP, lead, scene_v_ego) + for lead in (self.lead_one, self.lead_two)) + ) # StarPilot trackingLead is debounce/model-length based. Keep a raw close-lead # safety path so ACC/chill does not ignore a visible lead during that debounce. - lead_control_active = tracking_lead or raw_close_lead_control or early_truck_follow + lead_control_active = tracking_lead or raw_close_lead_control or early_truck_follow or rav4_radar_follow lead_one_active = bool(self.lead_one.status and lead_control_active) effective_t_follow = self.get_dynamic_t_follow(sm['starpilotPlan'].tFollow, self.lead_one if lead_one_active else None, v_ego) @@ -2209,10 +2222,12 @@ class LongitudinalPlanner: # Safety checks for rubber-banding mitigation max_jerk = np.max(np.abs(self.mpc.j_solution)) max_accel_change = np.max(np.abs(np.diff(self.mpc.a_solution))) - if max_jerk > 5.0: # m/s^3 - cloudlog.warning(f"High jerk detected: {max_jerk:.2f} m/s^3") - if max_accel_change > 2.0: # m/s^2 - cloudlog.warning(f"High acceleration change: {max_accel_change:.2f} m/s^2") + if (max_jerk > 5.0 or max_accel_change > 2.0) and now_t - self._safety_warning_log_t >= PLANNER_SAFETY_WARNING_INTERVAL: + cloudlog.warning( + f"Longitudinal planner output discontinuity: jerk={max_jerk:.2f} m/s^3, " + f"accel_change={max_accel_change:.2f} m/s^2" + ) + self._safety_warning_log_t = now_t # Interpolate 0.05 seconds and save as starting point for next iteration a_prev = self.a_desired diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index 202ced61c..9a3f866fd 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -27,8 +27,18 @@ 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_RAV4_TSS2_RADAR_FOLLOW_MIN_SPEED = 5.0 +TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MIN_CLOSING_SPEED = 0.75 +TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MIN_DISTANCE = 70.0 +TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MAX_DISTANCE = 100.0 +TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_TIME = 4.5 +TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_OFFSET = 32.0 +TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MAX_LATERAL_OFFSET = 1.75 TOYOTA_CAMRY_TSS2_FORCE_STOP_HANDOFF_M = 4.5 -TOYOTA_CAMRY_TSS2_FORCE_STOP_DISTANCE_BIAS_M = 2.0 +# The Camry's force-stop path otherwise consumes the model endpoint before the +# normal MPC stop-distance margin can be applied. Keep it within the forward +# offset range exposed by the Force Stop setting. +TOYOTA_CAMRY_TSS2_FORCE_STOP_DISTANCE_BIAS_M = 6.0 DEFAULT_FORCE_STOP_HANDOFF_M = 6.0 @@ -83,6 +93,31 @@ def get_toyota_rav4_tss2_early_lead_cap(CP, lead, v_ego, accel_min): return max(float(accel_min), -min(TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DECEL, decel)) +def is_toyota_rav4_tss2_radar_follow_lead(CP, lead, v_ego): + """Keep a credible RAV4 radar lead active through model-horizon dropouts.""" + if ( + not is_toyota_rav4_tss2_post_departure_tune(CP) or + lead is None or not bool(getattr(lead, "status", False)) or + not bool(getattr(lead, "radar", False)) or + float(v_ego) < TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MIN_SPEED or + abs(float(getattr(lead, "yRel", 0.0))) > TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MAX_LATERAL_OFFSET + ): + return False + + lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0) + closing_speed = float(v_ego) - lead_speed + distance_limit = float(np.clip( + TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_OFFSET + + TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_TIME * float(v_ego), + TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MIN_DISTANCE, + TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MAX_DISTANCE, + )) + return ( + float(getattr(lead, "dRel", float("inf"))) <= distance_limit and + closing_speed >= TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MIN_CLOSING_SPEED + ) + + def allow_radar_standstill_gap_settle(CP): """Keep the generic stopped-lead gap nudge out of the early RAV4 TSS2 path.""" return not ( diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 3ba70b77e..6e17b74a4 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -23,6 +23,7 @@ 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_toyota_rav4_tss2_radar_follow_lead, is_gm_silverado_early_follow_lead, is_toyota_rav4_tss2_post_departure_tune, ) @@ -1764,6 +1765,23 @@ def test_manual_resume_override_clears_no_lead_model_stop_at_standstill(model_ve assert planner.output_a_target >= 0.2 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_manual_resume_override_accepts_accelerator_pedal(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + sm = make_sm(0.0, desired_accel=0.0, min_accel=-0.5) + sm["carState"].standstill = True + sm["carState"].gasPressed = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["modelV2"].action.shouldStop = True + sm["starpilotPlan"].forcingStop = True + + planner.update(sm, make_toggles(model_version)) + + assert not planner.output_should_stop + assert planner.output_a_target >= 0.2 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_manual_resume_override_does_not_clear_stopped_lead_stop(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) @@ -2827,6 +2845,25 @@ def test_rav4_tss2_early_lead_cap_does_not_change_other_paths(): assert get_toyota_rav4_tss2_early_lead_cap(rav4, radar_lead, 21.0, -3.5) is None +def test_rav4_tss2_radar_follow_admits_closing_lead_before_model_tracking(): + rav4 = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2) + lead = make_lead(status=True, d_rel=74.0, v_lead=1.0, radar=True) + + assert is_toyota_rav4_tss2_radar_follow_lead(rav4, lead, 9.7) + + +def test_rav4_tss2_radar_follow_admission_is_vehicle_and_safety_scoped(): + rav4 = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2) + other = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2022) + lead = make_lead(status=True, d_rel=74.0, v_lead=1.0, radar=True) + far_lead = make_lead(status=True, d_rel=110.0, v_lead=1.0, radar=True) + vision_lead = make_lead(status=True, d_rel=74.0, v_lead=1.0, radar=False, model_prob=1.0) + + assert not is_toyota_rav4_tss2_radar_follow_lead(other, lead, 9.7) + assert not is_toyota_rav4_tss2_radar_follow_lead(rav4, far_lead, 9.7) + assert not is_toyota_rav4_tss2_radar_follow_lead(rav4, vision_lead, 9.7) + + @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/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index a5c9b9b98..fc48e72c8 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -126,7 +126,7 @@ def test_camry_tss2_uses_closer_force_stop_handoff(): def test_camry_tss2_gets_forward_force_stop_bias_only(): - assert get_force_stop_distance_bias("TOYOTA_CAMRY_TSS2") == pytest.approx(2.0) + assert get_force_stop_distance_bias("TOYOTA_CAMRY_TSS2") == pytest.approx(6.0) assert get_force_stop_distance_bias("TOYOTA_RAV4_TSS2") == pytest.approx(0.0) @@ -510,6 +510,23 @@ def test_force_stop_releases_after_cem_light_clears_while_moving(): assert not vcruise.force_stop_from_light +def test_force_stop_light_release_ignores_coarse_stopped_model_horizon(): + planner, vcruise = make_vcruise(red_light=True, raw_model_stopped=True, forcing_stop=True) + sm = make_sm(standstill=False) + toggles = make_toggles() + + update_vcruise(vcruise, sm, toggles, now=0.0, v_ego=3.0) + planner.starpilot_cem.stop_light_detected = False + + update_vcruise(vcruise, sm, toggles, now=0.25, v_ego=3.0) + assert vcruise.forcing_stop + + result = update_vcruise(vcruise, sm, toggles, now=0.75, v_ego=3.0) + assert result == pytest.approx(20.0) + assert not vcruise.forcing_stop + assert not vcruise.force_stop_from_light + + def test_force_stop_turn_scene_veto_blocks_new_activation(): _, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=False) sm = make_sm(standstill=False) diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index 1794f2a5d..29fab7440 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -408,7 +408,7 @@ class HudRenderer(Widget): # draw drop shadow circle_radius = 162 // 2 - rl.draw_circle_gradient(int(x + circle_radius), int(y + circle_radius), circle_radius, + rl.draw_circle_gradient(rl.Vector2(x + circle_radius, y + circle_radius), circle_radius, rl.Color(0, 0, 0, int(255 / 2 * alpha)), rl.BLANK) set_speed_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha)) @@ -630,7 +630,7 @@ class HudRenderer(Widget): center = rl.Vector2(button_rect.x + button_rect.width / 2, button_rect.y + button_rect.height / 2) radius = min(button_rect.width, button_rect.height) / 2 - rl.draw_circle_gradient(int(center.x), int(center.y), radius, rl.Color(0, 0, 0, 90), rl.BLANK) + rl.draw_circle_gradient(center, radius, rl.Color(0, 0, 0, 90), rl.BLANK) rl.draw_circle(int(center.x), int(center.y), radius, fill) rl.draw_ring(center, radius - 6, radius, 0, 360, 48, outline) diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 00bb79a70..bcc205818 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -487,7 +487,6 @@ class StarPilotVCruise: self.force_stop_from_light and not sm["carState"].standstill and not stop_light_detected and - not raw_model_stopped and not dash_active ) if light_stop_cleared: diff --git a/starpilot/system/the_galaxy/assets/components/tools/sentry.css b/starpilot/system/the_galaxy/assets/components/tools/sentry.css index f947b36c5..f7d2144e2 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/sentry.css +++ b/starpilot/system/the_galaxy/assets/components/tools/sentry.css @@ -86,6 +86,31 @@ outline: none; } +.sentry-range-control { + align-items: center; + display: flex; + gap: 0.75rem; + min-width: min(100%, 28rem); +} + +.sentry-range { + accent-color: var(--main-fg); + cursor: pointer; + flex: 1; + min-width: 12rem; +} + +.sentry-range:disabled { + cursor: not-allowed; +} + +.sentry-range-value { + color: var(--text-color); + font-variant-numeric: tabular-nums; + min-width: 3.5rem; + text-align: right; +} + .sentry-toggle { accent-color: var(--main-fg); cursor: pointer; @@ -190,4 +215,13 @@ min-width: 0; width: 100%; } + + .sentry-range-control { + min-width: 0; + width: 100%; + } + + .sentry-range { + min-width: 0; + } } diff --git a/starpilot/system/the_galaxy/assets/components/tools/sentry.js b/starpilot/system/the_galaxy/assets/components/tools/sentry.js index fb4262b6e..8bc5a18f4 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/sentry.js +++ b/starpilot/system/the_galaxy/assets/components/tools/sentry.js @@ -50,6 +50,11 @@ function startPolling() { pollTimer = window.setInterval(fetchStatus, 5000) } +function numericParam(key, fallback) { + const value = Number(state.params[key]) + return Number.isFinite(value) ? value : fallback +} + async function saveParam(key, value) { state.savingKey = key try { @@ -231,7 +236,7 @@ export function SentryMode() {

Configuration

-

Galaxy is the built-in notification and image viewer. Webhook and ntfy delivery are optional.

+

Galaxy is the built-in notification and image viewer. Sentry detects accelerometer movement. Webhook and ntfy delivery are optional.

${() => state.loading ? html`
Loading Sentry settings…
` : html` + + + +