Wait for Bolt steering transmit receipts before advancing counters

opendbc_repo/opendbc/car/gm/carcontroller.py

@@ -307,6 +307,8 @@ class CarController(CarControllerBase):
                                 CP.carFingerprint == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL and is_bolt_pedal_profile(CP) else None)
 
     self.lka_steering_cmd_counter = 0
+    self.bolt_steering_pending: tuple[int, int] | None = None
+    self.bolt_steering_receipt_seeded = False
     self.lka_icon_status_last = (False, False)
 
     self.hybrid_profile = malibu_hybrid_profile(self.CP)
@@ -455,6 +457,22 @@ class CarController(CarControllerBase):
       self.regen_paddle_pressed = commanded_accel < -.02
     return self.regen_paddle_pressed
 
+  def observe_steering_receipts(self, can_packets):
+    for stamp, packets in can_packets:
+      for address, raw, bus in packets:
+        if address != 0x180 or len(raw) != 4 or bus not in (128, 192):
+          continue
+        counter = (raw[0] >> 4) & 3
+        pending = self.bolt_steering_pending
+        if pending is not None:
+          if counter != pending[0] or stamp < pending[1]:
+            continue
+          self.bolt_steering_pending = None
+          # A rejected packet never reached the bus; retry its counter.
+          self.lka_steering_cmd_counter = (counter + 1) % 4 if bus == 128 else counter
+        elif bus == 128 and not self.bolt_steering_receipt_seeded:
+          self.lka_steering_cmd_counter = (counter + 1) % 4
+
   def update(self, CC, CS, now_nanos, *, startup_keepalive=False):
     if self.bolt_acc_brake_clock is not None and getattr(CS, "bolt_acc_pedal_friction_unavailable", False):
       # Do not keep requesting a rejected secondary brake or retry after a flag flap.
@@ -622,18 +640,17 @@ class CarController(CarControllerBase):
       if CS.loopback_lka_steering_cmd_ts_nanos == 0 or out_of_sync:
         steer_step = self.params.STEER_STEP
 
-    if self.bolt_steering_ack_counter:
-      if CS.loopback_lka_steering_cmd_updated:
-        self.lka_steering_cmd_counter = (CS.loopback_lka_steering_cmd_counter + 1) % 4
-    else:
+    if not self.bolt_steering_ack_counter:
       self.lka_steering_cmd_counter += 1 if CS.loopback_lka_steering_cmd_updated else 0
 
     # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we
     # received the ASCMLKASteeringCmd loopback confirmation too recently
     last_lka_steer_msg_ms = (now_nanos - CS.loopback_lka_steering_cmd_ts_nanos) * 1e-6
-    if (self.frame - self.last_steer_frame) >= steer_step and last_lka_steer_msg_ms > MIN_STEER_MSG_INTERVAL_MS:
+    if ((self.frame - self.last_steer_frame) >= steer_step and last_lka_steer_msg_ms > MIN_STEER_MSG_INTERVAL_MS and
+        (not self.bolt_steering_ack_counter or self.bolt_steering_pending is None)):
       # Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus
-      if CS.loopback_lka_steering_cmd_ts_nanos == 0 and not self.bolt_pedal_removed and not (self.hybrid_profile is not None and self.hybrid_profile.removed):
+      if (CS.loopback_lka_steering_cmd_ts_nanos == 0 and not self.bolt_steering_receipt_seeded and
+          not self.bolt_pedal_removed and not (self.hybrid_profile is not None and self.hybrid_profile.removed)):
         self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1
 
       if lat_active and not (self.ordinary_cc_profile and CC.enabled and not CS.out.cruiseState.enabled and not aol_lateral):
@@ -648,6 +665,9 @@ class CarController(CarControllerBase):
       self.apply_torque_last = apply_torque
       idx = self.lka_steering_cmd_counter % 4
       can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_torque, idx, lat_active))
+      if self.bolt_steering_ack_counter:
+        self.bolt_steering_pending = (idx, now_nanos)
+        self.bolt_steering_receipt_seeded = True
       if _projection is not None:
         _projection.emit('gm.ASCMLKASteeringCmd', apply_torque, lat_active, can_sends[-1])
       CS.steering_authority.command(now_nanos, lat_active)

opendbc_repo/opendbc/car/gm/carstate.py

@@ -1,4 +1,4 @@
-from opendbc.car.gm.steering_authority import SteeringAuthority
+from opendbc.car.gm.steering_authority import SteeringAuthority, STEERING_COMMAND_TIMEOUT_NS
 from opendbc.car.gm.values import is_volt_flashed_profile
 from opendbc.car.gps import CarGpsTracker, get_car_gps_config
 from opendbc.car.gm.values import gm_control_word, is_volt_one_pedal, camera_acc_pedal_profile, BrakeSource, volt_cc_pedal_profile
@@ -63,6 +63,7 @@ class CarState(CarStateBase):
     self.cluster_min_speed = CV.KPH_TO_MS / 2.
 
     self.loopback_lka_steering_cmd_updated = False
