def test_empty_radar_vin(self): self.delete_test_config_file() # first pass creates config file cs = CarSettings(optional_config_file_path=self.test_config_file) self.assertEqual(cs.radarVIN, " ") # second pass actually reads the file cs = CarSettings(optional_config_file_path=self.test_config_file) self.assertEqual(cs.radarVIN, " ")
def test_get_value(self): config_file_path = "./test_config_file3.cfg" cs = CarSettings(optional_config_file_path=config_file_path) value = cs.get_value("userHandle") self.assertEqual(value, 'your_tinkla_username') value = cs.get_value("doAutoUpdate") self.assertEqual(value, True) os.remove(config_file_path)
def test_defaults_missing_file(self): # First time proves that data is set locally cs = CarSettings(optional_config_file_path=self.test_config_file) self.check_defaults(cs) self.assertEqual(cs.did_write_file, True) # Run a second time to make sure it was saved and read correctly cs = CarSettings(optional_config_file_path=self.test_config_file) self.check_defaults(cs) self.assertEqual(cs.did_write_file, False)
def is_on_hotspot(): try: result = subprocess.check_output(["ifconfig", "wlan0"], encoding='utf8') # pylint: disable=unexpected-keyword-arg result = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0] is_android = result.startswith('192.168.43.') is_ios = result.startswith('172.20.10.') car_set = CarSettings() blockUploadWhileTethering = car_set.get_value("blockUploadWhileTethering") tetherIP = car_set.get_value("tetherIP") is_other_tether = blockUploadWhileTethering and result.startswith(tetherIP) is_entune = result.startswith('10.0.2.') return (is_android or is_ios or is_other_tether or is_entune) except: return False
def __init__(self): #self.start_server() self.tinklaClient = TinklaClient() openPilotId = "test_openpilotId" source = "tinkladTestClient" userHandle = "test_user_handle" info = tinkla.Interface.UserInfo.new_message( openPilotId=openPilotId, userHandle=userHandle, gitRemote="test_github.com/something", gitBranch="test_gitbranch", gitHash="test_123456") start_time = time.time() self.tinklaClient.setUserInfo(info) elapsed_time_us = (time.time() - start_time) * 1000 * 1000 print("Info Time Elapsed = %d" % (elapsed_time_us)) event = tinkla.Interface.Event.new_message( openPilotId=openPilotId, source=source, category=self.tinklaClient.eventCategoryKeys.userAction, name="pull_stalk", ) event.value.textValue = "up" start_time = time.time() self.tinklaClient.logUserEvent(event) elapsed_time_us = (time.time() - start_time) * 1000 * 1000 print("Event Time Elapsed = %d" % (elapsed_time_us)) carsettings = CarSettings("./bb_openpilot_config.cfg") carsettings.userHandle = userHandle print("userHandle = '%s'" % (userHandle)) print("attemptToSendPendingMessages") self.tinklaClient.attemptToSendPendingMessages() print("send crash log") self.tinklaClient.logCrashStackTraceEvent(openPilotId=openPilotId) print("send user info 2") info = tinkla.Interface.UserInfo.new_message( openPilotId=openPilotId, userHandle=userHandle + "2", gitRemote="test_github.com/something", gitBranch="test_gitbranch", gitHash="test_123456") self.tinklaClient.setUserInfo(info)
def get_car(logcan, sendcan, has_relay=False): if CarSettings().forceFingerprintTesla: candidate = "TESLA MODEL S" fingerprints = ["", "", ""] vin = "TESLAFORCED123456" #BB fw_candidates, car_fw = set(), [] source = car.CarParams.FingerprintSource.fixed cloudlog.warning("VIN %s", vin) Params().put("CarVin", vin) else: candidate, fingerprints, vin, car_fw, source = fingerprint( logcan, sendcan, has_relay) if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) candidate = "mock" CarInterface, CarController, CarState = interfaces[candidate] car_params = CarInterface.get_params(candidate, fingerprints, has_relay, car_fw) car_params.carVin = vin car_params.carFw = car_fw car_params.fingerprintSource = source return CarInterface(car_params, CarController, CarState), car_params
def __init__(self, mocked, RI): self.current_time = 0 self.mocked = mocked self.RI = RI self.tracks = defaultdict(dict) self.last_md_ts = 0 self.last_controls_state_ts = 0 self.active = 0 # v_ego self.v_ego = 0. self.v_ego_hist_t = deque([0], maxlen=v_len) self.v_ego_hist_v = deque([0], maxlen=v_len) self.v_ego_t_aligned = 0. self.ready = False self.icCarLR = None if (RI.TRACK_RIGHT_LANE or RI.TRACK_LEFT_LANE ) and CarSettings().get_value("useTeslaRadar"): self.icCarLR = messaging.pub_sock(service_list['uiIcCarLR'].port) self.lane_width = 3.0 #only used for left and right lanes self.path_x = np.arange(0.0, 160.0, 0.1) # 160 meters is max self.poller = zmq.Poller() self.pathPlanSocket = messaging.sub_sock(service_list['pathPlan'].port, conflate=True, poller=self.poller) self.dPoly = [0., 0., 0., 0.]
def test_float_parsing(self): config_file_path = "./test_config_file2.cfg" self.create_empty_config_file( config_file_path, test_parameter_string="radar_offset = 3.14") cs = CarSettings(optional_config_file_path=config_file_path) self.assertEqual(cs.radarOffset, 3.14) os.remove(config_file_path)
def main(): global gpsLocationExternal, ubloxGnss nav_frame_buffer = {} nav_frame_buffer[0] = {} for i in range(1, 33): nav_frame_buffer[0][i] = {} if not CarSettings().get_value("useTeslaGPS"): gpsLocationExternal = messaging.pub_sock('gpsLocationExternal') ubloxGnss = messaging.pub_sock('ubloxGnss') dev = init_reader() while True: try: msg = dev.receive_message() except serial.serialutil.SerialException as e: print(e) dev.close() dev = init_reader() if msg is not None: handle_msg(dev, msg, nav_frame_buffer) else: while True: time.sleep(1.1)
def __init__(self, CP): # radar self.pts = {} self.extPts = {} self.delay = 0 self.TRACK_LEFT_LANE = True self.TRACK_RIGHT_LANE = True self.updated_messages = set() self.canErrorCounter = 0 self.AHB_car_detected = False self.track_id = 0 self.radar_fault = False self.radar_wrong_config = False self.radar_off_can = CP.radarOffCan self.radar_ts = CP.radarTimeStep if not self.radar_off_can: self.pts = {} self.extPts = {} self.valid_cnt = {key: 0 for key in RADAR_A_MSGS} self.rcp = _create_radard_can_parser() self.radarOffset = CarSettings().get_value("radarOffset") self.trackId = 1 self.trigger_start_msg = RADAR_A_MSGS[0] self.trigger_end_msg = RADAR_B_MSGS[-1] self.delay = int(round(0.1 / CP.radarTimeStep)) # 0.1s delay of radar
def test_radar_vin_with_data(self): config_file_path = "./test_config_file2.cfg" self.create_empty_config_file( config_file_path, test_parameter_string="radar_vin = 12345678901234567") cs = CarSettings(optional_config_file_path=config_file_path) self.assertEqual(cs.radarVIN, "12345678901234567") os.remove(config_file_path)
def __init__(self): try: params = Params() except OSError: params = Params(db="./params") try: self.carSettings = CarSettings() except IOError: self.carSettings = CarSettings(optional_config_file_path="./bb_openpilot.cfg") self.openPilotId = params.get("DongleId") self.userHandle = self.carSettings.userHandle self.gitRemote = params.get("GitRemote") self.gitBranch = params.get("GitBranch") self.gitHash = params.get("GitCommit") self.start_client() tinklaClient = self
def test_update_empty_radar_vin(self): config_file_path = "./test_config_file2.cfg" self.create_empty_config_file( config_file_path, test_parameter_string="radar_vin = ") cs = CarSettings(optional_config_file_path=config_file_path) # Should be the correct all spaces VIN self.assertEqual(cs.radarVIN, " ") os.remove(config_file_path)
def test_defaults_non_overriding(self): config_file_path = "./test_config_file2.cfg" self.create_empty_config_file( config_file_path, test_parameter_string="force_pedal_over_cc = True") cs = CarSettings(optional_config_file_path=config_file_path) # Should still be true, even though the defaut is False self.assertEqual(cs.forcePedalOverCC, True) os.remove(config_file_path)
def __init__(self): carSettings = CarSettings() params = Params() self.openPilotId = params.get("DongleId") self.userHandle = carSettings.userHandle self.gitRemote = params.get("GitRemote") self.gitBranch = params.get("GitBranch") self.gitHash = params.get("GitCommit") self.start_client() tinklaClient = self
def __init__(self,CP): # radar self.pts = {} self.extPts = {} self.delay = 0.1 self.useTeslaRadar = CarSettings().get_value("useTeslaRadar") self.TRACK_LEFT_LANE = True self.TRACK_RIGHT_LANE = True self.updated_messages = set() self.canErrorCounter = 0 if self.useTeslaRadar: self.pts = {} self.extPts = {} self.valid_cnt = {key: 0 for key in RADAR_A_MSGS} self.delay = 0.1 # Delay of radar self.rcp = _create_radard_can_parser() self.logcan = messaging.sub_sock(service_list['can'].port) self.radarOffset = CarSettings().get_value("radarOffset") self.trackId = 1 self.trigger_start_msg = RADAR_A_MSGS[0] self.trigger_end_msg = RADAR_B_MSGS[-1]
def manager_prepare(spinner=None): # build cereal first subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal")) carSettings = CarSettings() # build all processes os.chdir(os.path.dirname(os.path.abspath(__file__))) for i, p in enumerate(managed_processes): if spinner is not None: spinText = carSettings.spinnerText spinner.update(spinText % (100.0 * (i + 1) / len(managed_processes),)) prepare_managed_process(p)
def __init__(self, CP): # radar self.pts = {} self.extPts = {} self.delay = 0 self.useTeslaRadar = CarSettings().get_value("useTeslaRadar") self.TRACK_LEFT_LANE = True self.TRACK_RIGHT_LANE = True self.updated_messages = set() self.canErrorCounter = 0 self.AHB_car_detected = False if self.useTeslaRadar: self.pts = {} self.extPts = {} self.valid_cnt = {key: 0 for key in RADAR_A_MSGS} self.rcp = _create_radard_can_parser() self.radarOffset = CarSettings().get_value("radarOffset") self.trackId = 1 self.trigger_start_msg = RADAR_A_MSGS[0] self.trigger_end_msg = RADAR_B_MSGS[-1] self.radar_ts = CP.radarTimeStep
def __init__(self): # radar self.pts = {} self.delay = 0.1 self.useTeslaRadar = CarSettings().get_value("useTeslaRadar") self.TRACK_LEFT_LANE = True self.TRACK_RIGHT_LANE = True if self.useTeslaRadar: self.pts = {} self.valid_cnt = {key: 0 for key in RADAR_A_MSGS} self.delay = 0.05 # Delay of radar self.rcp = _create_radard_can_parser() self.logcan = messaging.sub_sock(service_list['can'].port)
def sendUserInfoToTinkla(params): carSettings = CarSettings() gitRemote = params.get("GitRemote") gitBranch = params.get("GitBranch") gitHash = params.get("GitCommit") dongleId = params.get("DongleId") userHandle = carSettings.userHandle info = tinkla.Interface.UserInfo.new_message(openPilotId=dongleId, userHandle=userHandle, gitRemote=gitRemote, gitBranch=gitBranch, gitHash=gitHash) tinklaClient.setUserInfo(info)
def main(gctx=None): params = Params() carSettings = CarSettings() doAutoUpdate = carSettings.doAutoUpdate while True: # try network ping_failed = subprocess.call( ["ping", "-W", "4", "-c", "1", "8.8.8.8"]) if ping_failed: time.sleep(60) continue if doAutoUpdate: # download application update try: r = subprocess.check_output( NICE_LOW_PRIORITY + ["git", "fetch"], stderr=subprocess.STDOUT).decode('utf8') except subprocess.CalledProcessError as e: cloudlog.event("git fetch failed", cmd=e.cmd, output=e.output, returncode=e.returncode) time.sleep(60) continue cloudlog.info("git fetch success: %s", r) # Write update available param try: cur_hash = subprocess.check_output( ["git", "rev-parse", "HEAD"]).rstrip() upstream_hash = subprocess.check_output( ["git", "rev-parse", "@{u}"]).rstrip() params.put("UpdateAvailable", str(int(cur_hash != upstream_hash))) except: params.put("UpdateAvailable", "0") # Write latest release notes to param try: r = subprocess.check_output( ["git", "--no-pager", "show", "@{u}:RELEASES.md"]) r = r[:r.find(b'\n\n')] # Slice latest release notes params.put("ReleaseNotes", r + b"\n") except: params.put("ReleaseNotes", "") t = datetime.datetime.now().isoformat() params.put("LastUpdateTime", t.encode('utf8')) time.sleep(60 * 60)
def test_comment_update(self): config_file_path = "./test_config_file3.cfg" old_comment = "# do_auto_update - old description (default: true)" old_entry = "do_auto_update = False" self.create_empty_config_file(config_file_path, test_parameter_string=old_comment + "\n" + old_entry) expected_comment = "set this setting to false if you do not want op to autoupdate every time you reboot and there is a change on the repo." cs = CarSettings(optional_config_file_path=config_file_path) # Make sure setting didn't change self.assertEqual(cs.doAutoUpdate, False) # Make sure comment was updated: fd = open(config_file_path, "r") contents = fd.read() fd.close() self.assertTrue(contents.find(expected_comment) != -1) # File should have changed (to update comment) self.assertEqual(cs.did_write_file, True) # Next time we read, file shouldn't change anymore: cs = CarSettings(optional_config_file_path=config_file_path) self.assertEqual(cs.did_write_file, False) # Now remove a config option to cause an update: fd = open(config_file_path, "r") contents = fd.read() fd.close() new_contents = contents.replace("limit_battery_max = 80", "") fd = open(config_file_path, "w") fd.write(new_contents) fd.close() cs = CarSettings(optional_config_file_path=config_file_path) another_comment = 'if you use an aftermarket tesla bosch radar that already has a coded vin, you will have to enter that vin' # Make sure other comments were written: fd = open(config_file_path, "r") contents = fd.read() fd.close() self.assertTrue(contents.find(another_comment) != -1) os.remove(config_file_path)
def test_comments(self): expected_comment = "set this setting to false if you do not want op to autoupdate every time you reboot and there is a change on the repo" config_file_path = "./test_config_file2.cfg" self.create_empty_config_file( config_file_path, test_parameter_string="force_pedal_over_cc = True") cs = CarSettings(optional_config_file_path=config_file_path) # Should still be true, even though the defaut is False self.assertEqual(cs.forcePedalOverCC, True) # Make sure comment was added: fd = open(config_file_path, "r") contents = fd.read() fd.close() self.assertNotEqual(contents.find(expected_comment), -1) os.remove(config_file_path)
def manager_prepare(spinner=None): carSettings = CarSettings() # build all processes os.chdir(os.path.dirname(os.path.abspath(__file__))) # Spinner has to start from 70 here total = 100.0 if prebuilt else 50.0 for i, p in enumerate(managed_processes): if spinner is not None: spinText = carSettings.spinnerText spinner.update(spinText % ((100.0 - total) + total * (i + 1) / len(managed_processes), )) prepare_managed_process(p)
def main(gctx=None): poller = zmq.Poller() if not CarSettings().get_value("useTeslaGPS"): gpsLocationExternal = messaging.pub_sock( service_list['gpsLocationExternal'].port) ubloxGnss = messaging.pub_sock(service_list['ubloxGnss'].port) # ubloxRaw = messaging.sub_sock(service_list['ubloxRaw'].port, poller) # buffer with all the messages that still need to be input into the kalman while 1: polld = poller.poll(timeout=1000) for sock, mode in polld: if mode != zmq.POLLIN: continue logs = messaging.drain_sock(sock) for log in logs: buff = log.ubloxRaw ttime = log.logMonoTime msg = ublox.UBloxMessage() msg.add(buff) if msg.valid(): if msg.name() == 'NAV_PVT': sol = gen_solution(msg) if unlogger: sol.logMonoTime = ttime else: sol.logMonoTime = int( realtime.sec_since_boot() * 1e9) gpsLocationExternal.send(sol.to_bytes()) elif msg.name() == 'RXM_RAW': raw = gen_raw(msg) if unlogger: raw.logMonoTime = ttime else: raw.logMonoTime = int( realtime.sec_since_boot() * 1e9) ubloxGnss.send(raw.to_bytes()) else: print "INVALID MESSAGE" else: while True: time.sleep(1.1)
def get_car(logcan, sendcan, has_relay=False): if CarSettings().forceFingerprintTesla: candidate = "TESLA MODEL S" fingerprints = ["", "", ""] vin = "TESLAFORCED123456" cloudlog.warning("VIN %s", vin) Params().put("CarVin", vin) else: candidate, fingerprints, vin = fingerprint(logcan, sendcan, has_relay) if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) candidate = "mock" CarInterface, CarController = interfaces[candidate] car_params = CarInterface.get_params(candidate, fingerprints, vin, has_relay) return CarInterface(car_params, CarController), car_params
def __init__(self): self.tracks = set() #BB frame delay for dRel calculation, in seconds self.frame_delay = 0.2 self.useTeslaRadar = CarSettings().get_value("useTeslaRadar")
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # Scaled tire stiffness ts_factor = 8 ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "tesla" ret.carFingerprint = candidate teslaModel = read_db('/data/params','TeslaModel') if teslaModel is not None: teslaModel = teslaModel.decode() if teslaModel is None: teslaModel = "S" ret.safetyModel = car.CarParams.SafetyModel.tesla ret.safetyParam = 1 ret.carVin = "TESLAFAKEVIN12345" ret.enableCamera = True ret.enableGasInterceptor = False #keep this False for now print ("ECU Camera Simulated: ", ret.enableCamera) print ("ECU Gas Interceptor: ", ret.enableGasInterceptor) ret.enableCruise = not ret.enableGasInterceptor mass_models = 4722./2.205 + STD_CARGO_KG wheelbase_models = 2.959 # RC: I'm assuming center means center of mass, and I think Model S is pretty even between two axles centerToFront_models = wheelbase_models * 0.5 #BB was 0.48 centerToRear_models = wheelbase_models - centerToFront_models rotationalInertia_models = 2500 tireStiffnessFront_models = 85100 #BB was 85400 tireStiffnessRear_models = 90000 # will create Kp and Ki for 0, 20, 40, 60 mph ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 8.94, 17.88, 26.82 ], [0., 8.94, 17.88, 26.82]] if candidate == CAR.MODELS: stop_and_go = True ret.mass = mass_models ret.wheelbase = wheelbase_models ret.centerToFront = centerToFront_models ret.steerRatio = 11.5 # Kp and Ki for the lateral control for 0, 20, 40, 60 mph ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.20, 0.80, 0.60, 0.30], [0.16, 0.12, 0.08, 0.04]] ret.lateralTuning.pid.kf = 0.00006 # Initial test value TODO: investigate FF steer control for Model S? ret.steerActuatorDelay = 0.2 #ret.steerReactance = 1.0 #ret.steerInductance = 1.0 #ret.steerResistance = 1.0 # Kp and Ki for the longitudinal control if teslaModel == "S": ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.01,0.01,0.01] elif teslaModel == "SP": ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725] elif teslaModel == "SD": ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.01,0.01,0.01] elif teslaModel == "SPD": ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725] else: #use S numbers if we can't match anything ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.375, 0.325, 0.3] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.08,0.08,0.08] else: raise ValueError("unsupported car %s" % candidate) ret.steerControlType = car.CarParams.SteerControlType.angle # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not # conflict with PCM acc ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 25.5 * CV.MPH_TO_MS centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for Model S ret.rotationalInertia = rotationalInertia_models * \ ret.mass * ret.wheelbase**2 / (mass_models * wheelbase_models**2) # TODO: start from empirically derived lateral slip stiffness and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront = (tireStiffnessFront_models * ts_factor) * \ ret.mass / mass_models * \ (centerToRear / ret.wheelbase) / (centerToRear_models / wheelbase_models) ret.tireStiffnessRear = (tireStiffnessRear_models * ts_factor) * \ ret.mass / mass_models * \ (ret.centerToFront / ret.wheelbase) / (centerToFront_models / wheelbase_models) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. # no max steer limit VS speed ret.steerMaxBP = [0.,15.] # m/s ret.steerMaxV = [420.,420.] # max steer allowed ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.3] #if ret.enableGasInterceptor else [0.] # max gas allowed ret.brakeMaxBP = [0., 20.] # m/s ret.brakeMaxV = [1., 1.] # max brake allowed - BB: since we are using regen, make this even ret.longitudinalTuning.deadzoneBP = [0., 9.] #BB: added from Toyota to start pedal work; need to tune ret.longitudinalTuning.deadzoneV = [0., 0.] #BB: added from Toyota to start pedal work; need to tune; changed to 0 for now ret.stoppingControl = True ret.openpilotLongitudinalControl = True ret.steerLimitAlert = False ret.startAccel = 0.5 ret.steerRateCost = 1.0 ret.radarOffCan = not CarSettings().get_value("useTeslaRadar") ret.radarTimeStep = 0.05 #20Hz return ret
def fingerprint(logcan, sendcan, has_relay): fixed_fingerprint = os.environ.get('FINGERPRINT', "") skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) if has_relay and not fixed_fingerprint and not skip_fw_query: # Vin query only reliably works thorugh OBDII bus = 1 cached_params = Params().get("CarParamsCache") if cached_params is not None: cached_params = car.CarParams.from_bytes(cached_params) if cached_params.carName == "mock": cached_params = None if cached_params is not None and len(cached_params.carFw) > 0 and cached_params.carVin is not VIN_UNKNOWN: cloudlog.warning("Using cached CarParams") vin = cached_params.carVin car_fw = list(cached_params.carFw) else: cloudlog.warning("Getting VIN & FW versions") _, vin = get_vin(logcan, sendcan, bus) car_fw = get_fw_versions(logcan, sendcan, bus) fw_candidates = match_fw_to_car(car_fw) else: vin = VIN_UNKNOWN fw_candidates, car_fw = set(), [] cloudlog.warning("VIN %s", vin) Params().put("CarVin", vin) finger = gen_empty_fingerprint() candidate_cars = {i: all_known_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1 frame = 0 frame_fingerprint = 10 # 0.1s car_fingerprint = None done = False while not done: a = get_one_can(logcan) for can in a.can: # need to independently try to fingerprint both bus 0 and 1 to work # for the combo black_panda and honda_bosch. Ignore extended messages # and VIN query response. # Include bus 2 for toyotas to disambiguate cars using camera messages # (ideally should be done for all cars but we can't for Honda Bosch) if can.src in range(0, 4): finger[can.src][can.address] = len(can.dat) for b in candidate_cars: if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) 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: # Toyota needs higher time to fingerprint, since DSU does not broadcast immediately if only_toyota_left(candidate_cars[b]): frame_fingerprint = 100 # 1s if len(candidate_cars[b]) == 1: if frame > frame_fingerprint: # fingerprint done car_fingerprint = candidate_cars[b][0] if (car_fingerprint is None) and CarSettings().forceFingerprintTesla: print ("Fingerprinting Failed: Returning Tesla (based on branch)") car_fingerprint = "TESLA MODEL S" vin = "TESLAFAKEVIN12345" # 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()) or frame > 200 succeeded = car_fingerprint is not None done = failed or succeeded frame += 1 source = car.CarParams.FingerprintSource.can # If FW query returns exactly 1 candidate, use it if len(fw_candidates) == 1: car_fingerprint = list(fw_candidates)[0] source = car.CarParams.FingerprintSource.fw if fixed_fingerprint: car_fingerprint = fixed_fingerprint source = car.CarParams.FingerprintSource.fixed cloudlog.warning("fingerprinted %s", car_fingerprint) return car_fingerprint, finger, vin, car_fw, source
def radard_thread(sm=None, pm=None, can_sock=None): set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) use_tesla_radar = CarSettings().get_value("useTeslaRadar") mocked = (CP.carName == "mock") or ((CP.carName == "tesla") and not use_tesla_radar) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters', 'pathPlan']) # *** publish radarState and liveTracks if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) icLeads = messaging.pub_sock('uiIcLeads') ahbInfo = messaging.pub_sock('ahbInfo') RI = RadarInterface(CP) rk = Ratekeeper(1.0 / DT_RDR, print_delay_threshold=None) RD = RadarD(mocked, RI, use_tesla_radar,RI.delay) has_radar = not CP.radarOffCan or mocked last_md_ts = 0. v_ego = 0. while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) sm.update(0) if sm.updated['controlsState']: v_ego = sm['controlsState'].vEgo rr,rrext,ahbCarDetected = RI.update(can_strings,v_ego) if rr is None: continue dat,datext = RD.update(rk.frame, sm, rr, has_radar, rrext) dat.radarState.cumLagMs = -rk.remaining*1000. pm.send('radarState', dat) icLeads.send(datext.to_bytes()) ahbInfoMsg = tesla.AHBinfo.new_message() ahbInfoMsg.source = 0 ahbInfoMsg.radarCarDetected = ahbCarDetected ahbInfoMsg.cameraCarDetected = False ahbInfo.send(ahbInfoMsg.to_bytes()) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(sorted(tracks.keys())): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } pm.send('liveTracks', dat) rk.monitor_time()