diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 6a131c77de..c344852a66 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -77,6 +77,7 @@ interfaces = load_interfaces(interface_names) # **** for use live only **** def fingerprint(logcan, sendcan, num_pandas): + # return None, {}, "", [], "source", True fixed_fingerprint = os.environ.get('FINGERPRINT', "") skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) ecu_rx_addrs = set() @@ -110,6 +111,7 @@ def fingerprint(logcan, sendcan, num_pandas): vin, vin_rx_addr = VIN_UNKNOWN, 0 exact_fw_match, fw_candidates, car_fw = True, set(), [] cached = False + vin, vin_rx_addr = VIN_UNKNOWN, 0 if not is_valid_vin(vin): cloudlog.event("Malformed VIN", vin=vin, error=True) @@ -128,39 +130,39 @@ def fingerprint(logcan, sendcan, num_pandas): car_fingerprint = None done = False - # drain CAN socket so we always get the latest messages - messaging.drain_sock_raw(logcan) - - while not done: - a = get_one_can(logcan) - - 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: - 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 - - frame += 1 - + # # drain CAN socket so we always get the latest messages + # messaging.drain_sock_raw(logcan) + # + # while not done: + # a = get_one_can(logcan) + # + # 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: + # 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 + # + # frame += 1 + # exact_match = True source = car.CarParams.FingerprintSource.can @@ -188,9 +190,9 @@ def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): CarInterface, CarController, CarState = interfaces[candidate] CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed) - CP.carVin = vin - CP.carFw = car_fw - CP.fingerprintSource = source + # CP.carVin = vin + # CP.carFw = car_fw + # CP.fingerprintSource = source CP.fuzzyFingerprint = not exact_match return CarInterface(CP, CarController, CarState), CP diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index f85b7f6b7d..acac5061d0 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -2,6 +2,7 @@ from collections import defaultdict from typing import Any, Dict, List, Set from tqdm import tqdm +import time import panda.python.uds as uds from cereal import car @@ -182,7 +183,7 @@ def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]: ecu_responses = set() for obd_multiplexing in queries: - set_obd_multiplexing(params, obd_multiplexing) + # set_obd_multiplexing(params, obd_multiplexing) for query in queries[obd_multiplexing]: ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1)) return ecu_responses @@ -206,7 +207,10 @@ def get_brand_ecu_matches(ecu_rx_addrs): def set_obd_multiplexing(params: Params, obd_multiplexing: bool): + print('SETTING MULTIPLEXING') + return if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing: + print('SET MULTIPLEXING') cloudlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}") params.remove("ObdMultiplexingChanged") params.put_bool("ObdMultiplexingEnabled", obd_multiplexing) @@ -232,6 +236,7 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pand def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, debug=False, progress=False): + t = time.perf_counter() versions = VERSIONS.copy() params = Params() @@ -270,6 +275,8 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, # Get versions and build capnp list to put into CarParams car_fw = [] requests = [(brand, config, r) for brand, config, r in REQUESTS if query_brand is None or brand == query_brand] + # print('ready to query', time.perf_counter() - t) + t = time.perf_counter() for addr in tqdm(addrs, disable=not progress): for addr_chunk in chunks(addr): for brand, config, r in requests: @@ -277,15 +284,16 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, if r.bus > num_pandas * 4 - 1: continue - # Toggle OBD multiplexing for each request - if r.bus % 4 == 1: - set_obd_multiplexing(params, r.obd_multiplexing) + # # Toggle OBD multiplexing for each request + # if r.bus % 4 == 1: + # set_obd_multiplexing(params, r.obd_multiplexing) try: addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and (len(r.whitelist_ecus) == 0 or ecu_types[(b, a, s)] in r.whitelist_ecus)] if addrs: + print(addrs) query = IsoTpParallelQuery(sendcan, logcan, r.bus, addrs, r.request, r.response, r.rx_offset, debug=debug) for (tx_addr, sub_addr), version in query.get_data(timeout).items(): f = car.CarParams.CarFw.new_message() @@ -307,6 +315,8 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, except Exception: cloudlog.exception("FW query exception") + # print('query took', time.perf_counter() - t) + return car_fw diff --git a/selfdrive/car/tests/test_fw_fingerprint_metrics.py b/selfdrive/car/tests/test_fw_fingerprint_metrics.py index 67c1438724..4afa48ff1b 100755 --- a/selfdrive/car/tests/test_fw_fingerprint_metrics.py +++ b/selfdrive/car/tests/test_fw_fingerprint_metrics.py @@ -16,7 +16,6 @@ ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} class TestFwFingerprint(unittest.TestCase): - # TODO: test multiple pandas @parameterized.expand([(1,), (2,)]) def test_fw_query_metrics(self, num_pandas): for brand, config in FW_QUERY_CONFIGS.items(): @@ -32,9 +31,9 @@ class TestFwFingerprint(unittest.TestCase): for r in requests: total_time += 0.1 - if r.obd_multiplexing != obd_multiplexing and r.bus % 4 == 1: - obd_multiplexing = r.obd_multiplexing - total_time += 0.1 + # if r.obd_multiplexing != obd_multiplexing and r.bus % 4 == 1: + # obd_multiplexing = r.obd_multiplexing + # total_time += 0.1 total_time = round(total_time, 2) self.assertLessEqual(total_time, 1.1) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1391d6570c..5ebd34331c 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import os +import time import math from typing import SupportsFloat @@ -98,9 +99,13 @@ class Controls: print("Waiting for CAN messages...") get_one_can(self.can_sock) + t = time.perf_counter() num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) + print('took {} to get num pandas'.format(time.perf_counter() - t)) + t = time.perf_counter() experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") and not is_release_branch() self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) + print('took {} to get car'.format(time.perf_counter() - t)) else: self.CI, self.CP = CI, CI.CP diff --git a/selfdrive/controls/tests/test_startup.py b/selfdrive/controls/tests/test_startup.py index 6d487fce10..afbc7b25a7 100755 --- a/selfdrive/controls/tests/test_startup.py +++ b/selfdrive/controls/tests/test_startup.py @@ -39,31 +39,8 @@ CX5_FW_VERSIONS = [ class TestStartup(unittest.TestCase): - @parameterized.expand([ - # TODO: test EventName.startup for release branches - - # officially supported car - (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS, "toyota"), - (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS, "toyota"), - - # dashcamOnly car - (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS, "mazda"), - (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS, "mazda"), - - # unrecognized car with no fw - (EventName.startupNoFw, None, None, ""), - (EventName.startupNoFw, None, None, ""), - - # unrecognized car - (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), - (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), - - # fuzzy match - (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), - (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), - ]) @with_processes(['controlsd']) - def test_startup_alert(self, expected_event, car_model, fw_versions, brand): + def test_startup_time(self): # TODO: this should be done without any real sockets controls_sock = messaging.sub_sock("controlsState") @@ -74,42 +51,47 @@ class TestStartup(unittest.TestCase): params.put_bool("Passive", False) params.put_bool("OpenpilotEnabledToggle", True) - # Build capnn version of FW array - if fw_versions is not None: - car_fw = [] - cp = car.CarParams.new_message() - for ecu, addr, subaddress, version in fw_versions: - f = car.CarParams.CarFw.new_message() - f.ecu = ecu - f.address = addr - f.fwVersion = version - f.brand = brand - - if subaddress is not None: - f.subAddress = subaddress - - car_fw.append(f) - cp.carVin = "1" * 17 - cp.carFw = car_fw - params.put("CarParamsCache", cp.to_bytes()) + # # Build capnp version of FW array + # if fw_versions is not None: + # car_fw = [] + # cp = car.CarParams.new_message() + # for ecu, addr, subaddress, version in fw_versions: + # f = car.CarParams.CarFw.new_message() + # f.ecu = ecu + # f.address = addr + # f.fwVersion = version + # f.brand = brand + # + # if subaddress is not None: + # f.subAddress = subaddress + # + # car_fw.append(f) + # cp.carVin = "1" * 17 + # cp.carFw = car_fw + # params.put("CarParamsCache", cp.to_bytes()) time.sleep(2) # wait for controlsd to be ready pm.send('can', can_list_to_can_capnp([[0, 0, b"", 0]])) time.sleep(0.1) + # TODO: test multi pandas msg = messaging.new_message('pandaStates', 1) msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno pm.send('pandaStates', msg) + print('NOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOW Time started now', flush=True) + start_t = time.perf_counter() # fingerprint - if (car_model is None) or (fw_versions is not None): + car_model = None # "TOYOTA COROLLA 2017" + if (car_model is None):# or (fw_versions is not None): finger = {addr: 1 for addr in range(1, 100)} else: finger = _FINGERPRINTS[car_model][0] for _ in range(1000): # controlsd waits for boardd to echo back that it has changed the multiplexing mode + # TODO: do we need some decimation to replicate 0.1s time online? if not params.get_bool("ObdMultiplexingChanged"): params.put_bool("ObdMultiplexingChanged", True) @@ -118,14 +100,104 @@ class TestStartup(unittest.TestCase): time.sleep(0.01) msgs = messaging.drain_sock(controls_sock) - if len(msgs): - event_name = msgs[0].controlsState.alertType.split("/")[0] - self.assertEqual(EVENT_NAME[expected_event], event_name, - f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}") + if params.get_bool("FirmwareQueryDone"): + # if len(msgs): + print('total_time:', time.perf_counter() - start_t, 's') + # event_name = msgs[0].controlsState.alertType.split("/")[0] + # print('event_name', event_name) + # self.assertEqual(EVENT_NAME[expected_event], event_name, + # f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}") break else: self.fail(f"failed to fingerprint {car_model}") + # @parameterized.expand([ + # # TODO: test EventName.startup for release branches + # + # # officially supported car + # (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS, "toyota"), + # (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS, "toyota"), + # + # # dashcamOnly car + # (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS, "mazda"), + # (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS, "mazda"), + # + # # unrecognized car with no fw + # (EventName.startupNoFw, None, None, ""), + # (EventName.startupNoFw, None, None, ""), + # + # # unrecognized car + # (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), + # (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), + # + # # fuzzy match + # (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), + # (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), + # ]) + # @with_processes(['controlsd']) + # def test_startup_alert(self, expected_event, car_model, fw_versions, brand): + # + # # TODO: this should be done without any real sockets + # controls_sock = messaging.sub_sock("controlsState") + # pm = messaging.PubMaster(['can', 'pandaStates']) + # + # params = Params() + # params.clear_all() + # params.put_bool("Passive", False) + # params.put_bool("OpenpilotEnabledToggle", True) + # + # # Build capnn version of FW array + # if fw_versions is not None: + # car_fw = [] + # cp = car.CarParams.new_message() + # for ecu, addr, subaddress, version in fw_versions: + # f = car.CarParams.CarFw.new_message() + # f.ecu = ecu + # f.address = addr + # f.fwVersion = version + # f.brand = brand + # + # if subaddress is not None: + # f.subAddress = subaddress + # + # car_fw.append(f) + # cp.carVin = "1" * 17 + # cp.carFw = car_fw + # params.put("CarParamsCache", cp.to_bytes()) + # + # time.sleep(2) # wait for controlsd to be ready + # + # pm.send('can', can_list_to_can_capnp([[0, 0, b"", 0]])) + # time.sleep(0.1) + # + # msg = messaging.new_message('pandaStates', 1) + # msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno + # pm.send('pandaStates', msg) + # + # # fingerprint + # if (car_model is None) or (fw_versions is not None): + # finger = {addr: 1 for addr in range(1, 100)} + # else: + # finger = _FINGERPRINTS[car_model][0] + # + # for _ in range(1000): + # # controlsd waits for boardd to echo back that it has changed the multiplexing mode + # if not params.get_bool("ObdMultiplexingChanged"): + # params.put_bool("ObdMultiplexingChanged", True) + # + # msgs = [[addr, 0, b'\x00' * length, 0] for addr, length in finger.items()] + # pm.send('can', can_list_to_can_capnp(msgs)) + # + # time.sleep(0.01) + # msgs = messaging.drain_sock(controls_sock) + # if len(msgs): + # event_name = msgs[0].controlsState.alertType.split("/")[0] + # self.assertEqual(EVENT_NAME[expected_event], event_name, + # f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}") + # break + # else: + # self.fail(f"failed to fingerprint {car_model}") + if __name__ == "__main__": unittest.main()