+    self.bolt_steering_pending: tuple[int, int] | None = None
     self.loopback_lka_steering_cmd_counter = 0
     self.loopback_lka_steering_cmd_ts_nanos = 0
     self.pt_lka_steering_cmd_counter = 0
@@ -503,7 +504,9 @@ class CarState(CarStateBase):
 
     # 0 inactive, 1 active, 2 temporarily limited, 3 failed
     self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
-    ret.steerFaultTemporary = self.lkas_status == 2
+    pending = self.bolt_steering_pending
+    receipt_timeout = pending is not None and pt_cp._last_update_nanos - pending[1] > STEERING_COMMAND_TIMEOUT_NS
+    ret.steerFaultTemporary = self.lkas_status == 2 or receipt_timeout
     ret.steerFaultPermanent = self.lkas_status == 3
 
     # 1 - open, 0 - closed

opendbc_repo/opendbc/car/gm/interface.py

@@ -51,6 +51,9 @@ class CarInterface(CarInterfaceBase):
     return super().apply(c, now_nanos)
 
   def update(self, can_packets):
+    if self.CC.bolt_steering_ack_counter:
+      self.CC.observe_steering_receipts(can_packets)
+      self.CS.bolt_steering_pending = self.CC.bolt_steering_pending
     self.CS.steering_authority.observe(can_packets)
     if malibu_hybrid_profile(self.CP) is not None:
       self.CS.hybrid_buttons.observe_packets(can_packets)

opendbc_repo/opendbc/car/gm/tests/test_bolt_steering_receipts.py

