mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-11-12 15:33:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix Force Stop preview tracking and requested braking release

Browse Source
This commit is contained in:
firestar5683
2026-11-10 16:08:35 -06:00
parent 5f9d63494a 9f45afd3ec
commit 87bef90abf
17 changed files with 557 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/ioniq6_longitudinal.py
+6 -2
View File
@@ -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))
opendbc_repo/opendbc/car/hyundai/tests/test_ioniq6_longitudinal.py
+44
View File
@@ -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)
openpilot/cereal/custom.capnp
+3
View File
@@ -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
+5
View File
@@ -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
+8
View File
@@ -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
@@ -410,6 +411,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))
@@ -511,5 +515,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
+2
View File
@@ -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
+42 -9
View File
@@ -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':
@@ -56,6 +60,7 @@ class StopFrame:
resume_requested: bool = False
perception_available: bool = True
sign: bool = False
moving_preview: bool = False
@dataclass(frozen=True)
@@ -72,6 +77,7 @@ class StopPlan:
takeoff_light_observed: bool = False
takeoff_light: bool = False
resume_override: bool = False
episode_ns: int = 0
class ForceStop:
@@ -96,6 +102,9 @@ class ForceStop:
self.previously_enabled = False
self.distance = 0.0
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
@@ -187,7 +196,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)
@@ -208,15 +217,20 @@ 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
self.preview_tracking = tune.distance_control and frame.moving_preview
self.forcing |= not frame.standstill or held
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)):
@@ -225,17 +239,25 @@ 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)
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))
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
self.preview_tracking = False
self.sign_confirmed = False
self._clear_standstill()
self.distance = self.distance_cap = horizon if available else 0.0
@@ -245,11 +267,22 @@ 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
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 > 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,
obstacle = (stop_length * .93 + offset + tune.distance_bias_m
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,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,
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
+39
View File
@@ -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/force_stop_runtime.py
+8 -2
View File
@@ -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
openpilot/starpilot/longitudinal/inputs.py
+6
View File
@@ -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
+128
View File
@@ -15,11 +15,75 @@ 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')
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)
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)
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.
@@ -229,6 +293,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)
@@ -238,6 +324,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()
@@ -291,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.)
openpilot/starpilot/longitudinal/tests/test_force_stop_braking.py
+125
View File
@@ -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)
openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py
+15
View File
@@ -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
prebuilt
View File
prebuilt.json
-1
View File
@@ -1 +0,0 @@
{"protected_inventory_sha256": "b5c4808e99cbe09936d17a902d08c523dbc7acd8df9cb5de226b4207d095f661", "schema_version": 1}
tools/ci/custom-schema-contract.json
+124 -1
View File
@@ -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
}
}
}
}
}
}
tools/ci/tests/test_schema_policy.py
+2 -1
View File
@@ -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)))