diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 03351fdb9..2604753eb 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -15,13 +15,6 @@ from openpilot.selfdrive.car import gen_empty_fingerprint FRAME_FINGERPRINT = 100 # 1s -def get_one_can(logcan): - while True: - can = messaging.recv_one_retry(logcan) - if len(can.can) > 0: - return can - - def load_interfaces(brand_names): ret = {} for brand_name in brand_names: @@ -48,7 +41,7 @@ interface_names = _get_interface_names() interfaces = load_interfaces(interface_names) -def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]: +def can_fingerprint(can_recv: Callable) -> tuple[str | None, dict[int, dict]]: finger = gen_empty_fingerprint() candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1 frame = 0 @@ -56,34 +49,35 @@ def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]: done = False while not done: - a = next_can() + # can_recv() may return zero or multiple packets, so we increment frame for each one we receive + can_packets = can_recv() + for can_packet in can_packets: + for can in can_packet.can: + # The fingerprint dict is generated for all buses, this way the car interface + # can use it to detect a (valid) multipanda setup and initialize accordingly + if can.src < 128: + if can.src not in finger: + finger[can.src] = {} + finger[can.src][can.address] = len(can.dat) - for can in a.can: - # The fingerprint dict is generated for all buses, this way the car interface - # can use it to detect a (valid) multipanda setup and initialize accordingly - if can.src < 128: - if can.src not in finger: - finger[can.src] = {} - finger[can.src][can.address] = len(can.dat) + for b in candidate_cars: + # Ignore extended messages and VIN query response. + if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8): + candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b]) + # if we only have one car choice and the time since we got our first + # message has elapsed, exit for b in candidate_cars: - # Ignore extended messages and VIN query response. - if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8): - candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b]) + if len(candidate_cars[b]) == 1 and frame > FRAME_FINGERPRINT: + # fingerprint done + car_fingerprint = candidate_cars[b][0] - # if we only have one car choice and the time since we got our first - # message has elapsed, exit - for b in candidate_cars: - if len(candidate_cars[b]) == 1 and frame > FRAME_FINGERPRINT: - # fingerprint done - car_fingerprint = candidate_cars[b][0] + # bail if no cars left or we've been waiting for more than 2s + failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > FRAME_FINGERPRINT) or frame > 200 + succeeded = car_fingerprint is not None + done = failed or succeeded - # bail if no cars left or we've been waiting for more than 2s - failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > FRAME_FINGERPRINT) or frame > 200 - succeeded = car_fingerprint is not None - done = failed or succeeded - - frame += 1 + frame += 1 return car_fingerprint, finger @@ -139,7 +133,7 @@ def fingerprint(logcan, sendcan, set_obd_multiplexing, num_pandas, cached_params # CAN fingerprint # drain CAN socket so we get the latest messages messaging.drain_sock_raw(logcan) - car_fingerprint, finger = can_fingerprint(lambda: get_one_can(logcan)) + car_fingerprint, finger = can_fingerprint(lambda: messaging.drain_sock(logcan, wait_for_one=True)) exact_match = True source = car.CarParams.FingerprintSource.can diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index a8afa0dec..d00998acb 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -14,7 +14,7 @@ from openpilot.common.swaglog import cloudlog, ForwardingHandler from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.selfdrive.car import DT_CTRL, carlog -from openpilot.selfdrive.car.car_helpers import get_car, get_one_can +from openpilot.selfdrive.car.car_helpers import get_car from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.controls.lib.events import Events @@ -58,7 +58,10 @@ class Car: if CI is None: # wait for one pandaState and one CAN packet print("Waiting for CAN messages...") - get_one_can(self.can_sock) + while True: + can = messaging.recv_one_retry(self.can_sock) + if len(can.can) > 0: + break experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) diff --git a/selfdrive/car/tests/test_can_fingerprint.py b/selfdrive/car/tests/test_can_fingerprint.py index f236986d8..01033f7b3 100644 --- a/selfdrive/car/tests/test_can_fingerprint.py +++ b/selfdrive/car/tests/test_can_fingerprint.py @@ -17,7 +17,7 @@ class TestCanFingerprint: fingerprint_iter = iter([can]) empty_can = messaging.new_message('can', 0) - car_fingerprint, finger = can_fingerprint(lambda: next(fingerprint_iter, empty_can)) # noqa: B023 + car_fingerprint, finger = can_fingerprint(lambda: [next(fingerprint_iter, empty_can)]) # noqa: B023 assert car_fingerprint == car_model assert finger[0] == fingerprint @@ -54,8 +54,8 @@ class TestCanFingerprint: def test(): nonlocal frames frames += 1 - return can # noqa: B023 + return [can] # noqa: B023 car_fingerprint, _ = can_fingerprint(test) assert car_fingerprint == car_model - assert frames == expected_frames + 2# TODO: fix extra frames + assert frames == expected_frames + 2 # TODO: fix extra frames diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 609da3967..b25f318b4 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -335,7 +335,7 @@ def card_fingerprint_callback(rc, pm, msgs, fingerprint): m = canmsgs.pop(0) rc.send_sync(pm, "can", m.as_builder().to_bytes()) - rc.wait_for_next_recv(False) + rc.wait_for_next_recv(True) def get_car_params_callback(rc, pm, msgs, fingerprint):