diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index a11924caf3..5f85a0c14c 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/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) diff --git a/opendbc_repo/opendbc/car/gm/carstate.py b/opendbc_repo/opendbc/car/gm/carstate.py index 7e8a2c092a..4b5d50495f 100644 --- a/opendbc_repo/opendbc/car/gm/carstate.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index ccacd5cc21..c639d1d472 100644 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/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) diff --git a/opendbc_repo/opendbc/car/gm/tests/test_bolt_steering_receipts.py b/opendbc_repo/opendbc/car/gm/tests/test_bolt_steering_receipts.py new file mode 100644 index 0000000000..5880f2b85a --- /dev/null +++ b/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 diff --git a/opendbc_repo/opendbc/car/gm/tests/test_bolt_volt_configurations.py b/opendbc_repo/opendbc/car/gm/tests/test_bolt_volt_configurations.py index 32907f4dbf..781aacba18 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_bolt_volt_configurations.py +++ b/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) diff --git a/openpilot/starpilot/aol/tests/gm_bolt_recovery.py b/openpilot/starpilot/aol/tests/gm_bolt_recovery.py index b41fe54035..9613353fde 100644 --- a/openpilot/starpilot/aol/tests/gm_bolt_recovery.py +++ b/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) diff --git a/prebuilt.json b/prebuilt.json index a6c81c02d7..3bdf26d244 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "2a8df7b619523551dd43e8fb18af97eb7692ac868dbf8e72b5d4978ad076bace", "schema_version": 1} +{"protected_inventory_sha256": "c0bbe3fed66c4fab365737818d8d026731792c31a4be48cd932baee04115225c", "schema_version": 1}