@@ -0,0 +1,79 @@
+from types import SimpleNamespace
+import pytest
+from opendbc.car import structs
+from opendbc.car.gm.interface import CarInterface
+from opendbc.car.gm.tests.test_bolt_pedal import params
+from opendbc.car.gm.values import CAR
+from openpilot.starpilot.aol.tests.test_gm_held_native import parse
+
+
+def setup():
+  cp = params(CAR.CHEVROLET_BOLT_CC_2018_2021, pedal=True, camera=True)
+  cp.alternativeExperience = 32
+  ci = CarInterface(cp)
+  shell = SimpleNamespace(CP=cp, last_apply=1_360_000_000)
+  for tick in range(37):
+    now = 1_000_000_000 + tick * 10_000_000
+    ci.update([(now, parse(shell, now, counter=tick % 4, packets_only=True))])
+  control = structs.CarControl(latActive=True)
+  control.actuators.torque = .1
+  return ci, shell, control.as_reader()
+
+
+def step(ci, shell, control, now, receipt=None):
+  messages = []
+  for tick in range(shell.last_apply + 10_000_000, now + 1, 10_000_000):
+    packets = parse(shell, tick, counter=(tick // 10_000_000) % 4, packets_only=True)
+    batches = [receipt] if receipt is not None and tick == now else []
+    batches.append((tick, packets))
+    cs = ci.update(batches)
+    _, emitted = ci.apply(control, tick)
+    messages.extend(msg for msg in emitted if msg[0] == 0x180 and msg[2] == 0)
+  shell.last_apply = now
+  return cs, messages
+
+
+def first():
+  ci, shell, control = setup()
+  cs, messages = step(ci, shell, control, 1_400_000_000)
+  assert cs.canValid and len(messages) == 1 and messages[0][1][0] & 8
+  return ci, shell, control, messages[0]
+
+
+def test_delayed_receipt_never_retransmits_an_unconfirmed_counter():
+  ci, shell, control, sent = first()
+  counter = (sent[1][0] >> 4) & 3
+  for now in [1_420_000_000, 1_430_000_000, 1_440_000_000]:
+    _, messages = step(ci, shell, control, now)
+    assert not messages
+  receipt = (1_441_000_000, [(sent[0], sent[1], 128)])
+  _, messages = step(ci, shell, control, 1_460_000_000, receipt)
+  assert len(messages) == 1 and (messages[0][1][0] >> 4) & 3 == (counter + 1) % 4
+
+
+@pytest.mark.parametrize('kind', ['wrong_counter', 'wrong_bus', 'old', 'short'])
+def test_unrelated_receipt_does_not_release_pending_command(kind):
+  ci, shell, control, sent = first()
+  raw = bytearray(sent[1])
+  if kind == 'wrong_counter':
+    raw[0] ^= 0x10
+  receipt = (1_399_000_000 if kind == 'old' else 1_421_000_000,
+             [(sent[0], bytes(raw[:3] if kind == 'short' else raw), 130 if kind == 'wrong_bus' else 128)])
+  _, messages = step(ci, shell, control, 1_440_000_000, receipt)
+  assert not messages and ci.CC.bolt_steering_pending is not None
+
+
+def test_explicit_native_rejection_retries_the_same_counter():
+  ci, shell, control, sent = first()
+  receipt = (1_421_000_000, [(sent[0], sent[1], 192)])
+  _, messages = step(ci, shell, control, 1_440_000_000, receipt)
+  assert len(messages) == 1 and messages[0][1][0] & 0x30 == sent[1][0] & 0x30
+
+
+def test_missing_receipt_is_reported_and_late_confirmation_recovers():
+  ci, shell, control, sent = first()
+  cs, messages = step(ci, shell, control, 1_510_000_000)
+  assert cs.steerFaultTemporary and not messages
+  receipt = (1_521_000_000, [(sent[0], sent[1], 128)])
+  cs, messages = step(ci, shell, control, 1_540_000_000, receipt)
+  assert not cs.steerFaultTemporary and len(messages) == 1

opendbc_repo/opendbc/car/gm/tests/test_bolt_volt_configurations.py

@@ -73,23 +73,22 @@ class TestBoltVoltConfigurations(unittest.TestCase):
             cp = pedal_params(identity, setting=True, pedal=True, alpha_long=alpha, removed=removed)
             if identity == CAR.CHEVROLET_BOLT_CC_2018_2021 and not removed:
               self.assertEqual(cp.safetyConfigs[0].safetyParam, 0x9D)
-            cs = CarState(cp)
-            parsers = cs.get_can_parsers(cp)
+            ci = CarInterface(cp)
+            controller = ci.CC
             packer = CANPacker(DBC[identity][Bus.pt])
-            controller = CarController(DBC[identity], cp)
             control = structs.CarControl()
-            for step, (received, expected) in enumerate((([3], 0), ([0, 1], 2), ([0, 0], 1),
-                                                        ([1], 2), ([], 2), ([2, 3], 0), ([0], 1))):
+            for step, (received, expected) in enumerate((([3], 0), ([0], 1), ([], None),
+                                                        ([1], 2), ([2, 2], 3), ([3], 0), ([0], 1))):
               now = 1_000_000_000 + step * 100_000_000
               packets = [packer.make_can_msg("ASCMLKASteeringCmd", 128, {"RollingCounter": value}) for value in received]
-              for parser in parsers.values():
-                parser.update([(now - 20_000_000, packets)])
-              cs.out = cs.update(parsers).as_reader()
-              self.assertEqual(cs.loopback_lka_steering_cmd_updated, bool(received))
+              ci.update([(now - 20_000_000, packets)])
+              self.assertEqual(ci.CS.loopback_lka_steering_cmd_updated, bool(received))
               controller.frame = 20 + step * 10
-              _, commands = controller.update(control.as_reader(), cs, now)
+              _, commands = ci.apply(control.as_reader(), now)
               steering = [message for message in commands if message[0] == 0x180 and message[2] == 0]
-              self.assertEqual(len(steering), 1)
+              self.assertEqual(len(steering), int(expected is not None))
+              if expected is None:
+                continue
               data = steering[0][1]
               self.assertEqual((data[0] >> 4) & 3, expected)
               self.assertFalse(data[0] & 8)

openpilot/starpilot/aol/tests/gm_bolt_recovery.py

@@ -105,6 +105,7 @@ def exercise_bolt_cancel_recovery(self, identity, removed, *, critical=None, com
     gen2_drive_scene = identity == CAR.CHEVROLET_BOLT_CC_2022_2023 and word == 0x19D and not pedal_scene
     trajectory = set()
     sent = []
+    steering_loopback = []
     cancel_start, pause_start = (40, 32) if pedal_scene else (10, 10)
     main_start, ticks = (646, 652) if pedal_scene else (26, 36)
     temporary_eps_tick, stale_axis_tick = (-1, -1) if pedal_scene else (20, 23)
@@ -235,7 +236,8 @@ def exercise_bolt_cancel_recovery(self, identity, removed, *, critical=None, com
         if communications is not None:
           packets = [packet for packet in packets if packet[0] != 0xBD]
           packets.append(packer.make_can_msg('EBCMRegenPaddle', 0, {'RegenPaddle': int(regen)}))
-        cs = ci.update([(now, packets)])
+        cs = ci.update([(now - 5_000_000, steering_loopback), (now, packets)])
+        steering_loopback = []
         if communications is not None:
           self.assertEqual(cs.regenBraking, regen, msg=ctx)
           self.assertEqual(cs.brakePressed, brake, msg=ctx)
@@ -573,6 +575,8 @@ def exercise_bolt_cancel_recovery(self, identity, removed, *, critical=None, com
             last_paddle_feeds[message[0]] = raw
           else:
             self.assertTrue(native('tx', message, now // 1000 + 1), ((tick, message), ctx))
+            if message[0] == 0x180:
+              steering_loopback.append((message[0], bytes(message[1]), 128))
             if pedal_scene and message[0] == 0x1E1:
               self.assertEqual(message[2], 0, msg=ctx)
               self.assertEqual(bytes(message[1]), button_bytes(CruiseButtons.CANCEL, (tick + 1) % 4), msg=ctx)

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "2a8df7b619523551dd43e8fb18af97eb7692ac868dbf8e72b5d4978ad076bace", "schema_version": 1}
+{"protected_inventory_sha256": "c0bbe3fed66c4fab365737818d8d026731792c31a4be48cd932baee04115225c", "schema_version": 1}