Preserve Ioniq 5 PE stop targets through distance braking

openpilot/selfdrive/controls/lib/longcontrol.py

@@ -97,7 +97,7 @@ 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
+      if (context.force_stop_braking and self.extension is not None and self.extension.force_stop_distance_braking 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)

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -32,7 +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.force_stop_braking import distance_target, eligible as distance_braking_eligible
 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
@@ -111,6 +111,7 @@ class LongitudinalPlanner:
     self.plan_drive_id = 0
     self.throttle_gate = ModelThrottleGate()
     self.ioniq6_throttle_gate_enabled = ioniq6_long_eligible(CP)
+    self.force_stop_distance_braking = distance_braking_eligible(CP)
     self.lane_change_gap = LaneChangeGap()
     self.experimental_release = ExperimentalRelease()
     self.lead_approach = LeadApproach(dt, CP.longitudinalActuatorDelay)
@@ -410,7 +411,7 @@ 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:
+    if self.force_stop_distance_braking 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:
@@ -514,7 +515,7 @@ 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
+    hold.forceStopBraking = bool(self.force_stop_distance_braking 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)

openpilot/starpilot/longitudinal/extension.py

@@ -2,6 +2,7 @@
 from dataclasses import dataclass
 
 from openpilot.common.realtime import DT_CTRL
+from openpilot.starpilot.longitudinal.force_stop_braking import eligible as distance_braking_eligible
 from opendbc.car.structs import car
 from openpilot.starpilot.longitudinal.toyota_output_policy import COROLLA, ToyotaOutputPolicy, development_enabled, production_enabled
 from openpilot.starpilot.longitudinal.ioniq6_start import Ioniq6StartPolicy, StartEvidence, eligible as ioniq6_start_eligible
@@ -41,6 +42,7 @@ class LongitudinalExtension:
     self.toyota_stopped_lead = toyota_production and str(CP.carFingerprint) == COROLLA
     self.toyota_output = ToyotaOutputPolicy(CP) if toyota_production or development_enabled(CP) else None
     self.ioniq6_start = Ioniq6StartPolicy() if ioniq6_start_eligible(CP) else None
+    self.force_stop_distance_braking = distance_braking_eligible(CP)
     self.vehicle_target = getattr(self.vehicle_policy, "target", None)
     self.kp = self.vehicle_policy.kp if self.vehicle_policy is not None else 0.0
     self.ki = getattr(self.vehicle_policy, "ki", None)
@@ -162,7 +164,7 @@ def create_extension(cp, startup_preferences=None):
   extension = LongitudinalExtension(cp, startup_preferences)
   if (extension.vehicle_policy is None and extension.toyota_output is None and extension.ioniq6_start is None and
       extension.bolt_mode is None and extension.stopping_policy is None and extension.stopping_decel_rate == 1.0 and
-      not extension.ignore_cruise_standstill):
+      not extension.ignore_cruise_standstill and not extension.force_stop_distance_braking):
     return None
   return extension
 

openpilot/starpilot/longitudinal/force_stop.py

@@ -39,7 +39,7 @@ class StopTune:
 
 
 def vehicle_tune(fingerprint: str) -> StopTune:
-  if fingerprint == 'HYUNDAI_IONIQ_6':
+  if fingerprint in ('HYUNDAI_IONIQ_6', 'HYUNDAI_IONIQ_5_PE'):
     return StopTune(distance_control=True)
   if fingerprint == 'TOYOTA_CAMRY_TSS2':
     return StopTune(handoff_m=4.5, distance_bias_m=6.0)

openpilot/starpilot/longitudinal/force_stop_braking.py

@@ -1,12 +1,24 @@
-"""Ioniq 6 distance braking under the existing Force Stop stop/hold intent."""
+"""Distance braking under the existing qualified Ioniq stop/hold intent."""
 
 import math
 
