diff --git a/opendbc_repo/opendbc/car/honda/bosch_longitudinal.py b/opendbc_repo/opendbc/car/honda/bosch_longitudinal.py index 247017a3b8c..37a204776a7 100644 --- a/opendbc_repo/opendbc/car/honda/bosch_longitudinal.py +++ b/opendbc_repo/opendbc/car/honda/bosch_longitudinal.py @@ -3,6 +3,7 @@ import numpy as np from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, DT_CTRL, structs +from opendbc.car.honda.bosch_a_radar import VERIFIED_BOSCH_A_CARS from opendbc.car.honda.brake_onset import BrakeOnsetCompensation from opendbc.car.honda.bosch_gas_learner import CivicGasLearner, LEARN_VERSION, gas_lookup_accel from opendbc.car.honda.values import CAR, HondaFlags @@ -10,6 +11,26 @@ LongCtrlState = structs.CarControl.Actuators.LongControlState BOSCH_BRAKE_FORCE_ON = -0.12 BOSCH_BRAKE_FORCE_RELEASE = -0.02 +# Bosch-A stop-and-go brake cycling, measured on the Civic (STATUS 263/265, route 0000001f--39b2448f7d). At 2-4 m/s with a command of +# -0.1..-0.3 the brake request toggled 15-25 times a minute: brake-on cut the gas from ~30-40 to 0, and every release +# jumped the gas straight to the 60-unit cap. Replay of this class on 1f's logged command, open loop, 50 Hz, with +# long-control drops resetting it, against the old path on the same harness (the state machine reproduces the +# logged BRAKE_REQUEST on 90.4 % of frames; no closed-loop gap or jerk numbers): +# BOSCH_A_BRAKE_FORCE_ON: brake request only below -0.20, the bottom of the gas lookup, so the band between it and +# -0.12 coasts with the gas off instead of braking. The only change that cuts toggles: 3.69 -> 3.17 per minute +# (an earlier sim that ignored long-control drops said 2.11). +# BOSCH_A_BRAKE_MIN_HOLD_S: once on, the request stays on this long unless the command asks for real acceleration +# (above BOSCH_A_BRAKE_HOLD_ESCAPE). Holds under 1.5 s 27.6 % -> 16.7 %; the rest are long-control drops and the +# escape (about 0.2 per minute, each shortening a hold by 0.04-0.4 s). +# BOSCH_A_RELEASE_GAS_START / _RAMP_S: after a release the gas starts at most at 35 and reaches the lookup over 1 s, +# instead of the 60 cap. Gas on the release frame 60 -> 35; mean over the first second 121 -> 81. +# Stopping still forces the request on, and a long-control drop still clears it, exactly as before. +# Runs on every car with the Bosch-A radar harness (bosch_a_harness), owner 2026-10-24: the CR-V 5G is static only. +BOSCH_A_BRAKE_FORCE_ON = -0.20 +BOSCH_A_BRAKE_MIN_HOLD_S = 1.5 +BOSCH_A_BRAKE_HOLD_ESCAPE = 0.2 +BOSCH_A_RELEASE_GAS_START = 35.0 +BOSCH_A_RELEASE_GAS_RAMP_S = 1.0 CLASSIC_BOSCH_CARS = frozenset(( CAR.HONDA_NBOX_2G, @@ -51,9 +72,51 @@ def update_honda_bosch_braking(braking: bool, gas_pedal_force: float, stopping: return gas_pedal_force <= BOSCH_BRAKE_FORCE_RELEASE return gas_pedal_force < BOSCH_BRAKE_FORCE_ON +class BoschABrakeEdge: + """Bosch-A harness brake request with a deeper on threshold, a minimum hold and a gas ramp after release.""" + def __init__(self, dt: float): + self.dt = dt + self.reset() + + def reset(self): + self.braking = False + self.hold_s = 0.0 + self.release_s = math.inf + + def update(self, gas_pedal_force: float, stopping: bool, long_active: bool) -> bool: + if not long_active: + self.reset() + return False + was_braking = self.braking + if stopping: + self.braking = True + elif self.braking: + held = self.hold_s < BOSCH_A_BRAKE_MIN_HOLD_S and gas_pedal_force <= BOSCH_A_BRAKE_HOLD_ESCAPE + self.braking = held or gas_pedal_force <= BOSCH_BRAKE_FORCE_RELEASE + else: + self.braking = gas_pedal_force < BOSCH_A_BRAKE_FORCE_ON + if self.braking: + self.hold_s = self.hold_s + self.dt if was_braking else 0.0 + self.release_s = math.inf + else: + self.release_s = 0.0 if was_braking else self.release_s + self.dt + return self.braking + + def limit_gas(self, gas: float) -> float: + """Gas after a brake release rises from BOSCH_A_RELEASE_GAS_START to the lookup over BOSCH_A_RELEASE_GAS_RAMP_S.""" + if self.braking or self.release_s >= BOSCH_A_RELEASE_GAS_RAMP_S or gas <= BOSCH_A_RELEASE_GAS_START: + return gas + return BOSCH_A_RELEASE_GAS_START + (gas - BOSCH_A_RELEASE_GAS_START) * self.release_s / BOSCH_A_RELEASE_GAS_RAMP_S + + def get_honda_bosch_wind_brake_mps2(v_ego: float) -> float: return float(np.interp(v_ego, [0.0, 13.4, 22.4, 31.3, 40.2], [0.0, 0.049, 0.136, 0.267, 0.441])) +def bosch_a_harness(cp): + """A verified classic Bosch car running the Bosch-A radar harness (same car gate as BoschARadar).""" + return (cp.carFingerprint in VERIFIED_BOSCH_A_CARS and not cp.radarUnavailable and + not cp.flags & (HondaFlags.BOSCH_RADARLESS | HondaFlags.BOSCH_CANFD | HondaFlags.BOSCH_ALT_RADAR)) + def qualified(cp): return (cp.brand == "honda" and cp.carFingerprint in CLASSIC_BOSCH_CARS and cp.flags & HondaFlags.BOSCH and not cp.flags & HondaFlags.BOSCH_CANFD and cp.openpilotLongitudinalControl and @@ -75,8 +138,11 @@ def __init__(self, cp, params): # Civic Bosch runs the ns live gas learner (bosch_gas_learner.py); every other car keeps the shared learner below. self.civic_learner = CivicGasLearner() if cp.carFingerprint == CAR.HONDA_CIVIC_BOSCH else None self.learner_version = LEARN_VERSION if self.civic_learner is not None else None - # Civic Bosch only: the fit in brake_onset.py is from this car. update() runs every other control frame. - self.brake_onset = BrakeOnsetCompensation(2 * DT_CTRL) if cp.carFingerprint == CAR.HONDA_CIVIC_BOSCH else None + # Bosch-A harness cars only (owner 2026-10-24). The brake_onset.py fit and the edge numbers are from the Civic; + # on the CR-V 5G they are static only. update() runs every other control frame. + bosch_a = bosch_a_harness(cp) + self.brake_onset = BrakeOnsetCompensation(2 * DT_CTRL) if bosch_a else None + self.brake_edge = BoschABrakeEdge(2 * DT_CTRL) if bosch_a else None self.set_factors(1.0, 1.0) def set_factors(self, gas, wind): @@ -141,9 +207,9 @@ def update(self, CC, CS, accel): gas_lookup_input = gas_pedal_force * self.bosch_gas_factor self.gas = float(np.interp(gas_lookup_input, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)) self.gas = min(self.gas, max(60.0, self.bosch_last_gas + 60.0)) - self.bosch_last_gas = self.gas stopping = actuators.longControlState == LongCtrlState.stopping - self.bosch_braking = update_honda_bosch_braking(self.bosch_braking, gas_pedal_force, stopping, CC.longActive) + self.bosch_braking = self._update_braking(gas_pedal_force, stopping, CC.longActive) + self.bosch_last_gas = self.gas return self.accel, self.gas, gas_pedal_force, self.bosch_braking def _update_civic(self, CC, CS, hill_brake, wind_brake_mps2, min_gas): @@ -160,7 +226,15 @@ def _update_civic(self, CC, CS, hill_brake, wind_brake_mps2, min_gas): lookup = gas_lookup_accel(gas_pedal_force, hill_brake, learner.gasfactor, min_gas) self.gas = float(np.interp(lookup, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)) self.gas = min(self.gas, max(60.0, self.bosch_last_gas + 60.0)) - self.bosch_last_gas = self.gas stopping = actuators.longControlState == LongCtrlState.stopping - self.bosch_braking = update_honda_bosch_braking(self.bosch_braking, gas_pedal_force, stopping, CC.longActive) + self.bosch_braking = self._update_braking(gas_pedal_force, stopping, CC.longActive) + self.bosch_last_gas = self.gas return self.accel, self.gas, gas_pedal_force, self.bosch_braking + + def _update_braking(self, gas_pedal_force, stopping, long_active): + """Brake request; on Bosch-A harness cars also the minimum hold and the gas ramp after release (self.gas).""" + if self.brake_edge is None: + return update_honda_bosch_braking(self.bosch_braking, gas_pedal_force, stopping, long_active) + braking = self.brake_edge.update(gas_pedal_force, stopping, long_active) + self.gas = self.brake_edge.limit_gas(self.gas) + return braking diff --git a/opendbc_repo/opendbc/car/honda/tests/test_bosch_longitudinal.py b/opendbc_repo/opendbc/car/honda/tests/test_bosch_longitudinal.py index 53c419bafec..643d1659ffc 100644 --- a/opendbc_repo/opendbc/car/honda/tests/test_bosch_longitudinal.py +++ b/opendbc_repo/opendbc/car/honda/tests/test_bosch_longitudinal.py @@ -127,3 +127,78 @@ def test_actual_configure_loads_typed_factors_without_startup_rewrite(): learning.owner.set_factors(1.2, 0.9) learning.persist(18000) assert writes == [("HondaGasFactorParams", 1.2), ("HondaWindFactorParams", 0.9)] + + +def test_civic_brake_request_starts_deeper_and_coasts_the_band_above(): + controller = owner(CAR.HONDA_CIVIC_BOSCH) + cc, cs = inputs(speed=3.0) + assert not controller.update(cc, cs, -0.15)[3] + assert controller.update(cc, cs, -0.30)[3] + + +def test_civic_brake_request_holds_at_least_its_minimum_unless_accel_is_asked(): + from opendbc.car.honda.bosch_longitudinal import BOSCH_A_BRAKE_MIN_HOLD_S + controller = owner(CAR.HONDA_CIVIC_BOSCH) + cc, cs = inputs(speed=3.0) + assert controller.update(cc, cs, -0.4)[3] + frames = 0 + while controller.update(cc, cs, 0.05)[3]: + frames += 1 + assert frames * controller.brake_edge.dt == pytest.approx(BOSCH_A_BRAKE_MIN_HOLD_S, abs=0.03) + assert controller.update(cc, cs, -0.4)[3] + assert not controller.update(cc, cs, 0.5)[3] # a clear accel request ends the hold at once + + +def test_civic_gas_after_release_ramps_from_35_instead_of_jumping_to_the_cap(): + from opendbc.car.honda.bosch_longitudinal import BOSCH_A_RELEASE_GAS_RAMP_S, BOSCH_A_RELEASE_GAS_START + controller = owner(CAR.HONDA_CIVIC_BOSCH) + cc, cs = inputs(speed=3.0) + for _ in range(100): + controller.update(cc, cs, -0.4) + gases = [] + for _ in range(int(BOSCH_A_RELEASE_GAS_RAMP_S / controller.brake_edge.dt) + 20): + _, gas, _, braking = controller.update(cc, cs, 0.3) + assert not braking + gases.append(gas) + assert gases[0] <= BOSCH_A_RELEASE_GAS_START + assert all(b >= a - 1e-6 for a, b in zip(gases, gases[1:], strict=False)) + assert max(b - a for a, b in zip(gases, gases[1:], strict=False)) < 10.0 + free = owner(CAR.HONDA_CIVIC_BOSCH) + for _ in range(100): + target = free.update(cc, cs, 0.3)[1] + assert gases[-1] == pytest.approx(target, rel=0.05) + + +def test_civic_brake_edge_keeps_stopping_and_long_drop_behavior(): + controller = owner(CAR.HONDA_CIVIC_BOSCH) + cc, cs = inputs(speed=1.0, state="stopping") + assert controller.update(cc, cs, 0.1)[3] + cc.longActive = False + assert not controller.update(cc, cs, -1.0)[3] + + +def test_brake_fixes_run_on_bosch_a_harness_cars_only(): + for identity in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G): + controller = owner(identity) + assert controller.brake_edge is not None and controller.brake_onset is not None + accord = owner(CAR.HONDA_ACCORD) + assert accord.brake_edge is None and accord.brake_onset is None + cp = CarInterface.get_params(CAR.HONDA_CIVIC_BOSCH, gen_empty_fingerprint(), [], True, False, False) + cp.radarUnavailable = True + no_radar = BoschLongitudinal(cp, CarControllerParams(cp)) + assert no_radar.brake_edge is None and no_radar.brake_onset is None + + +def test_crv_brake_edge_holds_the_request_and_ramps_gas_after_release(): + from opendbc.car.honda.bosch_longitudinal import BOSCH_A_RELEASE_GAS_RAMP_S, BOSCH_A_RELEASE_GAS_START + controller = owner(CAR.HONDA_CRV_5G) + cc, cs = inputs(speed=3.0) + assert not controller.update(cc, cs, -0.15)[3] + for _ in range(100): + assert controller.update(cc, cs, -0.4)[3] + gases = [] + for _ in range(int(BOSCH_A_RELEASE_GAS_RAMP_S / controller.brake_edge.dt) + 20): + _, gas, _, braking = controller.update(cc, cs, 0.3) + gases.append(gas) + assert not braking and gases[0] <= BOSCH_A_RELEASE_GAS_START + assert all(b >= a - 1e-6 for a, b in zip(gases, gases[1:], strict=False)) diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index f9e09fb8d59..4ecea2c54e8 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -498,11 +498,11 @@ def update(self, sm, *, cruise_ceiling: CruiseCeiling | None = None, sm, now_ns=sample_now_ns, drive_id=drive_id, target=float(output_a_target), stopping=self.output_should_stop, accel_min=ACCEL_MIN, active=profile_eligible and not reset_state and self.mpc.solution_status == 0) - if preap_acceleration_cap is not None: - output_a_target = min(output_a_target, preap_acceleration_cap) if self.human_driving: # Throttle side only: the cap is >= 0, so a braking target passes through. output_a_target = min(output_a_target, human_acceleration.cap(human_accel_base, v_cruise, v_ego)) + if preap_acceleration_cap is not None: + output_a_target = min(output_a_target, preap_acceleration_cap) if self.bosch_a_long.stock_feel and reset_state: # Engage init (Stock Brake Feel only, Bosch-A via active_for): while long is off, a_cruise starts the first # engaged frame at min(aEgo, cruise target) instead of aEgo (jerk-limited around aEgo), so it does not ramp down diff --git a/openpilot/starpilot/car/honda/radar.py b/openpilot/starpilot/car/honda/radar.py index 688fd168c87..221c1fc2464 100644 --- a/openpilot/starpilot/car/honda/radar.py +++ b/openpilot/starpilot/car/honda/radar.py @@ -9,6 +9,7 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.common.simple_kalman import KF1D +from openpilot.starpilot.car.honda.radar_reversal_bound import ReversalBound if TYPE_CHECKING: from openpilot.selfdrive.controls.radard import KalmanParams @@ -323,6 +324,7 @@ def __init__(self): self._post_standstill_candidate_id = -1 self._post_standstill_candidate_frames = 0 self._last_tracks_frame = -1 + self.reversal_bound = ReversalBound() def update_tracks(self, sm, rr, radar): self.ready = radar.ready @@ -364,6 +366,8 @@ def leads(self, sm, radar, model_v_ego): for index in range(2) ] output[0] = self._filter_post_standstill_lead(output[0]) + output = self.reversal_bound.apply(output, leads, self.v_ego, model_v_ego, + [radar.lead_prob_filters[index].x for index in range(2)], self.tracks) for index, lead in enumerate(output): if lead.get('present', False) and lead.get('radar', False): identifier = int(lead.get('radarTrackId', -1)) diff --git a/openpilot/starpilot/car/honda/radar_reversal_bound.py b/openpilot/starpilot/car/honda/radar_reversal_bound.py new file mode 100644 index 00000000000..b80365b8e10 --- /dev/null +++ b/openpilot/starpilot/car/honda/radar_reversal_bound.py @@ -0,0 +1,99 @@ +"""Bosch-A: after a radar lead's accel reverses from hard positive to hard negative, bound its braking authority. + +Route 30 (11c8fa231c0499ed/00000030--70508f00bd) T1420-1425, a two-lane S-curve with guardrails both sides and the +lead 4-6 m off-centre: radar track 60 kept its ID while its range swung 45 -> 54 -> 39 m and vLead 22 -> 27.6 -> 15.7 +in about 4 s. The model lead held 47-52 m at 21-22 m/s with a ~ 0, and on the stills the lead does not close. +Published aLeadK went +4.0 to -3.0 (the clamp) in 1 s; the negative half took the plan (lead0) to aTarget -2.87. + +A real car does not go from accelerating above +2 m/s^2 to braking below -2 m/s^2 inside 1.5 s. Under that rule the +model lead had 0 hits on 18 radar routes (0d-20, 25, 27, 2a, 2b, 2c, 2e, 2f, 30, 31, 32), while radar leads had 18. +Only route 30's hit had lead0 braking the plan below -1.0 in the next 4 s. Of the 58 engaged lead0 brakes below +-1.5 on those routes, none had a same-track reversal in the 4 s before it (log level, limited road evidence; one +positive event, on one road). + +So for LATCH_S after such a reversal the published lead's vLead/vLeadK/vRel and aLeadK are moved W of the way +toward the model lead, only when the model lead is the less severe, and only while the model lead is a credible +witness. dRel is never moved and the point is always published (D-041/D-042). The radar's own geometry bypasses the +bound when it is urgent (range or TTC short), so a real close-in brake keeps full authority. A false trigger costs +nothing unless the radar lead is braking the plan inside the latch. +Estimated effect on route 30 (regression on that route's lead0 rows, not a planner replay): -2.87 -> about -1.3. +""" +from __future__ import annotations + +import math +from typing import Any + +from openpilot.common.realtime import DT_MDL + +REVERSAL_HIGH = 2.0 # aLeadK above this arms a track (m/s^2) +REVERSAL_LOW = -2.0 # aLeadK below this within REVERSAL_WINDOW_S of an arm latches it +REVERSAL_WINDOW_S = 1.5 +LATCH_S = 4.0 # route 30: the brake came ~1.5 s after the reversal and lasted ~2 s +W = 0.6 # never all the way to the model lead (D-042) +URGENT_D = 30.0 # radar range below this: no bound +URGENT_TTC = 4.0 # radar TTC below this: no bound +WITNESS_PROB = 0.35 # filtered model lead prob for the model lead to count as a witness (= lead_detection_probability) + + +def urgent(lead: dict[str, Any]) -> bool: + d_rel = float(lead['dRel']) + closing = max(-float(lead['vRel']), 0.0) + return d_rel < URGENT_D or (closing > 0.1 and d_rel / closing < URGENT_TTC) + + +class ReversalBound: + """Per-track reversal latch over the published Bosch-A leads. Call `apply` once per radard cycle.""" + def __init__(self, dt: float = DT_MDL): + self.dt = dt + self.t = 0.0 + self.armed_t: dict[int, float] = {} # last time aLeadK was above REVERSAL_HIGH, per track + self.latched_until: dict[int, float] = {} + self.weights = [0.0, 0.0] # last applied weight per lead index, for replay/debug + + def _observe(self, track_id: int, a_lead: float) -> None: + if a_lead > REVERSAL_HIGH: + self.armed_t[track_id] = self.t + elif a_lead < REVERSAL_LOW: + armed = self.armed_t.get(track_id) + if armed is not None and self.t - armed <= REVERSAL_WINDOW_S: + self.latched_until[track_id] = self.t + LATCH_S + + def latched(self, track_id: int) -> bool: + return self.latched_until.get(track_id, -math.inf) >= self.t + + def _bound(self, lead: dict[str, Any], vision, v_ego: float, model_v_ego: float, lead_prob: float) -> tuple[dict[str, Any], float]: + if lead_prob < WITNESS_PROB or urgent(lead): + return lead, 0.0 + vision_v = v_ego + (float(vision.v[0]) - model_v_ego) + dv = W * max(vision_v - float(lead['vLead']), 0.0) + da = W * max(float(vision.a[0]) - float(lead['aLeadK']), 0.0) + if dv <= 0.0 and da <= 0.0: + return lead, 0.0 + out = dict(lead) + out['vLead'] = float(lead['vLead']) + dv + out['vLeadK'] = float(lead['vLeadK']) + dv + out['vRel'] = float(lead['vRel']) + dv + out['aLeadK'] = float(lead['aLeadK']) + da + return out, W + + def apply(self, output: list[dict[str, Any]], model_leads, v_ego: float, model_v_ego: float, lead_probs, track_ids) -> list[dict[str, Any]]: + self.t += self.dt + seen = set() + for lead in output: + if lead.get('present', False) and lead.get('radar', False): + track_id = int(lead.get('radarTrackId', -1)) + if track_id >= 0 and track_id not in seen: + seen.add(track_id) + self._observe(track_id, float(lead['aLeadK'])) + for track_id in [i for i in self.armed_t if i not in track_ids]: + del self.armed_t[track_id] + for track_id in [i for i in self.latched_until if i not in track_ids or self.latched_until[i] < self.t]: + del self.latched_until[track_id] + + bounded = [] + for index, lead in enumerate(output): + self.weights[index] = 0.0 + if lead.get('present', False) and lead.get('radar', False) and self.latched(int(lead.get('radarTrackId', -1))): + lead, self.weights[index] = self._bound(lead, model_leads[index], v_ego, model_v_ego, float(lead_probs[index])) + bounded.append(lead) + return bounded diff --git a/openpilot/starpilot/car/honda/tests/test_radar_reversal_bound.py b/openpilot/starpilot/car/honda/tests/test_radar_reversal_bound.py new file mode 100644 index 00000000000..6897dfa31d3 --- /dev/null +++ b/openpilot/starpilot/car/honda/tests/test_radar_reversal_bound.py @@ -0,0 +1,137 @@ +"""Reversal-latch bound (static). The positive case is shaped on route 30 T1420-1425 (track 60).""" +from types import SimpleNamespace + +import pytest + +from openpilot.common.realtime import DT_MDL +from openpilot.starpilot.car.honda.radar_reversal_bound import LATCH_S, REVERSAL_WINDOW_S, W, ReversalBound + +V_EGO = 22.0 + + +def radar_lead(tid, d, v, a): + return {'dRel': d, 'yRel': 0.0, 'vRel': v - V_EGO, 'vLead': v, 'vLeadK': v, 'aLeadK': a, 'aLeadTau': 0.6, + 'present': True, 'modelProb': 0.95, 'radar': True, 'radarTrackId': tid} + + +def vision(v=21.5, a=0.0): + return SimpleNamespace(x=[50.0], v=[v], a=[a]) + + +ABSENT = {'present': False} + + +def step(rb, lead, model=None, prob=0.95, tracks=None): + model = model or vision() + tracks = {lead['radarTrackId']: None} if tracks is None and lead.get('radar') else (tracks or {}) + return rb.apply([lead, ABSENT], [model, vision()], V_EGO, V_EGO, [prob, 0.0], tracks)[0] + + +def frames(seconds): + return int(round(seconds / DT_MDL)) + + +def reverse(rb, tid=60, gap_s=1.0, first=4.0, second=-3.0, d=45.0): + step(rb, radar_lead(tid, d, 27.6, first)) + for _ in range(frames(gap_s) - 1): + step(rb, radar_lead(tid, d, 24.0, 0.0)) + return step(rb, radar_lead(tid, d, 20.0, second)) + + +def test_route_30_shape_is_softened_not_dropped(): + rb = ReversalBound() + reverse(rb) + raw = radar_lead(60, 39.0, 15.7, -3.0) + out = step(rb, raw, vision(21.5, 0.0)) + assert out['present'] and out['radar'] and out['radarTrackId'] == 60 + assert out['dRel'] == raw['dRel'] + assert out['vLead'] == pytest.approx(15.7 + W * (21.5 - 15.7)) + assert out['vLeadK'] == pytest.approx(out['vLead']) + assert out['vRel'] == pytest.approx(out['vLead'] - V_EGO) + assert out['aLeadK'] == pytest.approx(-3.0 + W * 3.0) + assert rb.weights[0] == W + assert raw['vLead'] == 15.7 # input not mutated + + +def test_negative_then_positive_order_does_not_latch(): + rb = ReversalBound() + reverse(rb, first=-3.0, second=4.0) + raw = radar_lead(60, 45.0, 15.7, -1.5) # still braking, but not past REVERSAL_LOW again + assert step(rb, raw) == raw + + +def test_reversal_slower_than_window_does_not_latch(): + rb = ReversalBound() + reverse(rb, gap_s=REVERSAL_WINDOW_S + 0.2) + raw = radar_lead(60, 45.0, 15.7, -3.0) + assert step(rb, raw) == raw + + +def test_urgent_range_or_ttc_keeps_full_authority(): + rb = ReversalBound() + reverse(rb) + close = radar_lead(60, 29.0, 15.7, -3.0) + assert step(rb, close) == close + short_ttc = radar_lead(60, 40.0, V_EGO - 11.0, -3.0) # 11 m/s closing, TTC 3.6 s + assert step(rb, short_ttc) == short_ttc + + +def test_only_moves_toward_less_severe(): + rb = ReversalBound() + reverse(rb) + raw = radar_lead(60, 45.0, 21.0, -0.5) + out = step(rb, raw, vision(15.0, -2.5)) # vision more severe than radar: untouched + assert out == raw + assert rb.weights[0] == 0.0 + + +def test_latch_expires(): + rb = ReversalBound() + reverse(rb) + for _ in range(frames(LATCH_S) - 2): + out = step(rb, radar_lead(60, 45.0, 18.0, -1.0)) + assert out['vLead'] > 18.0 + for _ in range(4): + out = step(rb, radar_lead(60, 45.0, 18.0, -1.0)) + assert out['vLead'] == 18.0 + + +def test_retrigger_extends_latch(): + rb = ReversalBound() + reverse(rb) + for _ in range(frames(2.0)): + step(rb, radar_lead(60, 45.0, 22.0, 0.0)) + reverse(rb) + for _ in range(frames(3.0)): + out = step(rb, radar_lead(60, 45.0, 18.0, -1.0)) + assert out['vLead'] > 18.0 + + +def test_weak_model_lead_is_not_a_witness(): + rb = ReversalBound() + reverse(rb) + raw = radar_lead(60, 45.0, 15.7, -3.0) + assert step(rb, raw, prob=0.2) == raw + + +def test_other_track_not_affected(): + rb = ReversalBound() + reverse(rb, tid=60) + raw = radar_lead(61, 45.0, 15.7, -3.0) + assert step(rb, raw, tracks={60: None, 61: None}) == raw + + +def test_vision_only_and_absent_leads_pass_through(): + rb = ReversalBound() + reverse(rb) + vision_lead = {'present': True, 'radar': False, 'radarTrackId': -1, 'dRel': 45.0, 'vRel': -6.0, + 'vLead': 16.0, 'vLeadK': 16.0, 'aLeadK': -3.0} + out = rb.apply([vision_lead, ABSENT], [vision(), vision()], V_EGO, V_EGO, [0.95, 0.95], {60: None}) + assert out[0] is vision_lead and out[1] is ABSENT + + +def test_vanished_track_state_is_pruned(): + rb = ReversalBound() + reverse(rb) + rb.apply([ABSENT, ABSENT], [vision(), vision()], V_EGO, V_EGO, [0.0, 0.0], {}) + assert rb.armed_t == {} and rb.latched_until == {}