sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

not clean

Browse Source
pull/27753/head
Shane Smiskol 2 years ago
parent 56a1ae4bb7
commit dab35d582c
5 changed files with 179 additions and 91 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 74
      selfdrive/car/car_helpers.py
  2. 18
      selfdrive/car/fw_versions.py
  3. 7
      selfdrive/car/tests/test_fw_fingerprint_metrics.py
  4. 5
      selfdrive/controls/controlsd.py
  5. 166
      selfdrive/controls/tests/test_startup.py

74
selfdrive/car/car_helpers.py
Unescape View File

@ -77,6 +77,7 @@ interfaces = load_interfaces(interface_names)
# **** for use live only **** # **** for use live only ****
def fingerprint(logcan, sendcan, num_pandas): def fingerprint(logcan, sendcan, num_pandas):
# return None, {}, "", [], "source", True
fixed_fingerprint = os.environ.get('FINGERPRINT', "") fixed_fingerprint = os.environ.get('FINGERPRINT', "")
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
ecu_rx_addrs = set() ecu_rx_addrs = set()
@ -110,6 +111,7 @@ def fingerprint(logcan, sendcan, num_pandas):
vin, vin_rx_addr = VIN_UNKNOWN, 0 vin, vin_rx_addr = VIN_UNKNOWN, 0
exact_fw_match, fw_candidates, car_fw = True, set(), [] exact_fw_match, fw_candidates, car_fw = True, set(), []
cached = False cached = False
vin, vin_rx_addr = VIN_UNKNOWN, 0
if not is_valid_vin(vin): if not is_valid_vin(vin):
cloudlog.event("Malformed VIN", vin=vin, error=True) cloudlog.event("Malformed VIN", vin=vin, error=True)
@ -128,39 +130,39 @@ def fingerprint(logcan, sendcan, num_pandas):
car_fingerprint = None car_fingerprint = None
done = False done = False
# drain CAN socket so we always get the latest messages # # drain CAN socket so we always get the latest messages
messaging.drain_sock_raw(logcan) # messaging.drain_sock_raw(logcan)
#
while not done: # while not done:
a = get_one_can(logcan) # a = get_one_can(logcan)
#
for can in a.can: # for can in a.can:
# The fingerprint dict is generated for all buses, this way the car interface # # 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 # # can use it to detect a (valid) multipanda setup and initialize accordingly
if can.src < 128: # if can.src < 128:
if can.src not in finger: # if can.src not in finger:
finger[can.src] = {} # finger[can.src] = {}
finger[can.src][can.address] = len(can.dat) # finger[can.src][can.address] = len(can.dat)
#
for b in candidate_cars: # for b in candidate_cars:
# Ignore extended messages and VIN query response. # # Ignore extended messages and VIN query response.
if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8): # 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]) # 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 # # if we only have one car choice and the time since we got our first
# message has elapsed, exit # # message has elapsed, exit
for b in candidate_cars: # for b in candidate_cars:
if len(candidate_cars[b]) == 1 and frame > frame_fingerprint: # if len(candidate_cars[b]) == 1 and frame > frame_fingerprint:
# fingerprint done # # fingerprint done
car_fingerprint = candidate_cars[b][0] # car_fingerprint = candidate_cars[b][0]
#
# bail if no cars left or we've been waiting for more than 2s # # 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 # failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > frame_fingerprint) or frame > 200
succeeded = car_fingerprint is not None # succeeded = car_fingerprint is not None
done = failed or succeeded # done = failed or succeeded
#
frame += 1 # frame += 1
#
exact_match = True exact_match = True
source = car.CarParams.FingerprintSource.can 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] CarInterface, CarController, CarState = interfaces[candidate]
CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed) CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed)
CP.carVin = vin # CP.carVin = vin
CP.carFw = car_fw # CP.carFw = car_fw
CP.fingerprintSource = source # CP.fingerprintSource = source
CP.fuzzyFingerprint = not exact_match CP.fuzzyFingerprint = not exact_match
return CarInterface(CP, CarController, CarState), CP return CarInterface(CP, CarController, CarState), CP

