Track Ioniq Force Stop braking to the remaining stop distance

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;
 }
+

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
+

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)
+

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
+

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)
+

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

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)
+

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()

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)

prebuilt.json

@@ -1 +0,0 @@
-{"protected_inventory_sha256": "0a96cfa499ada974607b1255975f78be11f09ce5f3d1904e2a27598ca08e3d07", "schema_version": 1}

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)))
+