+from openpilot.starpilot.longitudinal.ioniq6_start import eligible as ioniq6_eligible
+from opendbc.car.hyundai.ioniq5pe_long_profile import qualified as ioniq5pe_qualified
+
 MAX_AGE_NS = 150_000_000
 STOP_RESERVE_M = .5
 MIN_ACCEL = -3.5
 
 
+def eligible(cp):
+  try:
+    if cp.passive or cp.dashcamOnly or cp.notCar:
+      return False
+    return ioniq6_eligible(cp) or ioniq5pe_qualified(cp)
+  except (AttributeError, IndexError, TypeError, ValueError, RuntimeError):
+    return False
+
+
 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):

openpilot/starpilot/longitudinal/force_stop_runtime.py

@@ -14,7 +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.longitudinal.force_stop_braking import eligible as distance_braking_eligible
 from openpilot.starpilot.speed_limits.vision.stop_observation import CONTROL_STOP_MAX_AGE_NS, StopConfirmation
 
 
@@ -204,7 +204,7 @@ class ForceStopRuntime:
                         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))
+      tune = replace(vehicle_tune(str(cp.carFingerprint)), distance_control=distance_braking_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)

openpilot/starpilot/longitudinal/inputs.py

@@ -13,6 +13,7 @@ from openpilot.starpilot.longitudinal.toyota_output_policy import (CLOCK_PAIR_MA
                                                                  development_enabled as toyota_development_enabled,
                                                                  production_enabled as toyota_production_enabled, leads_from_radar)
 from openpilot.starpilot.longitudinal.ioniq6_start import StartEvidence, eligible as ioniq6_start_eligible
+from openpilot.starpilot.longitudinal.force_stop_braking import eligible as distance_braking_eligible
 from openpilot.starpilot.longitudinal.lead_takeoff_preferences import TakeoffPreferences
 from openpilot.starpilot.longitudinal.takeoffs import eligible as takeoff_eligible
 from opendbc.car.gm.conventional_pedal import policy_for as conventional_pedal_policy_for
@@ -84,6 +85,7 @@ class LongitudinalInputs:
     self.toyota_corolla_stop = toyota_production and str(self.CP.carFingerprint) == COROLLA
     self.toyota_sienna_production = toyota_production and str(self.CP.carFingerprint) == SIENNA_4G
     self.ioniq6_start_enabled = ioniq6_start_eligible(self.CP)
+    self.force_stop_distance_braking = distance_braking_eligible(self.CP)
     gm_policy = gm_pedal_policy_for(self.CP)
     self.gm_start_enabled = gm_policy is not None and gm_policy.friction_variant
     installed_policy = installed_policy_for(self.CP)
@@ -131,8 +133,8 @@ class LongitudinalInputs:
 
   @property
   def optional_services(self):
-    if self.ioniq6_start_enabled:
-      return ['radarState', 'deviceState', 'starpilotLongitudinalPlan']
+    if self.force_stop_distance_braking:
+      return ['radarState', 'deviceState', 'starpilotLongitudinalPlan'] + (['slcState'] if self.ev9_long_enabled else [])
     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):
@@ -153,7 +155,7 @@ class LongitudinalInputs:
 
   def context(self, active):
     context = self._vehicle_context(active)
-    if active and self.ioniq6_start_enabled:
+    if active and self.force_stop_distance_braking:
       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

openpilot/starpilot/longitudinal/tests/test_force_stop.py

@@ -577,3 +577,74 @@ class MovingPreviewTests(unittest.TestCase):
       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)