18
selfdrive/car/fw_versions.py
Unescape View File

@ -2,6 +2,7 @@
from collections import defaultdict from collections import defaultdict
from typing import Any, Dict, List, Set from typing import Any, Dict, List, Set
from tqdm import tqdm from tqdm import tqdm
import time
import panda.python.uds as uds import panda.python.uds as uds
from cereal import car from cereal import car
@ -182,7 +183,7 @@ def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]:
ecu_responses = set() ecu_responses = set()
for obd_multiplexing in queries: for obd_multiplexing in queries:
set_obd_multiplexing(params, obd_multiplexing) # set_obd_multiplexing(params, obd_multiplexing)
for query in queries[obd_multiplexing]: for query in queries[obd_multiplexing]:
ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1)) ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1))
return ecu_responses return ecu_responses
@ -206,7 +207,10 @@ def get_brand_ecu_matches(ecu_rx_addrs):
def set_obd_multiplexing(params: Params, obd_multiplexing: bool): def set_obd_multiplexing(params: Params, obd_multiplexing: bool):
print('SETTING MULTIPLEXING')
return
if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing: if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing:
print('SET MULTIPLEXING')
cloudlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}") cloudlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}")
params.remove("ObdMultiplexingChanged") params.remove("ObdMultiplexingChanged")
params.put_bool("ObdMultiplexingEnabled", obd_multiplexing) 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): 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() versions = VERSIONS.copy()
params = Params() 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 # Get versions and build capnp list to put into CarParams
car_fw = [] car_fw = []
requests = [(brand, config, r) for brand, config, r in REQUESTS if query_brand is None or brand == query_brand] 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 in tqdm(addrs, disable=not progress):
for addr_chunk in chunks(addr): for addr_chunk in chunks(addr):
for brand, config, r in requests: 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: if r.bus > num_pandas * 4 - 1:
continue continue
# Toggle OBD multiplexing for each request # # Toggle OBD multiplexing for each request
if r.bus % 4 == 1: # if r.bus % 4 == 1:
set_obd_multiplexing(params, r.obd_multiplexing) # set_obd_multiplexing(params, r.obd_multiplexing)
try: try:
addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and 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)] (len(r.whitelist_ecus) == 0 or ecu_types[(b, a, s)] in r.whitelist_ecus)]
if addrs: if addrs:
print(addrs)
query = IsoTpParallelQuery(sendcan, logcan, r.bus, addrs, r.request, r.response, r.rx_offset, debug=debug) 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(): for (tx_addr, sub_addr), version in query.get_data(timeout).items():
f = car.CarParams.CarFw.new_message() 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: except Exception:
cloudlog.exception("FW query exception") cloudlog.exception("FW query exception")
# print('query took', time.perf_counter() - t)
return car_fw return car_fw

7
selfdrive/car/tests/test_fw_fingerprint_metrics.py
Unescape View File

@ -16,7 +16,6 @@ ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()}
class TestFwFingerprint(unittest.TestCase): class TestFwFingerprint(unittest.TestCase):
# TODO: test multiple pandas
@parameterized.expand([(1,), (2,)]) @parameterized.expand([(1,), (2,)])
def test_fw_query_metrics(self, num_pandas): def test_fw_query_metrics(self, num_pandas):
for brand, config in FW_QUERY_CONFIGS.items(): for brand, config in FW_QUERY_CONFIGS.items():
@ -32,9 +31,9 @@ class TestFwFingerprint(unittest.TestCase):
for r in requests: for r in requests:
total_time += 0.1 total_time += 0.1
if r.obd_multiplexing != obd_multiplexing and r.bus % 4 == 1: # if r.obd_multiplexing != obd_multiplexing and r.bus % 4 == 1:
obd_multiplexing = r.obd_multiplexing # obd_multiplexing = r.obd_multiplexing
total_time += 0.1 # total_time += 0.1
total_time = round(total_time, 2) total_time = round(total_time, 2)
self.assertLessEqual(total_time, 1.1) self.assertLessEqual(total_time, 1.1)

