diff --git a/opendbc_repo/opendbc/bluepilot_lateral/core/angle.py b/opendbc_repo/opendbc/bluepilot_lateral/core/angle.py index 8d789921a9..e22c887325 100644 --- a/opendbc_repo/opendbc/bluepilot_lateral/core/angle.py +++ b/opendbc_repo/opendbc/bluepilot_lateral/core/angle.py @@ -38,6 +38,8 @@ class Inputs: lane_direction: int = 0 limit_status: int = 0 geometry: LaneGeometry | None = None + speed_min: float | None = None + speed_max: float | None = None @dataclass(frozen=True) @@ -85,6 +87,12 @@ class AngleController: if not i.active or not all(math.isfinite(v) for v in values) or i.speed < 0.0: self.reset() return Command() + speed_min = max(i.speed, 0.1) if i.speed_min is None else i.speed_min + speed_max = max(i.speed, 0.1) if i.speed_max is None else i.speed_max + if (not all(math.isfinite(v) for v in (speed_min, speed_max)) or + not 0 < speed_min <= max(i.speed, 0.1) <= speed_max): + self.reset() + return Command() if i.steering_pressed and not self.press_last: self.preturned = abs(i.steering_angle) > 45.0 self.press_last = i.steering_pressed @@ -152,7 +160,7 @@ class AngleController: # measured-curvature interval and the modern ISO envelope. If those bands # cannot meet the previous actuator ROC, send a neutral mode-0 frame. effective_speed = max(i.speed, 0.1) - cap = min(bounds.CURVATURE_MAX, bounds.LATERAL_ACCEL / max(i.speed, 1.0) ** 2) + cap = min(bounds.CURVATURE_MAX, bounds.LATERAL_ACCEL / max(speed_max, 1.0) ** 2) low_kappa, high_kappa = -cap, cap if i.speed > 9.0: low_kappa = max(low_kappa, i.measured_curvature - 0.0018) @@ -161,8 +169,10 @@ class AngleController: # gain (0.95..1.3), so the worst equivalent curvature remains bounded. lower_gain = bounds.GAIN_MAX if low_kappa >= 0.0 else bounds.GAIN_MIN upper_gain = bounds.GAIN_MIN if high_kappa >= 0.0 else bounds.GAIN_MAX - lower = max(-bounds.PATH_MAX, low_kappa * effective_speed * lower_gain, self.path_last - roc) - upper = min(-bounds.PATH_MIN, high_kappa * effective_speed * upper_gain, self.path_last + roc) + lower_speed = speed_max if low_kappa >= 0.0 else speed_min + upper_speed = speed_min if high_kappa >= 0.0 else speed_max + lower = max(-bounds.PATH_MAX, low_kappa * lower_speed * lower_gain, self.path_last - roc) + upper = min(-bounds.PATH_MIN, high_kappa * upper_speed * upper_gain, self.path_last + roc) # Bound the actual-angle equivalent derivative at the worst admitted gain. # 45ms core margin remains inside the native elapsed-time/50ms cap. jerk_delta = bounds.LATERAL_JERK / max(i.speed - 1.0, 1.0) ** 2 * bounds.CORE_STEER_PERIOD_S * bounds.GAIN_MIN * effective_speed diff --git a/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot.py b/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot.py index 78bc8896e1..69ffbd3b0a 100644 --- a/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot.py +++ b/opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot.py @@ -88,6 +88,7 @@ class Host: self.cp = cp self.controller = AngleController(low_gain=low, high_gain=high) self.inputs = None + self.tx_blocked = None def update(self, CC, CS): if self.inputs is not None: @@ -102,6 +103,25 @@ class Host: (not aol or CS.out.gearShifter in (structs.CarState.GearShifter.drive, structs.CarState.GearShifter.sport, structs.CarState.GearShifter.low)) and not CS.out.steerFaultTemporary and not CS.out.steerFaultPermanent and not CS.out.vehicleSensorsInvalid) + # Rejected frames do not advance Panda's accepted counter/path history. + # One real blocked-TX increment requires an accepted neutral resynchronization, + # rather than sending an indefinitely rejected chain of active counters. + counter_reader = getattr(self.inputs, "native_tx_blocked", None) + blocked = counter_reader() if ready and callable(counter_reader) else None + if not ready: + self.tx_blocked = None + elif type(blocked) is int and 0 <= blocked <= 0xFFFFFFFF: + changed = self.tx_blocked is not None and blocked != self.tx_blocked + self.tx_blocked = blocked + ready = not changed + speed_min = speed_max = None + speed_window = getattr(CS, "bluepilot_speed_window", None) + if speed_window is not None: + low, high, step = speed_window + # Allow source progress across the existing 100 ms CAN freshness window. + margin = max(0.05, 5 * step) + speed_min = max(0.1, min(low, CS.out.vEgoRaw) - margin) + speed_max = max(high, CS.out.vEgoRaw, 0.1) + margin meta = model.meta if model is not None else None return self.controller.step(Inputs( active=ready, speed=CS.out.vEgoRaw, measured_curvature=-CS.out.yawRate / max(CS.out.vEgoRaw, 0.1), @@ -111,4 +131,4 @@ class Host: lane_change=int(getattr(meta.laneChangeState, "raw", meta.laneChangeState)) in (1, 2, 3) if meta else False, lane_direction=int(getattr(meta.laneChangeDirection, "raw", meta.laneChangeDirection)) if meta else 0, limit_status=getattr(CS, "lat_ctl_lim_stat", 0), - geometry=model_geometry(model, fresh=snapshot is not None))) + geometry=model_geometry(model, fresh=snapshot is not None), speed_min=speed_min, speed_max=speed_max)) diff --git a/opendbc_repo/opendbc/car/ford/carstate.py b/opendbc_repo/opendbc/car/ford/carstate.py index b5a0d8ded6..c009f11d01 100644 --- a/opendbc_repo/opendbc/car/ford/carstate.py +++ b/opendbc_repo/opendbc/car/ford/carstate.py @@ -40,6 +40,8 @@ class CarState(CarStateBase): self.lkas_available = False self.lkas_available_ts_nanos = 0 self.lateral_motion_control = None + self.bluepilot_speed_samples = [0] * 6 if CP.safetyConfigs and CP.safetyConfigs[-1].safetyParam & 128 else None + self.bluepilot_speed_window = None def get_car_gps(self): return self.car_gps_tracker.get() @@ -63,6 +65,14 @@ class CarState(CarStateBase): # car speed ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS + if self.bluepilot_speed_samples is not None: + # Match Panda's six raw ABS speed samples, including its millimetre/second rounding. + for value in cp.vl_all["BrakeSysFeatures"]["Veh_V_ActlBrk"]: + self.bluepilot_speed_samples.append(math.floor(value * CV.KPH_TO_MS * 1000 + .5)) + self.bluepilot_speed_samples = self.bluepilot_speed_samples[-6:] + samples = self.bluepilot_speed_samples + step = max(abs(b - a) for a, b in zip(samples, samples[1:], strict=False)) / 1000 + self.bluepilot_speed_window = (min(samples) / 1000, max(samples) / 1000, step) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1 diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index cd995f141d..9ea56f7e72 100755 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -1762,8 +1762,9 @@ class SelfdriveD: # Ioniq6 also checks the physical brake before its separately published host sample. # Retain only its already granted LAT; all proof, health and LONG withdrawal gates remain. # The exact vehicle policies and all proof/health checks above remain required. + from openpilot.starpilot.car.ford.aol import early_native_long_withdrawal_supported as ford_early_long return bool(gm_native_accepts_cp(self.CP, native.safetyModel, native.safetyParam) or - early_native_long_withdrawal_supported(self.CP) or + early_native_long_withdrawal_supported(self.CP) or ford_early_long(self.CP) or toyota_aol_qualified(self.CP, marked_only=True) or not native.pandaInventory[owners[0]].controlsAllowed and not self.sm['pandaStates'][owners[0]].controlsAllowed) diff --git a/openpilot/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index b15012e594..8e0452c775 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/openpilot/starpilot/aol/transport_pause.py @@ -320,7 +320,8 @@ class TransportPauseFeedback: from openpilot.starpilot.car.hyundai.aol import qualified_ccnc_long_aol as ev9_qualified from opendbc.car import structs ev9 = cp.brand == 'hyundai' and ev9_qualified(cp, marked_only=True) - early_long = gm or cp.brand == 'hyundai' and early_native_long_withdrawal_supported(cp) or ev9 + from openpilot.starpilot.car.ford.aol import early_native_long_withdrawal_supported as ford_early_long + early_long = gm or cp.brand == 'hyundai' and early_native_long_withdrawal_supported(cp) or ev9 or ford_early_long(cp) from openpilot.starpilot.car.gm.aol import f1_conventional_lateral_owner, gm_physical_pedal_withdrawal gm_pedal = gm_physical_pedal_withdrawal(cp, cs) and not f1_conventional_lateral_owner(cp) event_ns = int(sm.logMonoTime['onroadEvents']) diff --git a/openpilot/starpilot/car/ford/aol.py b/openpilot/starpilot/car/ford/aol.py index 37d9089bab..d2a40e3c50 100644 --- a/openpilot/starpilot/car/ford/aol.py +++ b/openpilot/starpilot/car/ford/aol.py @@ -23,6 +23,13 @@ def native_accepts_cp(cp, model, param): return qualified(cp, marked_only=True) and native_profile_supported(model, param) and param == aol_safety_config(cp).safetyParam +def early_native_long_withdrawal_supported(cp): + # Panda can observe a pedal before the parsed host sample. The existing + # independent-LAT proof remains mandatory; this never authorizes LONG. + return bool(qualified(cp, marked_only=True) and cp.openpilotLongitudinalControl and + aol_safety_config(cp).safetyParam & ANGLE) + + def native_latch_rejected(cp, native): return qualified(cp, marked_only=True) and native.requestedLateral and not native.lateralAllowed diff --git a/openpilot/starpilot/controller_extensions.py b/openpilot/starpilot/controller_extensions.py index 65f532bee0..849c86e0b6 100644 --- a/openpilot/starpilot/controller_extensions.py +++ b/openpilot/starpilot/controller_extensions.py @@ -123,6 +123,14 @@ class ManualTurnInputs: except (AttributeError, KeyError, TypeError, ValueError, OverflowError): return False + def native_tx_blocked(self): + """Read only after native_permission qualified this same Panda snapshot.""" + try: + value = self.sm["pandaStates"][-1].safetyTxBlocked + return int(value) if 0 <= value <= 0xFFFFFFFF else None + except (AttributeError, IndexError, KeyError, TypeError, ValueError, OverflowError): + return None + def assist_permission(self): if not self.track_assist_permission: return (), False diff --git a/openpilot/starpilot/tests/ford_lifecycle_helpers.py b/openpilot/starpilot/tests/ford_lifecycle_helpers.py index 2e5b3d903a..91a4b43b57 100644 --- a/openpilot/starpilot/tests/ford_lifecycle_helpers.py +++ b/openpilot/starpilot/tests/ford_lifecycle_helpers.py @@ -233,7 +233,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag for original in messages: if original.which() == 'modelV2': original = original.as_builder() - original.modelV2.action.desiredCurvature = 0.0001 + original.modelV2.action.desiredCurvature = getattr(monkeypatch, "ford_test_curvature", 0.0001) original = original.as_reader() snapshots.append(original) return control_update(timestamp, snapshots) @@ -256,6 +256,9 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag local.setattr(sd.sm, 'update_msgs', location_update) + delayed_pedal_frames = [] + delayed_pedals_seen = set() + def step(*, engaged=False, brake=False, reverse=False, warmup=False, fault=None, main_off=False, availability_transition=False): nonlocal steps, warmup_waits, qualified_samples, active_lat_frames, active_long_frames, nonzero_after_brake, brake_seen brake_seen |= brake @@ -267,7 +270,8 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag request = 0 if prior_axis is None else (int(prior_axis.aolAxisState.desiredLateral) | (int(prior_axis.aolAxisState.desiredLongitudinal) << 1)) arm_heartbeat = retained_arm(published.get('aolIntentWire'), prior_axis, now[0]) heartbeat = bool(request or prior_state is not None and prior_state.selfdriveState.enabled or arm_heartbeat) - frames = [] + frames = delayed_pedal_frames[:] + delayed_pedal_frames.clear() def receive(name, values): fields = dict(values) @@ -281,7 +285,14 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag if not (fault == 'source-loss' and name == 'Yaw_Data_FD1'): accepted = safety.safety_rx_hook(joined_packet(frame)) assert bool(accepted) == (not (fault == 'rx-quality' and name == 'BrakeSysFeatures')) - frames.append(frame) + pedal = name == 'EngVehicleSpThrottle' and fault == 'gas' or name == 'EngBrakeData' and brake + if getattr(monkeypatch, 'ford_native_first_pedal', False) and pedal and name not in delayed_pedals_seen: + # The real native RX already consumed this frame above. Deliver it + # to the host on the next update, retaining native authority now. + delayed_pedals_seen.add(name) + delayed_pedal_frames.append(frame) + else: + frames.append(frame) with patch.object(native, 'rx', side_effect=receive): native.pump( @@ -290,7 +301,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag gear=1 if reverse else 3, mask=request, heartbeat=heartbeat and fault != 'heartbeat', - speed=15.0, + speed=(3.1 + .12 * ((steps // 2) % 12) / 11 if getattr(monkeypatch, "ford_speed_window", False) else 15.0), eps=1 if fault == 'temporary-eps' else 0, ) assert native.tick == steps @@ -385,7 +396,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag qualified_samples += 1 parsed_before_producer = state.to_dict() assert (main_off or state.cruiseState.available or availability_transition and not state.cruiseState.enabled) and not state.steeringPressed - assert bool(state.gasPressed) == (fault == 'gas') + assert bool(state.gasPressed) == (fault == 'gas' and not any(frame[0] == 0x204 for frame in delayed_pedal_frames)) axis, sd_state, command = drive( steps, car_state_message=published['carState'], @@ -401,6 +412,9 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag panda_state_message=panda, safety_message=wire, ) + if delayed_pedal_frames: + assert command.latActive and host.aol_card_intent.allowed_latch, 'Native-first pedal erased granted lateral intent' + assert not command.longActive, 'Native LONG withdrawal was ignored' assert state.to_dict() == parsed_before_producer, 'Authority harness changed actual parsed CarState' previous_sd.update(sd.pm.messages) control = controls.pm.messages['carControl'] @@ -493,12 +507,18 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag assert bool(command.longActive) == joined.expected_long for _ in range(30): state, axis, sd_state, command = step(engaged=True, fault='gas') + if getattr(monkeypatch, 'ford_native_first_pedal', False): + assert command.latActive and host.aol_card_intent.allowed_latch, 'Native-first gas erased lateral intent' + assert not command.longActive or joined.last_permission & 2 assert command.latActive and not command.longActive and host.aol_card_intent.allowed_latch for _ in range(30): state, axis, sd_state, command = step(engaged=True) assert command.latActive and bool(command.longActive) == joined.expected_long for _ in range(30): state, axis, sd_state, command = step(brake=True) + if getattr(monkeypatch, 'ford_native_first_pedal', False): + assert command.latActive and host.aol_card_intent.allowed_latch, 'Native-first brake erased lateral intent' + assert not command.longActive or joined.last_permission & 2 assert host.aol_card_intent.allowed_latch and command.latActive and not command.longActive if getattr(monkeypatch, 'ford_angle', False): for _ in range(20): diff --git a/openpilot/starpilot/tests/test_ford_bluepilot_aol.py b/openpilot/starpilot/tests/test_ford_bluepilot_aol.py index 3d9bbf3037..09e2918b29 100644 --- a/openpilot/starpilot/tests/test_ford_bluepilot_aol.py +++ b/openpilot/starpilot/tests/test_ford_bluepilot_aol.py @@ -24,3 +24,108 @@ def test_angle_aol_brake_reverse_fault_and_hands_free(ford_joined,monkeypatch): def test_mache_nonzero_angle_after_brake_with_hands_free(ford_joined,monkeypatch): monkeypatch.ford_nonzero = True run_actual_producer(ford_joined,monkeypatch,scenario='configuration-parity') + + +@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) +@pytest.mark.parametrize('alpha', [False, True]) +@pytest.mark.parametrize('hands_free', [True]) +@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) +def test_tight_turn_speed_window_stays_inside_native_caps(ford_joined, monkeypatch): + # Route19's first blocked frames exceed Panda's cap by using the latest + # speed instead of its six-sample minimum. Exercise real parser/native windows. + monkeypatch.ford_nonzero = True + monkeypatch.ford_speed_window = True + monkeypatch.ford_test_curvature = .03 + run_actual_producer(ford_joined, monkeypatch, scenario='configuration-parity') + + +@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) +@pytest.mark.parametrize('alpha', [False, True]) +@pytest.mark.parametrize('hands_free', [True]) +@pytest.mark.parametrize('topology', ['automatic']) +def test_real_rejected_packet_resynchronizes_without_permission_bypass(ford_joined, monkeypatch): + from types import SimpleNamespace + from opendbc.car import structs + from opendbc.safety.tests.libsafety import libsafety_py + + joined = ford_joined + native, safety = joined.native, joined.safety + native.arm() + adapter = joined.host.CI.CC.bp_lat + inputs = adapter.host.inputs + blocked = 0 + monkeypatch.setattr(inputs, 'update', lambda: None) + monkeypatch.setattr(inputs, 'lateral_snapshot', lambda speed: None) + # Isolate the adapter recovery while preserving actual physical native authority. + # The full caller matrix above retains real wire/inventory/freshness qualification. + monkeypatch.setattr(inputs, 'native_permission', lambda cp, max_age_ns: bool(safety.aol_get_permission_mask() & 1)) + CC = structs.CarControl(enabled=True, latActive=True) + CC.actuators.curvature = .0001 + state = structs.CarState(canValid=True, vEgoRaw=15., gearShifter=structs.CarState.GearShifter.drive) + state.cruiseState.available = True + CS = SimpleNamespace(out=state, lat_ctl_lim_stat=0) + rejected = recovered = False + neutral_after_rejection = False + for frame in range(180): + native.pump(1, mask=1, speed=15.) + now = (1_000_000 + native.tick * 10_000) * 1000 + from openpilot.cereal import messaging + panda = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now) + panda.pandaStates[0].safetyTxBlocked = blocked + inputs.sm.update_msgs(now / 1e9, [panda.as_reader()]) + packets = [] + if frame % 5 == 0: + packets += adapter.step_lateral(CC, CS, native.packer, joined.host.CI.CC.CAN, frame, now) + if frame % 3 == 0: + packets += adapter.step_lka(native.packer, joined.host.CI.CC.CAN, now) + for address, original, bus in packets: + data = bytearray(original) + active = address == 0x3D6 and (data[0] >> 4) & 7 == 1 + if active and frame >= 80 and not rejected: + data[1] ^= 1 # One actual bad checksum; native must reject it. + assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)) + blocked += 1 + rejected = True + continue + accepted = safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)) + assert accepted, (frame, hex(address), data.hex()) + if rejected and address == 0x3D6: + neutral_after_rejection |= not active + recovered |= active and neutral_after_rejection + assert rejected and neutral_after_rejection and recovered + # Host recovery never replaces native authorization. + safety.aol_set_host_request(0) + native.pump(5, mask=0, speed=15.) + now = (1_000_000 + native.tick * 10_000) * 1000 + neutral = adapter.step_lateral(CC, CS, native.packer, joined.host.CI.CC.CAN, 180, now)[0] + assert (neutral[1][0] >> 4) & 7 == 0 + + +@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18), (CAR.FORD_ESCAPE_MK4, 32), (CAR.FORD_F_150_MK14, 66)]) +@pytest.mark.parametrize('alpha', [True]) +@pytest.mark.parametrize('hands_free', [True]) +@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) +def test_native_first_pedal_preserves_only_granted_lateral(ford_joined, monkeypatch): + monkeypatch.ford_nonzero = True + monkeypatch.ford_native_first_pedal = True + run_actual_producer(ford_joined, monkeypatch, scenario='configuration-parity') + + +@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18)]) +@pytest.mark.parametrize('alpha', [True]) +@pytest.mark.parametrize('hands_free', [True]) +@pytest.mark.parametrize('topology', ['automatic', 'manual-multi']) +def test_early_pedal_admission_requires_exact_marked_angle_long_profile(ford_joined): + from opendbc.car import structs + from openpilot.starpilot.car.ford.aol import early_native_long_withdrawal_supported + cp = ford_joined.host.CP + assert early_native_long_withdrawal_supported(cp) + for field, value in (('alternativeExperience', 0), ('openpilotLongitudinalControl', False), + ('passive', True), ('dashcamOnly', True), ('brand', 'hyundai')): + changed = structs.CarParams(**cp.to_dict()) + setattr(changed, field, value) + assert not early_native_long_withdrawal_supported(changed) + for mask in (128, 1, 16): + changed = structs.CarParams(**cp.to_dict()) + changed.safetyConfigs[-1].safetyParam ^= mask + assert not early_native_long_withdrawal_supported(changed) diff --git a/prebuilt.json b/prebuilt.json index 5abc5430a2..a99a54178a 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "28e81fb375b1a055d806280af30b92101095fcc3a11d1bb200e9a645a3d84d25", "schema_version": 1} +{"protected_inventory_sha256": "689ef29810231d81be86a79ecf7c83429f53f5bee5e38ae268e9a3ae977cfcbb", "schema_version": 1}