+
+
+class Ioniq5PreviewTests(unittest.TestCase):
+  def test_only_pe_selects_the_distance_control_tune(self):
+    self.assertTrue(vehicle_tune('HYUNDAI_IONIQ_5_PE').distance_control)
+    for fingerprint in ('HYUNDAI_IONIQ_5', 'OTHER'):
+      self.assertFalse(vehicle_tune(fingerprint).distance_control)
+
+  def test_recorded_early_stop_input_keeps_the_fresh_preview(self):
+    owner = ForceStop()
+    owner.last_model_ns = BASE
+    owner.distance = owner.distance_cap = 6.29
+    owner.forcing = True
+    owner.timer = 1.
+    owner.entry_speed = 6.54
+    owner.preview_tracking = True
+    plan = owner.step(preview_frame(1, speed_mps=4.019109725952148,
+                                   horizon_m=27.711849212646484, moving_preview=True),
+                      vehicle_tune('HYUNDAI_IONIQ_5_PE'))
+    self.assertFalse(plan.should_stop)
+    self.assertEqual(plan.tracked_distance_m, 27.711849212646484)
+
+  def test_rolling_preview_is_not_counted_down(self):
+    owner = ForceStop()
+    for tick in range(180):
+      plan = owner.step(preview_frame(tick, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_5_PE'))
+    self.assertTrue(plan.forcing)
+    self.assertFalse(plan.should_stop)
+    self.assertEqual(plan.tracked_distance_m, 30.)
+
+  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_5_PE'))
+    self.assertLess(plan.tracked_distance_m, 80.)
+
+  def test_terminal_stop_cannot_resume_preview_tracking(self):
+    owner = ForceStop()
+    tune = vehicle_tune('HYUNDAI_IONIQ_5_PE')
+    for tick in range(15):
+      before = owner.step(preview_frame(tick, horizon_m=5., moving_preview=True), tune)
+    self.assertTrue(before.should_stop)
+    after = owner.step(preview_frame(15, horizon_m=35., moving_preview=True), tune)
+    self.assertTrue(after.should_stop)
+    self.assertLess(after.tracked_distance_m, before.tracked_distance_m)
+
+  def test_fixed_model_or_confirmed_sign_ends_preview_tracking(self):
+    for signal in ({'model_should_stop': True}, {'sign': True}):
+      owner = ForceStop()
+      tune = vehicle_tune('HYUNDAI_IONIQ_5_PE')
+      for tick in range(15):
+        owner.step(preview_frame(tick, moving_preview=True), tune)
+      owner.step(preview_frame(15, moving_preview=True, **signal), tune)
+      self.assertFalse(owner.preview_tracking)
+      before = owner.distance
+      after = owner.step(preview_frame(16, horizon_m=35., moving_preview=True), tune)
+      self.assertLess(after.tracked_distance_m, before)
+
+  def test_missing_geometry_keeps_odometry_and_gas_still_releases(self):
+    owner = ForceStop()
+    tune = vehicle_tune('HYUNDAI_IONIQ_5_PE')
+    for tick in range(15):
+      before = owner.step(preview_frame(tick, moving_preview=True), tune)
+    after = owner.step(preview_frame(15, horizon_m=None, road_curvature=None,
+                                     perception_available=False, moving_preview=True), tune)
+    self.assertAlmostEqual(after.tracked_distance_m, before.tracked_distance_m - .2)
+    self.assertFalse(owner.step(preview_frame(16, pedal_override=True), tune).forcing)

openpilot/starpilot/longitudinal/tests/test_force_stop_braking.py

@@ -123,3 +123,70 @@ class NativeForceStopBrakingTests(TestCase):
       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)
+
+
+def pe_pair():
+  from opendbc.car.hyundai.tests.test_ioniq5pe_stock import params
+  from opendbc.car.hyundai.ioniq5pe_long_profile import candidate
+  stock = params()
+  return stock, candidate(stock, enabled=True, is_release=False)
+
+
+class Ioniq5PEDistanceBrakingTests(TestCase):
+  def test_only_the_existing_sealed_pe_owner_is_admitted(self):
+    from openpilot.starpilot.longitudinal.force_stop_braking import eligible
+    stock, cp = pe_pair()
+    self.assertFalse(eligible(stock))
+    self.assertTrue(eligible(cp))
+    for field, value in [('passive', True), ('dashcamOnly', True), ('notCar', True),
+                         ('pcmCruise', True), ('openpilotLongitudinalControl', False),
+                         ('alphaLongitudinalAvailable', False), ('alternativeExperience', 1),
+                         ('longitudinalActuatorDelay', math.nan)]:
+      with self.subTest(field=field):
+        _, invalid = pe_pair()
+        setattr(invalid, field, value)
+        self.assertFalse(eligible(invalid))
+    _, invalid = pe_pair()
+    invalid.safetyConfigs[0].safetyParam = 0x5491
+    self.assertFalse(eligible(invalid))
+
+  def test_real_inputs_and_control_keep_the_pe_stop_without_ioniq6_start(self):
+    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 car_state
+    from opendbc.can.packer import CANPacker
+    from opendbc.car.hyundai.hyundaicanfd import CanBus, create_acc_control
+    stock, cp = pe_pair()
+    sm = inputs()
+    sm.all_checks = lambda names: all(sm.valid.get(name, False) for name in names)
+    state = car_state(speed=4., standstill=False)
+    sm['carState'] = state
+    with tempfile.TemporaryDirectory() as directory:
+      reader = LongitudinalInputs(cp, Params(directory), lambda: sm)
+      self.assertFalse(reader.ioniq6_start_enabled)
+      self.assertIn('starpilotLongitudinalPlan', reader.optional_services)
+      if reader.ev9_long_enabled:
+        self.assertIn('slcState', 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)
+      self.assertIsNone(control.extension.ioniq6_start)
+      control.long_control_state = LongCtrlState.pid
+      control.last_output_accel = -1.5
+      output = control.update(True, state, -.8, True, (-3.5, 2.), context=context)
+      self.assertAlmostEqual(output, -.8)
+      self.assertEqual(control.long_control_state, LongCtrlState.stopping)
+      packet = create_acc_control(CANPacker('hyundai_canfd_generated'), CanBus(cp), True,
+                                  -1.5, output, True, False, 40., NS(leadDistanceBars=2))
+      self.assertEqual((int.from_bytes(packet[1], 'little') >> 184) & 1, 1)
+      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, state, -.8, True, (-3.5, 2.), context=stale), -.8)
+      normal = LongControl(stock)
+      normal.long_control_state = LongCtrlState.pid
+      normal.last_output_accel = -1.5
+      self.assertLess(normal.update(True, state, -.8, True, (-3.5, 2.), context=context), -1.5)

openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py

@@ -124,8 +124,13 @@ class ForceStopRuntimeTests(unittest.TestCase):
     from openpilot.starpilot.longitudinal.tests.test_ioniq6_start import candidate
 
     stock, admitted = candidate()
+    from opendbc.car.hyundai.tests.test_ioniq5pe_stock import params as pe_params
+    from opendbc.car.hyundai.ioniq5pe_long_profile import candidate as pe_candidate
+    pe_stock = pe_params()
+    pe_admitted = pe_candidate(pe_stock, enabled=True, is_release=False)
+    pe_stock.openpilotLongitudinalControl = True
     stock.openpilotLongitudinalControl = True  # An incomplete profile must still refuse the new tune.
-    for cp, expected in [(self.cp, False), (stock, False), (admitted, True)]:
+    for cp, expected in [(self.cp, False), (stock, False), (admitted, True), (pe_stock, False), (pe_admitted, True)]:
       self.cp = cp
       self.owner = ForceStopRuntime(self.params)
       with patch.object(self.owner.policy, 'step', wraps=self.owner.policy.step) as step:

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "8734a8ff1d7b1e249e65997f7d44f1b2c29eea1868dc45d344ebbadf2fd08cb0", "schema_version": 1}
+{"protected_inventory_sha256": "4362c257f7ab3147fec858192e1c502a0ae8f5f46dbf3eb895e55c9512898471", "schema_version": 1}