5
selfdrive/controls/controlsd.py
Unescape View File

@ -1,5 +1,6 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
import os import os
import time
import math import math
from typing import SupportsFloat from typing import SupportsFloat
@ -98,9 +99,13 @@ class Controls:
print("Waiting for CAN messages...") print("Waiting for CAN messages...")
get_one_can(self.can_sock) get_one_can(self.can_sock)
t = time.perf_counter()
num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) 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() 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) 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: else:
self.CI, self.CP = CI, CI.CP self.CI, self.CP = CI, CI.CP

166
selfdrive/controls/tests/test_startup.py
Unescape View File

@ -39,31 +39,8 @@ CX5_FW_VERSIONS = [
class TestStartup(unittest.TestCase): 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']) @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 # TODO: this should be done without any real sockets
controls_sock = messaging.sub_sock("controlsState") controls_sock = messaging.sub_sock("controlsState")
@ -74,42 +51,47 @@ class TestStartup(unittest.TestCase):
params.put_bool("Passive", False) params.put_bool("Passive", False)
params.put_bool("OpenpilotEnabledToggle", True) params.put_bool("OpenpilotEnabledToggle", True)
# Build capnn version of FW array # # Build capnp version of FW array
if fw_versions is not None: # if fw_versions is not None:
car_fw = [] # car_fw = []
cp = car.CarParams.new_message() # cp = car.CarParams.new_message()
for ecu, addr, subaddress, version in fw_versions: # for ecu, addr, subaddress, version in fw_versions:
f = car.CarParams.CarFw.new_message() # f = car.CarParams.CarFw.new_message()
f.ecu = ecu # f.ecu = ecu
f.address = addr # f.address = addr
f.fwVersion = version # f.fwVersion = version
f.brand = brand # f.brand = brand
#
if subaddress is not None: # if subaddress is not None:
f.subAddress = subaddress # f.subAddress = subaddress
#
car_fw.append(f) # car_fw.append(f)
cp.carVin = "1" * 17 # cp.carVin = "1" * 17
cp.carFw = car_fw # cp.carFw = car_fw
params.put("CarParamsCache", cp.to_bytes()) # params.put("CarParamsCache", cp.to_bytes())
time.sleep(2) # wait for controlsd to be ready time.sleep(2) # wait for controlsd to be ready
pm.send('can', can_list_to_can_capnp([[0, 0, b"", 0]])) pm.send('can', can_list_to_can_capnp([[0, 0, b"", 0]]))
time.sleep(0.1) time.sleep(0.1)
# TODO: test multi pandas
msg = messaging.new_message('pandaStates', 1) msg = messaging.new_message('pandaStates', 1)
msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno
pm.send('pandaStates', msg) pm.send('pandaStates', msg)
print('NOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOWNOW Time started now', flush=True)
start_t = time.perf_counter()
# fingerprint # 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)} finger = {addr: 1 for addr in range(1, 100)}
else: else:
finger = _FINGERPRINTS[car_model][0] finger = _FINGERPRINTS[car_model][0]
for _ in range(1000): for _ in range(1000):
# controlsd waits for boardd to echo back that it has changed the multiplexing mode # 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"): if not params.get_bool("ObdMultiplexingChanged"):
params.put_bool("ObdMultiplexingChanged", True) params.put_bool("ObdMultiplexingChanged", True)
@ -118,14 +100,104 @@ class TestStartup(unittest.TestCase):
time.sleep(0.01) time.sleep(0.01)
msgs = messaging.drain_sock(controls_sock) msgs = messaging.drain_sock(controls_sock)
if len(msgs): if params.get_bool("FirmwareQueryDone"):
event_name = msgs[0].controlsState.alertType.split("/")[0] # if len(msgs):
self.assertEqual(EVENT_NAME[expected_event], event_name, print('total_time:', time.perf_counter() - start_t, 's')
f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}") # 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 break
else: else:
self.fail(f"failed to fingerprint {car_model}") 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__": if __name__ == "__main__":
unittest.main() unittest.main()

Write Preview
Loading…
Cancel
Save