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()
