From 6d7aca20a988f84e82ab0476da181414862dbaf2 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Fri, 6 Nov 2026 11:45:46 -0600 Subject: [PATCH 1/4] Force Stop fix --- .../starpilot/longitudinal/force_stop.py | 9 ++++---- .../longitudinal/tests/test_force_stop.py | 22 +++++++++++++++++++ 2 files changed, 26 insertions(+), 5 deletions(-) diff --git a/openpilot/starpilot/longitudinal/force_stop.py b/openpilot/starpilot/longitudinal/force_stop.py index 6417e1b737..488cfbddf5 100644 --- a/openpilot/starpilot/longitudinal/force_stop.py +++ b/openpilot/starpilot/longitudinal/force_stop.py @@ -187,7 +187,7 @@ class ForceStop: else: self.timer = max(0, self.timer - DT_MDL * .25) force = self.timer >= .5 or self.forcing and not frame.standstill or held - light_cleared = (not held and self.forcing and self.from_light and not frame.standstill and + light_cleared = (not held and self.forcing and self.from_light and not frame.standstill and not self.sign_confirmed and available and not frame.light and not frame.sign and go_scene) low_speed_hold = (light_cleared and tune.low_speed_hold_mps is not None and self.entry_speed is not None and speed <= tune.low_speed_hold_mps and speed < self.entry_speed - .25) @@ -228,8 +228,6 @@ class ForceStop: if frame.sign and available: if horizon < SIGN_MODEL_AGREE_M: self.distance = min(self.distance, SIGN_SEED_M) - if horizon < SIGN_SEED_M: - self.distance = horizon effective = self.distance + offset + tune.distance_bias_m decel = SIGN_APPROACH_DECEL if frame.sign else MODEL_APPROACH_DECEL ceiling = math.sqrt(2 * decel * max(0, effective - tune.handoff_m)) @@ -245,10 +243,11 @@ class ForceStop: if effective > tune.handoff_m: ceiling = math.sqrt(2 * MODEL_APPROACH_DECEL * (effective - tune.handoff_m)) self.previously_enabled = frame.enabled - stop_length = self.distance if self.forcing and self.distance > tune.handoff_m else approach + stop_length = self.distance if self.forcing else approach # The MPC obstacle includes its standing-distance term; adding it at the # native boundary avoids moving the actual stop line by that buffer. - obstacle = stop_length * .93 + offset + tune.distance_bias_m if stop_length > tune.handoff_m else None + obstacle = (stop_length * .93 + offset + tune.distance_bias_m + if stop_length > 0 and stop_length + offset > tune.handoff_m else None) return StopPlan(now, ceiling, obstacle, self.forcing, bool(self.forcing and ceiling is not None and ceiling <= .5), self.distance if self.forcing else 0.0, approach, tune.jerk_scale if self.forcing or approach > 0 else 1.0, bool(held and self.forcing and not overridden), diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop.py b/openpilot/starpilot/longitudinal/tests/test_force_stop.py index fe1b9f5a58..664b310cfd 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop.py +++ b/openpilot/starpilot/longitudinal/tests/test_force_stop.py @@ -229,6 +229,28 @@ class ForceStopTests(unittest.TestCase): plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=190., light=False, standstill=True), self.tune) self.assertFalse(plan.forcing) + def test_latched_sign_survives_light_and_sign_dropout(self): + for tick in range(15): + self.owner.step(frame(tick, speed_mps=8., horizon_m=45., sign=True), self.tune) + self.assertTrue(self.owner.sign_confirmed) + for tick in range(15, 45): + plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=190., light=False), self.tune) + self.assertTrue(plan.forcing) + + def test_sign_seed_never_moves_the_stop_farther(self): + for tick in range(50): + previous = self.owner.step(frame(tick, speed_mps=8., horizon_m=45., sign=True), self.tune) + self.assertLess(previous.tracked_distance_m, 20.) + plan = self.owner.step(frame(50, speed_mps=8., horizon_m=20., sign=True), self.tune) + self.assertLessEqual(plan.tracked_distance_m, previous.tracked_distance_m) + + def test_positive_offset_keeps_stop_target_until_native_stop(self): + for tick in range(40): + plan = self.owner.step(frame(tick, horizon_m=12.), self.tune, 8) + if plan.forcing: + self.assertTrue(plan.should_stop or plan.obstacle_m is not None) + self.assertTrue(plan.should_stop) + def test_odometry_ceiling_bounds_reopened_model(self): for tick in range(15): self.owner.step(frame(tick), self.tune) From 0fdfd0ddbf9011a1dbc0c9175f2c86fc5886bdd1 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 6 Nov 2026 11:45:48 -0600 Subject: [PATCH 2/4] Track Ioniq Force Stop braking to the remaining stop distance --- openpilot/cereal/custom.capnp | 3 + .../selfdrive/controls/lib/longcontrol.py | 5 + .../controls/lib/longitudinal_planner.py | 8 ++ openpilot/starpilot/longitudinal/extension.py | 2 + .../starpilot/longitudinal/force_stop.py | 31 ++++- .../longitudinal/force_stop_braking.py | 39 ++++++ openpilot/starpilot/longitudinal/inputs.py | 6 + .../longitudinal/tests/test_force_stop.py | 36 +++++ .../tests/test_force_stop_braking.py | 125 ++++++++++++++++++ prebuilt | 0 prebuilt.json | 1 - tools/ci/tests/test_schema_policy.py | 3 +- 12 files changed, 253 insertions(+), 6 deletions(-) create mode 100644 openpilot/starpilot/longitudinal/force_stop_braking.py create mode 100644 openpilot/starpilot/longitudinal/tests/test_force_stop_braking.py delete mode 100644 prebuilt delete mode 100644 prebuilt.json diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index 8c74b4006f..c8621dd483 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -277,6 +277,8 @@ struct StarPilotLongitudinalPlan @0xda96579883444c35 { modelMonoTime @2 :UInt64; driveStartMonoTime @3 :UInt64; forceStopHolding @4 :Bool; + forceStopBraking @5 :Bool; + forceStopEpisodeMonoTime @6 :UInt64; } struct SlcState @0xa1680744031fdb2d { @@ -1571,3 +1573,4 @@ struct LiveTrackingResponseWitness @0xdaa1a9f0d8968e31 { windowStartMonoTime @4 :UInt64; independentPoses @5 :UInt16; residualRmse @6 :Float64; validUntilMonoTime @7 :UInt64; } + diff --git a/openpilot/selfdrive/controls/lib/longcontrol.py b/openpilot/selfdrive/controls/lib/longcontrol.py index 3b93e6e536..77ee94691d 100644 --- a/openpilot/selfdrive/controls/lib/longcontrol.py +++ b/openpilot/selfdrive/controls/lib/longcontrol.py @@ -1,4 +1,5 @@ import numpy as np +import math from opendbc.car.structs import car from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N @@ -96,6 +97,9 @@ class LongControl: output_accel -= self.stopping_decel_rate * DT_CTRL # m/s^2/s while trying to stop if self.extension is not None: output_accel = self.extension.stopping_output(output_accel, a_target, should_stop, CS) + if (context.force_stop_braking and self.extension is not None and self.extension.ioniq6_start is not None and + should_stop and not CS.standstill and CS.vEgo > .3 and math.isfinite(a_target)): + output_accel = min(a_target, 0.0) self.reset(reset_start=False) elif self.long_control_state == LongCtrlState.starting and self.extension is not None and self.extension.starting: @@ -119,3 +123,4 @@ class LongControl: output_accel = min(output_accel, planner_target) self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1]) return self.last_output_accel + diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index fa96601a55..614669c7c1 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -32,6 +32,7 @@ from openpilot.starpilot.longitudinal.ioniq6_start import eligible as ioniq6_lon from openpilot.starpilot.longitudinal.lane_change_gap import LaneChangeGap, project as project_lane_gap from openpilot.starpilot.longitudinal.experimental_release import ConditionalHandoff, ExperimentalRelease, project as project_release from openpilot.starpilot.longitudinal.force_stop import StopPlan +from openpilot.starpilot.longitudinal.force_stop_braking import distance_target from openpilot.starpilot.longitudinal.follow_jerk import FollowJerk from openpilot.starpilot.longitudinal.lead_takeoff import LeadTakeoff, project as project_takeoff from openpilot.starpilot.longitudinal.lead_behavior import adjust as adjust_lead_behavior @@ -409,6 +410,9 @@ class LongitudinalPlanner: candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc), (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)] + if self.ioniq6_throttle_gate_enabled and self.force_stop_plan.forcing and self.force_stop_plan.obstacle_m is not None: + stop_target = distance_target(v_ego, sm['carState'].aEgo, self.force_stop_plan.obstacle_m, action_t) + candidates.append((stop_target, LongitudinalPlanSource.cruise, False)) if sm['selfdriveState'].experimentalMode: candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)) @@ -510,5 +514,9 @@ class LongitudinalPlanner: hold.modelMonoTime = longitudinalPlan.modelMonoTime hold.driveStartMonoTime = self.plan_drive_id hold.forceStopHolding = bool(self.force_stop_plan.manual_hold) + hold.forceStopBraking = bool(self.ioniq6_throttle_gate_enabled and self.force_stop_plan.forcing and + self.force_stop_plan.should_stop) + hold.forceStopEpisodeMonoTime = self.force_stop_plan.episode_ns pm.send('longitudinalPlan', plan_send) pm.send('starpilotLongitudinalPlan', companion) + diff --git a/openpilot/starpilot/longitudinal/extension.py b/openpilot/starpilot/longitudinal/extension.py index f9f428c2e1..27901ef8bb 100644 --- a/openpilot/starpilot/longitudinal/extension.py +++ b/openpilot/starpilot/longitudinal/extension.py @@ -24,6 +24,7 @@ class LongitudinalContext: custom_acceleration: bool | None = None profile_max_accel: float | None = None faster_takeoffs: bool = False + force_stop_braking: bool = False class LongitudinalExtension: @@ -164,3 +165,4 @@ def create_extension(cp, startup_preferences=None): not extension.ignore_cruise_standstill): return None return extension + diff --git a/openpilot/starpilot/longitudinal/force_stop.py b/openpilot/starpilot/longitudinal/force_stop.py index 488cfbddf5..ff1e5f0247 100644 --- a/openpilot/starpilot/longitudinal/force_stop.py +++ b/openpilot/starpilot/longitudinal/force_stop.py @@ -4,6 +4,7 @@ from dataclasses import dataclass import math from openpilot.common.realtime import DT_MDL +from openpilot.starpilot.longitudinal.force_stop_braking import STOP_RESERVE_M STANDSTILL_HOLD_NS = 2_000_000_000 STANDSTILL_CLEAR_NS = 750_000_000 @@ -22,9 +23,12 @@ class StopTune: jerk_scale: float = 0.32 reanchor_speed_tolerance: float | None = None low_speed_hold_mps: float | None = None + distance_control: bool = False def vehicle_tune(fingerprint: str) -> StopTune: + if fingerprint == 'HYUNDAI_IONIQ_6': + return StopTune(distance_control=True) if fingerprint == 'TOYOTA_CAMRY_TSS2': return StopTune(handoff_m=4.5, distance_bias_m=6.0) if fingerprint == 'HYUNDAI_ELANTRA_2021': @@ -72,6 +76,7 @@ class StopPlan: takeoff_light_observed: bool = False takeoff_light: bool = False resume_override: bool = False + episode_ns: int = 0 class ForceStop: @@ -96,6 +101,8 @@ class ForceStop: self.previously_enabled = False self.distance = 0.0 self.distance_cap = 0.0 + self.episode_ns = 0 + self.stop_requested = False def _clear_standstill(self): self.standstill_since_ns = 0 @@ -208,6 +215,7 @@ class ForceStop: offset = max(-20, min(20, offset_ft)) * .3048 approach = 0.0 ceiling = None + native_stop = False if force and not overridden: if self.entry_speed is None and not frame.standstill: self.entry_speed = speed @@ -215,8 +223,11 @@ class ForceStop: if held: self.distance = 0 ceiling = 0.0 + native_stop = True else: - self.distance = max(0.0, self.distance - speed * dt) + self.distance -= speed * dt + if not tune.distance_control: + self.distance = max(0.0,self.distance) if (available and not frame.sign and self.distance > max(tune.handoff_m, 40) and not frame.model_should_stop and horizon > self.distance + 3 and (tune.reanchor_speed_tolerance is None or self.entry_speed is None or speed >= self.entry_speed - tune.reanchor_speed_tolerance)): @@ -231,6 +242,9 @@ class ForceStop: effective = self.distance + offset + tune.distance_bias_m decel = SIGN_APPROACH_DECEL if frame.sign else MODEL_APPROACH_DECEL ceiling = math.sqrt(2 * decel * max(0, effective - tune.handoff_m)) + native_stop = ceiling <= .5 + if tune.distance_control: + ceiling = math.sqrt(2 * decel * max(0, effective - STOP_RESERVE_M)) else: self.forcing = False self.entry_speed = None @@ -243,12 +257,21 @@ class ForceStop: if effective > tune.handoff_m: ceiling = math.sqrt(2 * MODEL_APPROACH_DECEL * (effective - tune.handoff_m)) self.previously_enabled = frame.enabled + self.stop_requested = bool(self.forcing and (self.stop_requested or native_stop)) + if self.forcing and not self.episode_ns: + self.episode_ns = now + elif not self.forcing: + self.episode_ns = 0 stop_length = self.distance if self.forcing else approach # The MPC obstacle includes its standing-distance term; adding it at the # native boundary avoids moving the actual stop line by that buffer. obstacle = (stop_length * .93 + offset + tune.distance_bias_m if stop_length > 0 and stop_length + offset > tune.handoff_m else None) - return StopPlan(now, ceiling, obstacle, self.forcing, bool(self.forcing and ceiling is not None and ceiling <= .5), - self.distance if self.forcing else 0.0, approach, + if tune.distance_control and self.forcing: + obstacle = max(0.0, self.distance * .93 + offset + tune.distance_bias_m) + should_stop = self.stop_requested if tune.distance_control else bool(self.forcing and ceiling is not None and ceiling <= .5) + return StopPlan(now, ceiling, obstacle, self.forcing, should_stop, + max(0.0,self.distance) if self.forcing else 0.0, approach, tune.jerk_scale if self.forcing or approach > 0 else 1.0, bool(held and self.forcing and not overridden), - resume_override=now < self.resume_until_ns) + resume_override=now < self.resume_until_ns, episode_ns=self.episode_ns) + diff --git a/openpilot/starpilot/longitudinal/force_stop_braking.py b/openpilot/starpilot/longitudinal/force_stop_braking.py new file mode 100644 index 0000000000..e1af20af49 --- /dev/null +++ b/openpilot/starpilot/longitudinal/force_stop_braking.py @@ -0,0 +1,39 @@ +"""Ioniq 6 distance braking under the existing Force Stop stop/hold intent.""" + +import math + +MAX_AGE_NS = 150_000_000 +STOP_RESERVE_M = .5 +MIN_ACCEL = -3.5 + + +def distance_target(speed, acceleration, distance, delay): + if (not all(math.isfinite(x) for x in (speed, acceleration, distance, delay)) or + speed < 0 or distance < 0 or not 0 <= delay <= 2): + return MIN_ACCEL + remaining = distance - speed * delay - .5 * max(acceleration, 0.) * delay**2 - STOP_RESERVE_M + future_speed = speed + max(acceleration, 0.) * delay + return max(MIN_ACCEL, -future_speed**2 / (2 * max(remaining, .01))) + + +def current_braking(sm, now_ns): + try: + device, plan, stop = (sm[name] for name in ('deviceState', 'longitudinalPlan', 'starpilotLongitudinalPlan')) + drive = int(device.startedMonoTime) + if not device.started or not 0 < drive < now_ns or not plan.shouldStop: + return False + for name in ('carState', 'deviceState', 'longitudinalPlan', 'starpilotLongitudinalPlan'): + stamp, receipt = int(sm.logMonoTime[name]), int(sm.recv_time[name] * 1e9) + maximum = 1_250_000_000 if name == 'deviceState' else MAX_AGE_NS + if (not sm.seen[name] or not sm.valid[name] or not sm.alive[name] or + not drive < stamp <= receipt <= now_ns or now_ns - stamp > maximum): + return False + return bool(stop.version == 1 and stop.forceStopBraking and stop.driveStartMonoTime == drive and + drive < stop.forceStopEpisodeMonoTime <= stop.modelMonoTime <= stop.sourcePlanMonoTime <= now_ns and + stop.sourcePlanMonoTime == sm.logMonoTime['longitudinalPlan'] and + stop.sourcePlanMonoTime == sm.logMonoTime['starpilotLongitudinalPlan'] and + stop.modelMonoTime == plan.modelMonoTime and now_ns - stop.modelMonoTime <= MAX_AGE_NS and + sm['carState'].canValid and not sm['carState'].canTimeout and + not sm['carState'].brakePressed and not sm['carState'].gasPressed) + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return False diff --git a/openpilot/starpilot/longitudinal/inputs.py b/openpilot/starpilot/longitudinal/inputs.py index 33c947ce30..92e8323be8 100644 --- a/openpilot/starpilot/longitudinal/inputs.py +++ b/openpilot/starpilot/longitudinal/inputs.py @@ -131,6 +131,8 @@ class LongitudinalInputs: @property def optional_services(self): + if self.ioniq6_start_enabled: + return ['radarState', 'deviceState', 'starpilotLongitudinalPlan'] if (self.toyota_sienna_replay or self.toyota_sienna_production or self.toyota_corolla_stop or self.ioniq6_start_enabled or self.gm_start_enabled or self.gm_volt_enabled or self.gm_euv_enabled or self.gm_cc_enabled or self.gm_cc_stop_enabled or self.gm_ascm_enabled or self.gm_suburban_enabled): @@ -151,6 +153,9 @@ class LongitudinalInputs: def context(self, active): context = self._vehicle_context(active) + if active and self.ioniq6_start_enabled: + from openpilot.starpilot.longitudinal.force_stop_braking import current_braking + context = replace(context, force_stop_braking=current_braking(self.sm, time.monotonic_ns())) self.faster_takeoffs = bool(self.takeoff_preferences is not None and self.takeoff_preferences.sample(time.monotonic_ns())) if not self.faster_takeoffs: @@ -424,3 +429,4 @@ class LongitudinalInputs: return None return PedalStartEvidence(drive_id, now_ns, any(lead.present for lead in leads), traffic_mode=traffic_mode, custom_acceleration=profile.custom_acceleration if profile is not None else False, profile_max_accel=ceiling) + diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop.py b/openpilot/starpilot/longitudinal/tests/test_force_stop.py index 664b310cfd..553a74527d 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop.py +++ b/openpilot/starpilot/longitudinal/tests/test_force_stop.py @@ -15,11 +15,46 @@ def frame(tick, **changes): class ForceStopTests(unittest.TestCase): + def test_ioniq_positive_offset_continues_to_consume_remaining_distance(self): + owner = ForceStop() + tune = vehicle_tune('HYUNDAI_IONIQ_6') + for tick in range(100): + plan = owner.step(frame(tick,speed_mps=4.,horizon_m=10.),tune,8) + self.assertTrue(plan.should_stop) + self.assertEqual(plan.tracked_distance_m,0.) + self.assertEqual(plan.obstacle_m,0.) + + def test_ioniq_keeps_rolling_goal_and_stop_intent_separate(self): + owner = ForceStop() + tune = vehicle_tune('HYUNDAI_IONIQ_6') + for tick in range(35): + plan = owner.step(frame(tick,speed_mps=4.,horizon_m=10.),tune) + if plan.forcing and plan.tracked_distance_m < 6: + self.assertTrue(plan.should_stop) + self.assertGreater(plan.speed_ceiling_mps,.5) + self.assertIsNotNone(plan.obstacle_m) + self.assertTrue(plan.should_stop) + + def test_episode_is_retained_until_release_and_new_commit_gets_new_identity(self): + owner = ForceStop() + tune = vehicle_tune('HYUNDAI_IONIQ_6') + for tick in range(20): + plan = owner.step(frame(tick),tune) + episode = plan.episode_ns + self.assertGreater(episode,BASE) + for tick in range(20,30): + self.assertEqual(owner.step(frame(tick),tune).episode_ns,episode) + self.assertEqual(owner.step(frame(30,pedal_override=True),tune).episode_ns,0) + for tick in range(31,260): + plan = owner.step(frame(tick),tune) + self.assertGreater(plan.episode_ns,episode) + def setUp(self): self.owner = ForceStop() self.tune = vehicle_tune('HYUNDAI_IONIQ_6') def test_original_approach_commit_kinematics_and_obstacle(self): + self.tune = vehicle_tune('OTHER') for tick in range(40): plan = self.owner.step(frame(tick, model_should_stop=True), self.tune) # The existing 0.5s accumulator crosses .5 on frame 10 with binary floats. @@ -260,6 +295,7 @@ class ForceStopTests(unittest.TestCase): self.assertLessEqual(plan.tracked_distance_m, cap - .5 + 15) def test_offset_and_handoff_keep_native_should_stop(self): + self.tune = vehicle_tune('OTHER') for offset in (-20, 0, 20): with self.subTest(offset=offset): self.owner.reset() diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop_braking.py b/openpilot/starpilot/longitudinal/tests/test_force_stop_braking.py new file mode 100644 index 0000000000..920ec95c83 --- /dev/null +++ b/openpilot/starpilot/longitudinal/tests/test_force_stop_braking.py @@ -0,0 +1,125 @@ +from types import SimpleNamespace as NS +from unittest import TestCase +import math +import tempfile +from unittest.mock import patch + +from openpilot.starpilot.longitudinal.force_stop_braking import current_braking, distance_target + +BASE = 100_000_000_000 +NOW = BASE+1_000_000_000 + + +def inputs(): + class Messages(dict): + pass + sm = Messages(deviceState=NS(started=True,startedMonoTime=BASE), + carState=NS(canValid=True,canTimeout=False,brakePressed=False,gasPressed=False), + longitudinalPlan=NS(shouldStop=True,modelMonoTime=NOW-20_000_000), + starpilotLongitudinalPlan=NS(version=1,forceStopBraking=True,driveStartMonoTime=BASE, + forceStopEpisodeMonoTime=BASE+1,modelMonoTime=NOW-20_000_000,sourcePlanMonoTime=NOW-10_000_000)) + sm.logMonoTime = dict.fromkeys(sm,NOW-10_000_000) + sm.recv_time = dict.fromkeys(sm,(NOW-5_000_000)/1e9) + sm.valid = dict.fromkeys(sm,True) + sm.alive = dict.fromkeys(sm,True) + sm.seen = dict.fromkeys(sm,True) + return sm + + +class ForceStopBrakingTests(TestCase): + def test_only_current_matching_stop_intent_qualifies(self): + self.assertTrue(current_braking(inputs(),NOW)) + for field,value in [('version',0),('forceStopBraking',False),('driveStartMonoTime',BASE-1), + ('forceStopEpisodeMonoTime',BASE),('forceStopEpisodeMonoTime',NOW+1), + ('modelMonoTime',NOW-21_000_000),('sourcePlanMonoTime',NOW-11_000_000)]: + with self.subTest(field=field,value=value): + sm = inputs() + setattr(sm['starpilotLongitudinalPlan'],field,value) + self.assertFalse(current_braking(sm,NOW)) + + def test_withdrawal_override_or_stale_source_keeps_original_stop(self): + for field in ['brakePressed','gasPressed','canTimeout']: + sm = inputs() + setattr(sm['carState'],field,True) + self.assertFalse(current_braking(sm,NOW)) + for name in inputs(): + for bad in ['seen','valid','alive']: + sm = inputs() + getattr(sm,bad)[name] = False + self.assertFalse(current_braking(sm,NOW)) + sm = inputs() + sm['longitudinalPlan'].shouldStop = False + self.assertFalse(current_braking(sm,NOW)) + self.assertFalse(current_braking(inputs(),NOW+2_000_000_000)) + + def test_distance_target_strengthens_with_less_room_or_more_latency(self): + self.assertLess(distance_target(4.,0.,6.,.5),distance_target(4.,0.,12.,.5)) + self.assertLess(distance_target(4.,0.,12.,.5),distance_target(4.,0.,12.,.1)) + self.assertLessEqual(distance_target(4.,1.,12.,.5),distance_target(4.,0.,12.,.5)) + self.assertEqual(distance_target(4.,0.,0.,.5),-3.5) + self.assertEqual(distance_target(4.,float('nan'),12.,.5),-3.5) + self.assertEqual(distance_target(4.,0.,12.,-1.),-3.5) + self.assertEqual(distance_target(0.,0.,0.,.5),0.) + + +class NativeForceStopBrakingTests(TestCase): + def test_actual_inputs_control_hold_and_can_request(self): + from openpilot.cereal import messaging + from openpilot.common.params import Params + from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState + from openpilot.starpilot.longitudinal.inputs import LongitudinalInputs + from openpilot.starpilot.longitudinal.tests.test_ioniq6_start import candidate, car_state + from opendbc.can.packer import CANPacker + from opendbc.car.hyundai.hyundaicanfd import create_acc_control + from opendbc.car.hyundai.ioniq6_longitudinal import Ioniq6LongitudinalPolicy + + stock,cp = candidate() + sm = inputs() + sm.all_checks = lambda names: all(sm.valid.get(name,False) for name in names) + stop = messaging.new_message('starpilotLongitudinalPlan',valid=True) + stop.logMonoTime = NOW-10_000_000 + for key,value in vars(sm['starpilotLongitudinalPlan']).items(): + setattr(stop.starpilotLongitudinalPlan,key,value) + plan = messaging.new_message('longitudinalPlan',valid=True) + plan.logMonoTime = NOW-10_000_000 + plan.longitudinalPlan.modelMonoTime = NOW-20_000_000 + plan.longitudinalPlan.shouldStop = True + sm['starpilotLongitudinalPlan'] = messaging.log_from_bytes(stop.to_bytes()).starpilotLongitudinalPlan + sm['longitudinalPlan'] = messaging.log_from_bytes(plan.to_bytes()).longitudinalPlan + car = car_state(speed=4.,standstill=False) + sm['carState'] = car + with tempfile.TemporaryDirectory() as directory: + reader = LongitudinalInputs(cp,Params(directory),lambda:sm) + self.assertIn('starpilotLongitudinalPlan',reader.optional_services) + with patch('openpilot.starpilot.longitudinal.inputs.time.monotonic_ns',return_value=NOW): + context = reader.context(True) + self.assertTrue(context.force_stop_braking) + control = LongControl(cp) + control.long_control_state = LongCtrlState.pid + control.last_output_accel = -1.5 + result = control.update(True,car,-.8,True,(-3.5,2.),context=context) + self.assertAlmostEqual(result,-.8) + self.assertEqual(control.long_control_state,LongCtrlState.stopping) + strong = control.update(True,car,-3.,True,(-3.5,2.),context=context) + self.assertEqual(strong,-3.) + self.assertTrue(math.isfinite(control.update(True,car,math.nan,True,(-3.5,2.),context=context))) + with patch('openpilot.starpilot.longitudinal.inputs.time.monotonic_ns',return_value=NOW+200_000_000): + stale = reader.context(True) + self.assertFalse(stale.force_stop_braking) + control.last_output_accel = -.8 + self.assertLess(control.update(True,car,-.8,True,(-3.5,2.),context=stale),-.8) + stopped = car_state() + control.last_output_accel = -1.5 + self.assertLess(control.update(True,stopped,0.,True,(-3.5,2.),context=context),-1.5) + policy = Ioniq6LongitudinalPolicy() + request = policy.update(0,active=True,override=False,accel=-.8,speed=4.,measured_accel=-1.5, + control_state=LongCtrlState.stopping,last_sent_accel=-1.5) + packet = create_acc_control(CANPacker('hyundai_canfd_generated'),NS(ECAN=1),True,-1.5,request.accel, + request.stopping,False,40.,NS(leadDistanceBars=2),direct_accel=True) + bits = int.from_bytes(packet[1],'little') + self.assertEqual((bits>>184)&1,1) + self.assertEqual((bits>>68)&7,1) + normal = LongControl(stock) + normal.long_control_state = LongCtrlState.pid + normal.last_output_accel = -1.5 + self.assertLess(normal.update(True,car,-.8,True,(-3.5,2.),context=context),-1.5) diff --git a/prebuilt b/prebuilt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/prebuilt.json b/prebuilt.json deleted file mode 100644 index fcda3bb8ca..0000000000 --- a/prebuilt.json +++ /dev/null @@ -1 +0,0 @@ -{"protected_inventory_sha256": "0a96cfa499ada974607b1255975f78be11f09ce5f3d1904e2a27598ca08e3d07", "schema_version": 1} diff --git a/tools/ci/tests/test_schema_policy.py b/tools/ci/tests/test_schema_policy.py index f6b8183b31..32a1146f1f 100644 --- a/tools/ci/tests/test_schema_policy.py +++ b/tools/ci/tests/test_schema_policy.py @@ -270,7 +270,7 @@ class SchemaPolicyTest(unittest.TestCase): original = b'forceStopHolding @4 :Bool;' self.assertIn(original, self.schemas[path]) appended = self.schemas.copy() - appended[path] = appended[path].replace(original, original + b'\n futureStatus @5 :UInt32;') + appended[path] = appended[path].replace(original, original + b'\n futureStatus @7 :UInt32;') self.assertEqual(validate(appended, self.policy, self.sync), []) for replacement in (b'', b'forceStopHolding @5 :Bool;', b'forceStopHolding @4 :UInt32;', b'changedHold @4 :Bool;'): @@ -474,3 +474,4 @@ class SchemaPolicyTest(unittest.TestCase): changed = self.schemas.copy() changed[path] = changed[path].replace(original, original + b'\n duplicate @12 :Text;', 1) self.assertTrue(any('Duplicate member name or ordinal' in error for error in validate(changed, self.policy, self.sync))) + From 9f45afd3ec75a62a83c04195218deb0b227efcf7 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 10 Nov 2026 16:01:29 -0600 Subject: [PATCH 3/4] Track moving Force Stop previews and ease requested braking release --- .../car/hyundai/ioniq6_longitudinal.py | 8 +- .../hyundai/tests/test_ioniq6_longitudinal.py | 44 ++++++ .../starpilot/longitudinal/force_stop.py | 15 ++- .../longitudinal/force_stop_runtime.py | 10 +- .../longitudinal/tests/test_force_stop.py | 70 ++++++++++ .../tests/test_force_stop_runtime.py | 15 +++ tools/ci/custom-schema-contract.json | 125 +++++++++++++++++- 7 files changed, 280 insertions(+), 7 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/ioniq6_longitudinal.py b/opendbc_repo/opendbc/car/hyundai/ioniq6_longitudinal.py index 915f5d38c4..c8253c493e 100644 --- a/opendbc_repo/opendbc/car/hyundai/ioniq6_longitudinal.py +++ b/opendbc_repo/opendbc/car/hyundai/ioniq6_longitudinal.py @@ -60,7 +60,11 @@ def update_calibration(state: Ioniq6LongitudinalState, accel: float, speed: floa elif starting or state.launch_active or (state.long_control_state_last == LongState.starting and control_state == LongState.pid): state.launch_active = True + hold_jerk = float(np.interp(speed, (0.0, 0.15, 0.6, 1.2, 2.0, 3.0), (0.35, 0.40, 0.48, 0.65, 0.85, 1.10))) * RESPONSE if state.stopping else 0.0 upper_limit = float(np.interp(speed, (0.0, 5.0, 20.0), (2.0, 3.0, 2.0))) * RESPONSE if control_state == LongState.pid else MIN_JERK + if state.stopping and accel > state.accel_last: + # Follow an already gentler planner request without weakening stronger braking. + upper_limit = hold_jerk lower_limit = float(np.interp(speed, (0.0, 5.0, 20.0), (5.0, 3.5, 3.0))) * RESPONSE horizon = float(np.interp(speed, LOOKAHEAD_SPEEDS, LOOKAHEAD_TIMES)) desired_jerk = float(np.clip((accel - state.accel_last) / horizon, -MAX_JERK, MAX_JERK)) @@ -70,10 +74,10 @@ def update_calibration(state: Ioniq6LongitudinalState, accel: float, speed: floa state.jerk_lower = min(lower_jerk, lower_limit) if state.stopping: - if speed <= 2.0: + # A saturated braking request must survive the comfort soft-stop cap. + if speed <= 2.0 and (speed <= .01 or accel > CarControllerParams.ACCEL_MIN): brake_cap = float(np.interp(speed, (0.0, 0.08, 0.25, 0.6, 1.2, 2.0, 3.0), (-0.15, -0.16, -0.22, -0.42, -0.78, -1.15, -1.40))) state.desired_accel = min(0.0, max(accel, brake_cap)) - hold_jerk = float(np.interp(speed, (0.0, 0.15, 0.6, 1.2, 2.0, 3.0), (0.35, 0.40, 0.48, 0.65, 0.85, 1.10))) * RESPONSE state.jerk_upper = min(state.jerk_upper, hold_jerk) else: state.desired_accel = float(np.clip(accel, CarControllerParams.ACCEL_MIN, 0.0)) diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_ioniq6_longitudinal.py b/opendbc_repo/opendbc/car/hyundai/tests/test_ioniq6_longitudinal.py index 9d15113c2e..bb1d9c9cb3 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_ioniq6_longitudinal.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_ioniq6_longitudinal.py @@ -61,6 +61,36 @@ class TestIoniq6Longitudinal(unittest.TestCase): for value, previous in zip(actual, expected, strict=True): self.assertAlmostEqual(value, previous, places=12) + def test_saturated_stopping_brake_survives_low_speed_comfort_cap(self): + for speed in (.1, .5, 1., 2.): + with self.subTest(speed=speed): + policy = Ioniq6LongitudinalPolicy() + last_sent = 0. + for frame in range(600): + result = policy.update(frame, active=True, override=False, accel=-3.5, speed=speed, + measured_accel=last_sent, control_state=LongState.stopping, + last_sent_accel=last_sent) + self.assertTrue(result.stopping) + self.assertGreaterEqual(result.accel, -3.5) + if frame % 2 == 0: + self.assertLessEqual(result.accel, last_sent) + self.assertGreaterEqual(result.accel + 1e-12, last_sent - .36) + last_sent = result.accel + self.assertAlmostEqual(last_sent, -3.5) + refused = policy.update(601, active=True, override=True, accel=-3.5, speed=speed, + measured_accel=last_sent, control_state=LongState.stopping, + last_sent_accel=last_sent) + self.assertEqual(refused.accel, 0.) + self.assertFalse(refused.stopping) + self.assertEqual(policy.state, Ioniq6LongitudinalState()) + + def test_saturated_request_returns_to_soft_hold_at_standstill(self): + state = Ioniq6LongitudinalState(accel_last=-3.5) + update_calibration(state, -3.5, 0., 0., LongState.stopping) + self.assertAlmostEqual(state.desired_accel, -.15) + self.assertTrue(state.stopping) + self.assertGreater(state.actual_accel, -3.5) + def test_soft_target_revokes_launch_on_offcycle_and_scheduled_ticks(self): for drop_frame in (31, 35): with self.subTest(drop_frame=drop_frame): @@ -323,6 +353,20 @@ class TestIoniq6Longitudinal(unittest.TestCase): counters.append(frame[1][2]) self.assertEqual(counters, [counter & 0xff for counter in range(260)]) + def test_already_gentler_request_uses_bounded_stop_release(self): + state = Ioniq6LongitudinalState(accel_last=-1.5) + update_calibration(state, -.5, 3., -1.5, LongState.stopping) + self.assertGreater(state.actual_accel, -1.47) + self.assertLessEqual(state.actual_accel, -.5) + self.assertLessEqual(state.jerk_upper, 1.32) + self.assertTrue(state.stopping) + + def test_stop_release_does_not_weaken_independent_braking_above_soft_cap(self): + state = Ioniq6LongitudinalState(accel_last=-3.2) + update_calibration(state, -3.2, 2.5, -3.2, LongState.stopping) + self.assertEqual(state.desired_accel, -3.2) + self.assertEqual(state.actual_accel, -3.2) + def test_stock_ioniq_and_other_cars_do_not_install_this_policy(self): for car in (CAR.HYUNDAI_IONIQ_6, CAR.HYUNDAI_IONIQ_5, CAR.KIA_EV6): cp = CarInterface.get_non_essential_params(car) diff --git a/openpilot/starpilot/longitudinal/force_stop.py b/openpilot/starpilot/longitudinal/force_stop.py index ff1e5f0247..26084b43e5 100644 --- a/openpilot/starpilot/longitudinal/force_stop.py +++ b/openpilot/starpilot/longitudinal/force_stop.py @@ -60,6 +60,7 @@ class StopFrame: resume_requested: bool = False perception_available: bool = True sign: bool = False + moving_preview: bool = False @dataclass(frozen=True) @@ -103,6 +104,7 @@ class ForceStop: self.distance_cap = 0.0 self.episode_ns = 0 self.stop_requested = False + self.preview_tracking = False def _clear_standstill(self): self.standstill_since_ns = 0 @@ -219,6 +221,7 @@ class ForceStop: if force and not overridden: if self.entry_speed is None and not frame.standstill: self.entry_speed = speed + self.preview_tracking = tune.distance_control and frame.moving_preview self.forcing |= not frame.standstill or held if held: self.distance = 0 @@ -236,6 +239,12 @@ class ForceStop: self.distance = min(self.distance, horizon) self.distance_cap = max(0.0, self.distance_cap - speed * dt) self.distance = min(self.distance, self.distance_cap + 15 * min(self.distance_cap / 60, 1)) + if available and (not frame.moving_preview or frame.model_should_stop or self.sign_confirmed): + self.preview_tracking = False + if (self.preview_tracking and available and frame.moving_preview and not self.stop_requested and + not self.sign_confirmed and not frame.model_should_stop): + # A moving model preview is not a fixed world endpoint to count down. + self.distance = self.distance_cap = horizon if frame.sign and available: if horizon < SIGN_MODEL_AGREE_M: self.distance = min(self.distance, SIGN_SEED_M) @@ -248,6 +257,7 @@ class ForceStop: else: self.forcing = False self.entry_speed = None + self.preview_tracking = False self.sign_confirmed = False self._clear_standstill() self.distance = self.distance_cap = horizon if available else 0.0 @@ -266,9 +276,10 @@ class ForceStop: # The MPC obstacle includes its standing-distance term; adding it at the # native boundary avoids moving the actual stop line by that buffer. obstacle = (stop_length * .93 + offset + tune.distance_bias_m - if stop_length > 0 and stop_length + offset > tune.handoff_m else None) + if stop_length > 0 and stop_length + max(0.0,offset) > tune.handoff_m else None) if tune.distance_control and self.forcing: - obstacle = max(0.0, self.distance * .93 + offset + tune.distance_bias_m) + obstacle = max(0.0,min(self.distance * .93 + offset + tune.distance_bias_m, + (self.distance + offset + tune.distance_bias_m) * .93)) should_stop = self.stop_requested if tune.distance_control else bool(self.forcing and ceiling is not None and ceiling <= .5) return StopPlan(now, ceiling, obstacle, self.forcing, should_stop, max(0.0,self.distance) if self.forcing else 0.0, approach, diff --git a/openpilot/starpilot/longitudinal/force_stop_runtime.py b/openpilot/starpilot/longitudinal/force_stop_runtime.py index 720d5345d2..8a1e7eb661 100644 --- a/openpilot/starpilot/longitudinal/force_stop_runtime.py +++ b/openpilot/starpilot/longitudinal/force_stop_runtime.py @@ -1,6 +1,7 @@ """Current-drive Force Stop owner with bounded preference reads and live scene inputs.""" from dataclasses import replace +import math from openpilot.starpilot.conditional_mode.curve import frozen_raw_curve from openpilot.starpilot.conditional_mode.preferences import CEMOptions, decode_preferences, PreferenceError @@ -13,6 +14,7 @@ from openpilot.starpilot.longitudinal.force_stop import ForceStop, StopFrame, St from openpilot.starpilot.model_geometry import road_curvature, standstill_clear_horizon, standstill_release_speed from openpilot.starpilot.saved_source import read_saved from openpilot.starpilot.longitudinal.stop_resume import StopResume +from openpilot.starpilot.longitudinal.ioniq6_start import eligible as ioniq6_long_eligible from openpilot.starpilot.speed_limits.vision.stop_observation import CONTROL_STOP_MAX_AGE_NS, StopConfirmation, read_stop_evidence @@ -191,8 +193,12 @@ class ForceStopRuntime: bool(car.leftBlinker or car.rightBlinker), float(car.steeringAngleDeg), resume_requested=resume_requested, perception_available=perception_available, sign=dashboard_stop_sign(sm, now_ns=now_ns, drive_id=drive_id) or - bool(vision_sign and 0 <= now_ns - vision_sign <= CONTROL_STOP_MAX_AGE_NS)) - self.plan = self.policy.step(frame, vehicle_tune(str(cp.carFingerprint)), self.offset) + bool(vision_sign and 0 <= now_ns - vision_sign <= CONTROL_STOP_MAX_AGE_NS), + moving_preview=bool(perception_available and not model.action.shouldStop and + len(model.velocity.x) == len(model.position.x) and + math.isfinite(model.velocity.x[-1]) and model.velocity.x[-1] > .3)) + tune = replace(vehicle_tune(str(cp.carFingerprint)), distance_control=ioniq6_long_eligible(cp)) + self.plan = self.policy.step(frame, tune, self.offset) if takeoff_enabled: self.plan = replace(self.plan, takeoff_light_observed=takeoff_observed, takeoff_light=takeoff_light) return self.plan diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop.py b/openpilot/starpilot/longitudinal/tests/test_force_stop.py index 553a74527d..8dc63b93c4 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop.py +++ b/openpilot/starpilot/longitudinal/tests/test_force_stop.py @@ -15,6 +15,34 @@ def frame(tick, **changes): class ForceStopTests(unittest.TestCase): + def test_negative_offset_keeps_the_cold_approach_obstacle(self): + for vehicle in ['OTHER','HYUNDAI_IONIQ_6']: + plan = ForceStop().step(frame(0,speed_mps=8.,horizon_m=10.),vehicle_tune(vehicle),-20) + self.assertFalse(plan.forcing) + assert plan.obstacle_m is not None + self.assertAlmostEqual(plan.obstacle_m,10*.93-20*.3048) + + def test_ioniq_negative_offset_keeps_the_existing_conservative_obstacle(self): + owner = ForceStop() + owner.last_model_ns = BASE+10*TICK + owner.distance = 10. + owner.distance_cap = 10. + owner.forcing = True + owner.timer = 1. + plan = owner.step(frame(11,speed_mps=4.,horizon_m=10.),vehicle_tune('HYUNDAI_IONIQ_6'),-20) + assert plan.obstacle_m is not None + self.assertAlmostEqual(plan.obstacle_m,9.8*.93-20*.3048) + + def test_ioniq_offset_moves_the_endpoint_without_rescaling_the_offset(self): + owner = ForceStop() + owner.last_model_ns = BASE+10*TICK + owner.distance = -1. + owner.forcing = True + owner.timer = 1. + plan = owner.step(frame(11,speed_mps=4.,horizon_m=10.),vehicle_tune('HYUNDAI_IONIQ_6'),8) + assert plan.obstacle_m is not None + self.assertAlmostEqual(plan.obstacle_m,(-1.2+8*.3048)*.93) + def test_ioniq_positive_offset_continues_to_consume_remaining_distance(self): owner = ForceStop() tune = vehicle_tune('HYUNDAI_IONIQ_6') @@ -31,6 +59,7 @@ class ForceStopTests(unittest.TestCase): plan = owner.step(frame(tick,speed_mps=4.,horizon_m=10.),tune) if plan.forcing and plan.tracked_distance_m < 6: self.assertTrue(plan.should_stop) + assert plan.speed_ceiling_mps is not None self.assertGreater(plan.speed_ceiling_mps,.5) self.assertIsNotNone(plan.obstacle_m) self.assertTrue(plan.should_stop) @@ -349,3 +378,44 @@ if __name__ == '__main__': self.owner.reset() first = self.owner.step(frame(0, speed_mps=0., horizon_m=30., standstill=True, raw_model_stopped=True, resume_requested=True), self.tune) self.assertFalse(first.resume_override) + + +def preview_frame(tick, **changes): + return replace(frame(tick, speed_mps=4., horizon_m=30., raw_model_stopped=True), **changes) + +class MovingPreviewTests(unittest.TestCase): + def test_moving_preview_does_not_become_an_odometry_countdown(self): + owner=ForceStop() + for tick in range(180):plan=owner.step(preview_frame(tick,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertTrue(plan.forcing);self.assertFalse(plan.should_stop);self.assertEqual(plan.tracked_distance_m,30.) + def test_both_fresh_horizons_extend_the_rolling_preview(self): + for horizon in [24.59375,32.34375]: + owner=ForceStop();owner.last_model_ns=BASE;owner.distance=owner.distance_cap=6.19;owner.forcing=True;owner.timer=1.;owner.entry_speed=4.;owner.preview_tracking=True + plan=owner.step(preview_frame(1,horizon_m=horizon,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertFalse(plan.should_stop);self.assertEqual(plan.tracked_distance_m,horizon) + def test_terminal_stop_cannot_reopen(self): + owner=ForceStop() + for tick in range(15):plan=owner.step(preview_frame(tick,horizon_m=5.,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertTrue(plan.should_stop);before=plan.tracked_distance_m + plan=owner.step(preview_frame(15,horizon_m=35.,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertTrue(plan.should_stop);self.assertLess(plan.tracked_distance_m,before) + def test_missing_geometry_keeps_last_preview_and_odometry(self): + owner=ForceStop() + for tick in range(15):before=owner.step(preview_frame(tick,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) + after=owner.step(preview_frame(15,horizon_m=None,road_curvature=None,perception_available=False,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertAlmostEqual(after.tracked_distance_m,before.tracked_distance_m-.2) + def test_confirmed_stop_and_sign_cannot_extend(self): + for kw in [{'model_should_stop':True},{'sign':True}]: + owner=ForceStop();owner.last_model_ns=BASE;owner.distance=owner.distance_cap=8.;owner.forcing=True;owner.timer=1. + after=owner.step(preview_frame(1,horizon_m=35.,moving_preview=True,**kw),vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertLess(after.tracked_distance_m,8.) + def test_once_stationary_endpoint_seen_it_cannot_become_a_moving_preview_again(self): + owner=ForceStop() + for tick in range(15):owner.step(preview_frame(tick,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) + fixed=owner.step(preview_frame(15,horizon_m=20.,moving_preview=False),vehicle_tune('HYUNDAI_IONIQ_6')) + reopened=owner.step(preview_frame(16,horizon_m=35.,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertLess(reopened.tracked_distance_m,fixed.tracked_distance_m) + def test_other_profiles_retain_existing_distance_policy(self): + owner=ForceStop() + for tick in range(180):plan=owner.step(preview_frame(tick,moving_preview=True),vehicle_tune('OTHER')) + self.assertTrue(plan.should_stop);self.assertLess(plan.tracked_distance_m,6.) diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py b/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py index c4dc90b5b5..f7cda43af1 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py +++ b/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py @@ -61,6 +61,21 @@ class ForceStopRuntimeTests(unittest.TestCase): self.assertTrue(output.forcing) return output + def test_distance_braking_requires_exact_ioniq_control_profile(self): + from unittest.mock import patch + from openpilot.starpilot.longitudinal.tests.test_ioniq6_start import candidate + + stock, admitted = candidate() + stock.openpilotLongitudinalControl = True # An incomplete profile must still refuse the new tune. + for cp, expected in [(self.cp, False), (stock, False), (admitted, True)]: + self.cp = cp + self.owner = ForceStopRuntime(self.params) + with patch.object(self.owner.policy, 'step', wraps=self.owner.policy.step) as step: + for tick in range(12): + self.sample(Frame(tick)) + self.assertTrue(step.called) + self.assertEqual(step.call_args.args[1].distance_control, expected) + def test_committed_stop_survives_valid_mode_settings_and_traffic_changes(self): import json from openpilot.starpilot.conditional_mode.policy import ModeChoice diff --git a/tools/ci/custom-schema-contract.json b/tools/ci/custom-schema-contract.json index c93fe40977..8773bfed23 100644 --- a/tools/ci/custom-schema-contract.json +++ b/tools/ci/custom-schema-contract.json @@ -27,7 +27,8 @@ "FordLearningEvidence": "15123011525280866286", "FordLearningEvent": "12859710833724959940", "CentreApplicationWitness": "12325001751236334142", - "LearningTorqueProjection": "17327869370982877613" + "LearningTorqueProjection": "17327869370982877613", + "StarPilotLongitudinalPlan": "15750873059323825205" }, "nodes": { "14441422469354927087": { @@ -13433,6 +13434,128 @@ } } } + }, + "15750873059323825205": { + "kind": "struct", + "data_words": 5, + "pointers": 0, + "is_group": false, + "union_count": 0, + "union_offset": 0, + "fields": { + "0": { + "slot": { + "offset": 0, + "type": { + "uint16": null + }, + "defaultValue": { + "uint16": 0 + } + }, + "name": "version", + "discriminantValue": 65535, + "ordinal": { + "explicit": 0 + } + }, + "1": { + "slot": { + "offset": 1, + "type": { + "uint64": null + }, + "defaultValue": { + "uint64": 0 + } + }, + "name": "sourcePlanMonoTime", + "discriminantValue": 65535, + "ordinal": { + "explicit": 1 + } + }, + "2": { + "slot": { + "offset": 2, + "type": { + "uint64": null + }, + "defaultValue": { + "uint64": 0 + } + }, + "name": "modelMonoTime", + "discriminantValue": 65535, + "ordinal": { + "explicit": 2 + } + }, + "3": { + "slot": { + "offset": 3, + "type": { + "uint64": null + }, + "defaultValue": { + "uint64": 0 + } + }, + "name": "driveStartMonoTime", + "discriminantValue": 65535, + "ordinal": { + "explicit": 3 + } + }, + "4": { + "slot": { + "offset": 16, + "type": { + "bool": null + }, + "defaultValue": { + "bool": false + } + }, + "name": "forceStopHolding", + "discriminantValue": 65535, + "ordinal": { + "explicit": 4 + } + }, + "5": { + "slot": { + "offset": 17, + "type": { + "bool": null + }, + "defaultValue": { + "bool": false + } + }, + "name": "forceStopBraking", + "discriminantValue": 65535, + "ordinal": { + "explicit": 5 + } + }, + "6": { + "slot": { + "offset": 4, + "type": { + "uint64": null + }, + "defaultValue": { + "uint64": 0 + } + }, + "name": "forceStopEpisodeMonoTime", + "discriminantValue": 65535, + "ordinal": { + "explicit": 6 + } + } + } } } } From 717b8f93c6d42656e8362ac028390547559bc2f9 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 10 Nov 2026 16:28:02 -0600 Subject: [PATCH 4/4] Keep fixed Force Stop endpoints latched --- .../starpilot/longitudinal/force_stop.py | 2 +- .../longitudinal/tests/test_force_stop.py | 111 ++++++++++++------ 2 files changed, 75 insertions(+), 38 deletions(-) diff --git a/openpilot/starpilot/longitudinal/force_stop.py b/openpilot/starpilot/longitudinal/force_stop.py index 26084b43e5..1544cd6488 100644 --- a/openpilot/starpilot/longitudinal/force_stop.py +++ b/openpilot/starpilot/longitudinal/force_stop.py @@ -231,7 +231,7 @@ class ForceStop: self.distance -= speed * dt if not tune.distance_control: self.distance = max(0.0,self.distance) - if (available and not frame.sign and self.distance > max(tune.handoff_m, 40) and not frame.model_should_stop and + if (not tune.distance_control and available and not frame.sign and self.distance > max(tune.handoff_m, 40) and not frame.model_should_stop and horizon > self.distance + 3 and (tune.reanchor_speed_tolerance is None or self.entry_speed is None or speed >= self.entry_speed - tune.reanchor_speed_tolerance)): self.distance = horizon diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop.py b/openpilot/starpilot/longitudinal/tests/test_force_stop.py index 8dc63b93c4..5dc356ce68 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop.py +++ b/openpilot/starpilot/longitudinal/tests/test_force_stop.py @@ -379,43 +379,80 @@ if __name__ == '__main__': first = self.owner.step(frame(0, speed_mps=0., horizon_m=30., standstill=True, raw_model_stopped=True, resume_requested=True), self.tune) self.assertFalse(first.resume_override) - def preview_frame(tick, **changes): - return replace(frame(tick, speed_mps=4., horizon_m=30., raw_model_stopped=True), **changes) + return replace(frame(tick, speed_mps=4.0, horizon_m=30.0, raw_model_stopped=True), **changes) + class MovingPreviewTests(unittest.TestCase): - def test_moving_preview_does_not_become_an_odometry_countdown(self): - owner=ForceStop() - for tick in range(180):plan=owner.step(preview_frame(tick,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) - self.assertTrue(plan.forcing);self.assertFalse(plan.should_stop);self.assertEqual(plan.tracked_distance_m,30.) - def test_both_fresh_horizons_extend_the_rolling_preview(self): - for horizon in [24.59375,32.34375]: - owner=ForceStop();owner.last_model_ns=BASE;owner.distance=owner.distance_cap=6.19;owner.forcing=True;owner.timer=1.;owner.entry_speed=4.;owner.preview_tracking=True - plan=owner.step(preview_frame(1,horizon_m=horizon,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) - self.assertFalse(plan.should_stop);self.assertEqual(plan.tracked_distance_m,horizon) - def test_terminal_stop_cannot_reopen(self): - owner=ForceStop() - for tick in range(15):plan=owner.step(preview_frame(tick,horizon_m=5.,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) - self.assertTrue(plan.should_stop);before=plan.tracked_distance_m - plan=owner.step(preview_frame(15,horizon_m=35.,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) - self.assertTrue(plan.should_stop);self.assertLess(plan.tracked_distance_m,before) - def test_missing_geometry_keeps_last_preview_and_odometry(self): - owner=ForceStop() - for tick in range(15):before=owner.step(preview_frame(tick,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) - after=owner.step(preview_frame(15,horizon_m=None,road_curvature=None,perception_available=False,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) - self.assertAlmostEqual(after.tracked_distance_m,before.tracked_distance_m-.2) - def test_confirmed_stop_and_sign_cannot_extend(self): - for kw in [{'model_should_stop':True},{'sign':True}]: - owner=ForceStop();owner.last_model_ns=BASE;owner.distance=owner.distance_cap=8.;owner.forcing=True;owner.timer=1. - after=owner.step(preview_frame(1,horizon_m=35.,moving_preview=True,**kw),vehicle_tune('HYUNDAI_IONIQ_6')) - self.assertLess(after.tracked_distance_m,8.) - def test_once_stationary_endpoint_seen_it_cannot_become_a_moving_preview_again(self): - owner=ForceStop() - for tick in range(15):owner.step(preview_frame(tick,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) - fixed=owner.step(preview_frame(15,horizon_m=20.,moving_preview=False),vehicle_tune('HYUNDAI_IONIQ_6')) - reopened=owner.step(preview_frame(16,horizon_m=35.,moving_preview=True),vehicle_tune('HYUNDAI_IONIQ_6')) - self.assertLess(reopened.tracked_distance_m,fixed.tracked_distance_m) - def test_other_profiles_retain_existing_distance_policy(self): - owner=ForceStop() - for tick in range(180):plan=owner.step(preview_frame(tick,moving_preview=True),vehicle_tune('OTHER')) - self.assertTrue(plan.should_stop);self.assertLess(plan.tracked_distance_m,6.) + def test_moving_preview_does_not_become_an_odometry_countdown(self): + owner = ForceStop() + for tick in range(180): + plan = owner.step(preview_frame(tick, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertTrue(plan.forcing) + self.assertFalse(plan.should_stop) + self.assertEqual(plan.tracked_distance_m, 30.0) + + def test_both_fresh_horizons_extend_the_rolling_preview(self): + for horizon in [24.59375, 32.34375]: + owner = ForceStop() + owner.last_model_ns = BASE + owner.distance = owner.distance_cap = 6.19 + owner.forcing = True + owner.timer = 1.0 + owner.entry_speed = 4.0 + owner.preview_tracking = True + plan = owner.step(preview_frame(1, horizon_m=horizon, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertFalse(plan.should_stop) + self.assertEqual(plan.tracked_distance_m, horizon) + + def test_terminal_stop_cannot_reopen(self): + owner = ForceStop() + for tick in range(15): + plan = owner.step(preview_frame(tick, horizon_m=5.0, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertTrue(plan.should_stop) + before = plan.tracked_distance_m + plan = owner.step(preview_frame(15, horizon_m=35.0, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertTrue(plan.should_stop) + self.assertLess(plan.tracked_distance_m, before) + + def test_missing_geometry_keeps_last_preview_and_odometry(self): + owner = ForceStop() + for tick in range(15): + before = owner.step(preview_frame(tick, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6')) + after = owner.step(preview_frame(15, horizon_m=None, road_curvature=None, perception_available=False, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertAlmostEqual(after.tracked_distance_m, before.tracked_distance_m - 0.2) + + def test_confirmed_stop_and_sign_cannot_extend(self): + for kw in [{'model_should_stop': True}, {'sign': True}]: + owner = ForceStop() + owner.last_model_ns = BASE + owner.distance = owner.distance_cap = 8.0 + owner.forcing = True + owner.timer = 1.0 + after = owner.step(preview_frame(1, horizon_m=35.0, moving_preview=True, **kw), vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertLess(after.tracked_distance_m, 8.0) + + def test_once_stationary_endpoint_seen_it_cannot_become_a_moving_preview_again(self): + owner = ForceStop() + for tick in range(15): + owner.step(preview_frame(tick, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6')) + fixed = owner.step(preview_frame(15, horizon_m=20.0, moving_preview=False), vehicle_tune('HYUNDAI_IONIQ_6')) + reopened = owner.step(preview_frame(16, horizon_m=35.0, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertLess(reopened.tracked_distance_m, fixed.tracked_distance_m) + + def test_fixed_endpoint_above_forty_metres_cannot_reopen(self): + owner = ForceStop() + owner.last_model_ns = BASE + owner.distance = owner.distance_cap = 80. + owner.entry_speed = 4. + owner.forcing = True + owner.timer = 1. + plan = owner.step(preview_frame(1, horizon_m=190., moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6')) + self.assertLess(plan.tracked_distance_m, 80.) + + def test_other_profiles_retain_existing_distance_policy(self): + owner = ForceStop() + for tick in range(180): + plan = owner.step(preview_frame(tick, moving_preview=True), vehicle_tune('OTHER')) + self.assertTrue(plan.should_stop) + self.assertLess(plan.tracked_distance_m, 6.0)