Resynchronize Ford angle only after steering rejections

opendbc_repo/opendbc/bluepilot_lateral/__init__.py

@@ -28,6 +28,7 @@ class BluePilotLateral:
     self.pending_generation = 0
     self.announced_generation = -1
     self.sent_generation = -2
+    self.steering_rejection_ns = 0
     self.apply_curvature_last = 0.0
 
   def observe_can(self, can_packets, main_bus):
@@ -40,7 +41,14 @@ class BluePilotLateral:
       if timestamp <= 0:
         continue
       for address, data, source in frames:
-        if source != (main_bus | 0x80) or len(data) != 8:
+        if len(data) != 8:
+          continue
+        if source == (main_bus | 0xC0) and address in (control_address, 0x3CA):
+          if timestamp > self.steering_rejection_ns:
+            self.steering_rejection_ns = timestamp
+            self.host.steering_rejected = True
+          continue
+        if source != (main_bus | 0x80):
           continue
         if address == control_address and data == self.control_return_payload and timestamp > self.control_returned_ns:
           self.control_return_payload = None

opendbc_repo/opendbc/bluepilot_lateral/hosts/starpilot.py

@@ -88,7 +88,7 @@ class Host:
     self.cp = cp
     self.controller = AngleController(low_gain=low, high_gain=high)
     self.inputs = None
-    self.tx_blocked = None
+    self.steering_rejected = False
 
   def update(self, CC, CS):
     if self.inputs is not None:
@@ -107,17 +107,10 @@ 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
+    # Only an actual steering/shadow rejection invalidates its accepted path
+    # and counter history. Unrelated ACC/UI rejections do not reset steering.
+    rejected, self.steering_rejected = self.steering_rejected, False
+    ready = ready and not rejected
     speed_min = speed_max = None
     speed_window = getattr(CS, "bluepilot_speed_window", None)
     if speed_window is not None:

openpilot/starpilot/tests/test_ford_bluepilot_aol.py

@@ -85,6 +85,8 @@ def test_real_rejected_packet_resynchronizes_without_permission_bypass(ford_join
         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
+        # Feed the real rejected CAN return, not just the aggregate counter.
+        adapter.observe_can([(now, [(address, bytes(data), bus | 0xC0)])], bus)
         rejected = True
         continue
       accepted = safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data))
@@ -379,3 +381,56 @@ def test_stock_forwarding_returns_cannot_starve_angle_reengagement(ford_joined,
   assert neutral_seen
   assert active[0] > 3 and active[1] > 10, active
   assert safety.aol_get_permission_mask() == 1
+
+
+@pytest.mark.parametrize('identity', [(CAR.FORD_MUSTANG_MACH_E_MK1, 18), (CAR.FORD_ESCAPE_MK4, 32)])
+@pytest.mark.parametrize('alpha', [False, True])
+@pytest.mark.parametrize('hands_free', [True])
+@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
+def test_unrelated_native_rejection_does_not_reset_granted_angle(ford_joined, monkeypatch):
+  from types import SimpleNamespace
+  from opendbc.car import structs
+  from opendbc.car.ford import fordcan
+  from opendbc.safety.tests.libsafety import libsafety_py
+  from openpilot.cereal import messaging
+
+  joined = ford_joined
+  native, safety = joined.native, joined.safety
+  native.arm()
+  adapter = joined.host.CI.CC.bp_lat
+  inputs = adapter.host.inputs
+  monkeypatch.setattr(inputs, 'update', lambda: None)
+  monkeypatch.setattr(inputs, 'lateral_snapshot', lambda speed, *, angle=False: None)
+  monkeypatch.setattr(inputs, 'native_permission', lambda cp, max_age_ns: bool(safety.aol_get_permission_mask() & 1))
+  control = structs.CarControl(enabled=True, latActive=True)
+  control.actuators.curvature = .0001
+  state = structs.CarState(canValid=True, vEgoRaw=15., gearShifter=structs.CarState.GearShifter.drive)
+  state.cruiseState.available = True
+  source = SimpleNamespace(out=state, lat_ctl_lim_stat=0)
+  blocked = active_after = 0
+  rejected = False
+  for frame in range(180):
+    native.pump(1, mask=1, speed=15.)
+    now = (1_000_000 + native.tick * 10_000) * 1000
+    if frame == 80:
+      # LONG is independently denied. Its real rejection must not erase LAT.
+      message = fordcan.create_acc_msg(native.packer, joined.host.CI.CC.CAN, True, 1., 1., False, False, v_ego_kph=50.)
+      address, data, bus = message
+      assert not safety.get_controls_allowed()
+      assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus - joined.offset, data))
+      assert safety.aol_get_permission_mask() == 1
+      adapter.observe_can([(now, [(address, data, bus | 0xC0)])], bus)
+      blocked += 1
+      rejected = True
+    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 = adapter.step_lateral(control, source, native.packer, joined.host.CI.CC.CAN, frame, now)
+    packets += adapter.step_lka(native.packer, joined.host.CI.CC.CAN, now)
+    for address, data, bus in packets:
+      assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus - joined.offset, data)), (frame, hex(address), data.hex())
+      if address in (0x3D3, 0x3D6) and frame >= 80:
+        active = ((data[4] >> 2) & 7) if address == 0x3D3 else ((data[0] >> 4) & 7)
+        assert active == 1, 'Unrelated LONG rejection reset independently granted steering'
+        active_after += 1
+  assert rejected and active_after > 10

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "0c59100a947bc75cc7f36a379478e16524c4fd5bc3bc546f02ca858b76be9ac2", "schema_version": 1}
+{"protected_inventory_sha256": "7731ca4f626838cad0d3b821f88a5b64f29c9b433a814d81df799b6fa4f042d1", "schema_version": 1}