From a913d65aa8bf898bb5c2e2e50284c7a1f63deca0 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 6 Oct 2026 21:16:24 -0500 Subject: [PATCH] Restore low-speed steering advisory --- openpilot/selfdrive/selfdrived/selfdrived.py | 10 +- .../starpilot/lateral/low_speed_advisory.py | 29 +++++ .../lateral/tests/test_low_speed_advisory.py | 100 ++++++++++++++++++ 3 files changed, 138 insertions(+), 1 deletion(-) create mode 100644 openpilot/starpilot/lateral/low_speed_advisory.py create mode 100644 openpilot/starpilot/lateral/tests/test_low_speed_advisory.py diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index b8a6a15aaa..7efa588635 100755 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -40,6 +40,7 @@ from openpilot.starpilot.conditional_mode.runtime_settings import ConditionalSet from openpilot.starpilot.conditional_mode.status import settings_fingerprint from openpilot.starpilot.feature_runtime import enabled as feature_enabled from openpilot.starpilot.nostalgia import aol_no_entry, paddle_cancel, physical_cancel, saved_enabled as nostalgia_saved_enabled +from openpilot.starpilot.lateral.low_speed_advisory import LowSpeedAdvisory from openpilot.starpilot.lateral.lane_change_status_wire import alert_wording, decode as decode_lane_status, fresh_for_model from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert @@ -104,6 +105,7 @@ class SelfdriveD: self.switchback_capable = ioniq6_media_eligible(self.CP) and not self.CP.passive and not self.CP.dashcamOnly and not self.CP.notCar self.switchback_status = SwitchbackStatusOwner() self.switchback_cooldown = SwitchbackCooldown() + self.low_speed_advisory = LowSpeedAdvisory() self.switchback_setting_ns = 0 self.switchback_cooldown_ns = 300_000_000_000 self.conditional_car_state_valid = False @@ -342,7 +344,13 @@ class SelfdriveD: # Add car events, ignore if CAN isn't valid if CS.canValid: car_events = self.car_events.update(CS, self.CS_prev, self.sm['carControl']).to_msg() - self.events.add_from_msg(car_events) + device = self.sm['deviceState'] + drive_id = (int(device.startedMonoTime) if self.sm.seen['deviceState'] and self.sm.alive['deviceState'] and + self.sm.valid['deviceState'] and device.started else 0) + show_low_speed = self.low_speed_advisory.update(float(CS.vEgo), float(self.CP.minSteerSpeed), drive_id=drive_id) + self.events.add_from_msg([event for event in car_events if event.name != EventName.belowSteerSpeed]) + if show_low_speed: + self.events.add(EventName.belowSteerSpeed) if self.tesla_stock_consumer.binding is not None: now_ns = self.aol_car_state_log_ns if REPLAY and self.aol_car_state_log_ns else time.monotonic_ns() companion = current_intent(self.sm, car_state_ns=int(self.sm.logMonoTime['carState']), now_ns=now_ns, diff --git a/openpilot/starpilot/lateral/low_speed_advisory.py b/openpilot/starpilot/lateral/low_speed_advisory.py new file mode 100644 index 0000000000..c2b9963fc5 --- /dev/null +++ b/openpilot/starpilot/lateral/low_speed_advisory.py @@ -0,0 +1,29 @@ +"""The first below-minimum-steering-speed episode in an onroad session.""" + +import math + + +class LowSpeedAdvisory: + def __init__(self): + self.drive_id = 0 + self.has_been_above = False + self.shown = False + self.showing = False + + def update(self, speed: float, minimum: float, *, drive_id: int = 0) -> bool: + # Unknown/transient device state does not restart the advisory lifecycle. + if drive_id > 0 and drive_id != self.drive_id: + self.drive_id = drive_id + self.has_been_above = False + self.shown = False + self.showing = False + if not math.isfinite(speed) or not math.isfinite(minimum) or minimum <= 0: + return False + under = speed < minimum + if not under: + self.has_been_above = True + self.showing = False + elif self.has_been_above and not self.shown: + self.shown = True + self.showing = True + return self.showing and under diff --git a/openpilot/starpilot/lateral/tests/test_low_speed_advisory.py b/openpilot/starpilot/lateral/tests/test_low_speed_advisory.py new file mode 100644 index 0000000000..fb4469112c --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_low_speed_advisory.py @@ -0,0 +1,100 @@ +import unittest + +from openpilot.starpilot.lateral.low_speed_advisory import LowSpeedAdvisory + + +class TestLowSpeedAdvisory(unittest.TestCase): + def test_startup_first_crossing_episode_and_later_crossings(self): + owner = LowSpeedAdvisory() + minimum = 10 / 3.6 + for speed in (0., 1., minimum - .001): + self.assertFalse(owner.update(speed, minimum, drive_id=10)) + self.assertFalse(owner.update(minimum, minimum, drive_id=10)) + for speed in (minimum - .001, 0., 1., 0.): + self.assertTrue(owner.update(speed, minimum, drive_id=10)) + self.assertFalse(owner.update(minimum, minimum, drive_id=10)) + for speed in (0., minimum + 1., 0.): + self.assertFalse(owner.update(speed, minimum, drive_id=10)) + + def test_confirmed_new_drive_and_process_reset(self): + owner = LowSpeedAdvisory() + owner.update(4., 3., drive_id=10) + self.assertTrue(owner.update(2., 3., drive_id=10)) + self.assertTrue(owner.update(2., 3., drive_id=0)) + self.assertFalse(owner.update(2., 3., drive_id=20)) + self.assertFalse(owner.update(3., 3., drive_id=20)) + self.assertTrue(owner.update(2., 3., drive_id=20)) + self.assertFalse(LowSpeedAdvisory().update(2., 3., drive_id=20)) + + def test_invalid_sample_cannot_create_or_erase_crossing(self): + owner = LowSpeedAdvisory() + for speed in (float('nan'), float('inf'), -float('inf')): + self.assertFalse(owner.update(speed, 3., drive_id=10)) + self.assertFalse(owner.update(2., 3., drive_id=10)) + owner.update(4., 3., drive_id=10) + self.assertTrue(owner.update(2., 3., drive_id=10)) + self.assertFalse(owner.update(float('nan'), 3., drive_id=0)) + self.assertTrue(owner.update(2., 3., drive_id=10)) + self.assertFalse(owner.update(2., 0., drive_id=10)) + + def test_actual_selfdrived_event_hook_preserves_faults(self): + from unittest.mock import patch + from opendbc.car import gen_empty_fingerprint, structs + from opendbc.car.gm.interface import CarInterface + from opendbc.car.gm.values import CAR + from openpilot.cereal import messaging + from openpilot.common.prefix import OpenpilotPrefix + from openpilot.selfdrive.selfdrived.events import ET, EventName + from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD + + fingerprint = gen_empty_fingerprint() + fingerprint[0][0x201] = 6 + fingerprint[2].update({0x180: 4, 0x320: 8}) + cp = CarInterface.get_params(CAR.CHEVROLET_BOLT_CC_2018_2021, fingerprint, [], False, False, False) + self.assertEqual(cp.safetyConfigs[0].safetyParam, 0x9D) + self.assertAlmostEqual(cp.minSteerSpeed, 10 / 3.6, delta=1e-6) + with OpenpilotPrefix(), patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', True): + drive = SelfdriveD(CP=cp) + drive.initialized = True + device = messaging.new_message('deviceState', valid=True, logMonoTime=1_000_000_000) + device.deviceState.started = True + device.deviceState.startedMonoTime = 1_000_000_000 + device.deviceState.freeSpacePercent = 50. + drive.sm.update_msgs(1., [device.as_reader()]) + state = structs.CarState() + state.canValid = True + state.gearShifter = structs.CarState.GearShifter.drive + state.cruiseState.available = True + state.vCruise = 40. + + def sample(speed, *, temporary=False, permanent=False, valid=True, timeout=False): + state.vEgo = speed + state.lowSpeedAlert = speed < cp.minSteerSpeed + state.steerFaultTemporary = temporary + state.steerFaultPermanent = permanent + state.canValid, state.canTimeout = valid, timeout + drive.update_events(state) + drive.CS_prev = structs.CarState.new_message(**state.to_dict()) + return set(drive.events.names) + + for speed, expected in ((0., False), (cp.minSteerSpeed, False), (1., True), + (0., True), (cp.minSteerSpeed, False), (1., False)): + with self.subTest(speed=speed, expected=expected): + self.assertEqual(EventName.belowSteerSpeed in sample(speed), expected) + # Actual CarEvents severity is retained, regardless of advisory suppression. + temporary = sample(4., temporary=True) + self.assertIn(EventName.steerTempUnavailableSilent, temporary) + warning = next(event for event in drive.events.to_msg() if event.name == EventName.steerTempUnavailableSilent) + self.assertTrue(warning.warning) + self.assertFalse(warning.immediateDisable) + permanent = sample(4., permanent=True) + self.assertIn(EventName.steerUnavailable, permanent) + self.assertTrue(drive.events.contains(ET.IMMEDIATE_DISABLE)) + self.assertIn(EventName.canError, sample(1., valid=False)) + self.assertIn(EventName.canBusMissing, sample(1., valid=False, timeout=True)) + device.deviceState.startedMonoTime = 2_000_000_000 + device.logMonoTime = 2_000_000_000 + drive.sm.update_msgs(2., [device.as_reader()]) + self.assertNotIn(EventName.belowSteerSpeed, sample(1.)) + self.assertNotIn(EventName.belowSteerSpeed, sample(cp.minSteerSpeed)) + self.assertIn(EventName.belowSteerSpeed, sample(1.))