diff --git a/openpilot/selfdrive/controls/lib/longcontrol.py b/openpilot/selfdrive/controls/lib/longcontrol.py index 77ee94691d..cba347988f 100644 --- a/openpilot/selfdrive/controls/lib/longcontrol.py +++ b/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) diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 6702082962..7ec6aa4fc1 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/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) diff --git a/openpilot/starpilot/longitudinal/extension.py b/openpilot/starpilot/longitudinal/extension.py index 27901ef8bb..3f6cb9239d 100644 --- a/openpilot/starpilot/longitudinal/extension.py +++ b/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 diff --git a/openpilot/starpilot/longitudinal/force_stop.py b/openpilot/starpilot/longitudinal/force_stop.py index dfbb2ff244..30dade23f7 100644 --- a/openpilot/starpilot/longitudinal/force_stop.py +++ b/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) diff --git a/openpilot/starpilot/longitudinal/force_stop_braking.py b/openpilot/starpilot/longitudinal/force_stop_braking.py index e1af20af49..e27983b255 100644 --- a/openpilot/starpilot/longitudinal/force_stop_braking.py +++ b/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): diff --git a/openpilot/starpilot/longitudinal/force_stop_runtime.py b/openpilot/starpilot/longitudinal/force_stop_runtime.py index 0a0e74fdbc..ef27628ff5 100644 --- a/openpilot/starpilot/longitudinal/force_stop_runtime.py +++ b/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) diff --git a/openpilot/starpilot/longitudinal/inputs.py b/openpilot/starpilot/longitudinal/inputs.py index 92e8323be8..2c78d99fb3 100644 --- a/openpilot/starpilot/longitudinal/inputs.py +++ b/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 diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop.py b/openpilot/starpilot/longitudinal/tests/test_force_stop.py index e7900b87ff..f4860b7671 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop.py +++ b/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) diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop_braking.py b/openpilot/starpilot/longitudinal/tests/test_force_stop_braking.py index 920ec95c83..e897d2dc6e 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop_braking.py +++ b/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) diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py b/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py index 1eec1dd9a2..529da2dd8b 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py +++ b/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: diff --git a/prebuilt.json b/prebuilt.json index a0ac87b45b..d72083a6f6 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "8734a8ff1d7b1e249e65997f7d44f1b2c29eea1868dc45d344ebbadf2fd08cb0", "schema_version": 1} +{"protected_inventory_sha256": "4362c257f7ab3147fec858192e1c502a0ae8f5f46dbf3eb895e55c9512898471", "schema_version": 1}