def disable_ecu(ecu_addr,
logcan,
sendcan,
bus,
timeout=0.5,
retry=5,
debug=False):
print(f"ecu disable {hex(ecu_addr)} ...")
for i in range(retry):
try:
# enter extended diagnostic session
query = IsoTpParallelQuery(sendcan,
logcan,
bus, [ecu_addr], [EXT_DIAG_REQUEST],
[EXT_DIAG_RESPONSE],
debug=debug)
for addr, dat in query.get_data(timeout).items(): # pylint: disable=unused-variable
print("ecu communication control disable tx/rx ...")
# communication control disable tx and rx
query = IsoTpParallelQuery(sendcan,
logcan,
bus, [ecu_addr], [COM_CONT_REQUEST],
[COM_CONT_RESPONSE],
debug=debug)
query.get_data(0)
return True
print(f"ecu disable retry ({i+1}) ...")
except Exception:
cloudlog.warning(
f"ecu disable exception: {traceback.format_exc()}")
return False
def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False):
for request, response in ((UDS_VIN_REQUEST, UDS_VIN_RESPONSE),
(OBD_VIN_REQUEST, OBD_VIN_RESPONSE)):
for i in range(retry):
try:
query = IsoTpParallelQuery(sendcan,
logcan,
bus,
FUNCTIONAL_ADDRS, [
request,
], [
response,
],
functional_addr=True,
debug=debug)
for addr, vin in query.get_data(timeout).items():
# Honda Bosch response starts with a length, trim to correct length
if vin.startswith(b'\x11'):
vin = vin[1:18]
return addr[0], vin.decode()
print(f"vin query retry ({i+1}) ...")
except Exception:
cloudlog.warning(
f"VIN query exception: {traceback.format_exc()}")
return 0, VIN_UNKNOWN
def get_fw_versions(logcan, sendcan, extra=None, timeout=0.1, debug=False, progress=False):
ecu_types = {}
# Extract ECU addresses to query from fingerprints
# ECUs using a subaddress need be queried one by one, the rest can be done in parallel
addrs = []
parallel_addrs = []
versions = get_interface_attr('FW_VERSIONS', ignore_none=True)
if extra is not None:
versions.update(extra)
for brand, brand_versions in versions.items():
for c in brand_versions.values():
for ecu_type, addr, sub_addr in c.keys():
a = (brand, addr, sub_addr)
if a not in ecu_types:
ecu_types[(addr, sub_addr)] = ecu_type
if sub_addr is None:
if a not in parallel_addrs:
parallel_addrs.append(a)
else:
if [a] not in addrs:
addrs.append([a])
addrs.insert(0, parallel_addrs)
fw_versions = {}
for i, addr in enumerate(tqdm(addrs, disable=not progress)):
for addr_chunk in chunks(addr):
for r in REQUESTS:
try:
addrs = [(a, s) for (b, a, s) in addr_chunk if b in (r.brand, 'any') and
(len(r.whitelist_ecus) == 0 or ecu_types[(a, s)] in r.whitelist_ecus)]
if addrs:
query = IsoTpParallelQuery(sendcan, logcan, r.bus, addrs, r.request, r.response, r.rx_offset, debug=debug)
t = 2 * timeout if i == 0 else timeout
fw_versions.update({addr: (version, r.request, r.rx_offset) for addr, version in query.get_data(t).items()})
except Exception:
cloudlog.warning(f"FW query exception: {traceback.format_exc()}")
# Build capnp list to put into CarParams
car_fw = []
for addr, (version, request, rx_offset) in fw_versions.items():
f = car.CarParams.CarFw.new_message()
f.ecu = ecu_types[addr]
f.fwVersion = version
f.address = addr[0]
f.responseAddress = uds.get_rx_addr_for_tx_addr(addr[0], rx_offset)
f.request = request
if addr[1] is not None:
f.subAddress = addr[1]
car_fw.append(f)
return car_fw
def recompute_route(self):
if self.last_position is None:
return
new_destination = coordinate_from_param("NavDestination", self.params)
if new_destination is None:
self.clear_route()
return
should_recompute = self.should_recompute()
if new_destination != self.nav_destination:
cloudlog.warning(
f"Got new destination from NavDestination param {new_destination}"
)
should_recompute = True
# Don't recompute when GPS drifts in tunnels
if not self.gps_ok and self.step_idx is not None:
return
if self.recompute_countdown == 0 and should_recompute:
self.recompute_countdown = 2**self.recompute_backoff
self.recompute_backoff = min(6, self.recompute_backoff + 1)
self.calculate_route(new_destination)
else:
self.recompute_countdown = max(0, self.recompute_countdown - 1)
def manager_thread() -> None:
cloudlog.bind(daemon="manager")
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
params = Params()
ignore: List[str] = []
if params.get("DongleId",
encoding='utf8') in (None, UNREGISTERED_DONGLE_ID):
ignore += ["manage_athenad", "uploader"]
if os.getenv("NOBOARD") is not None:
ignore.append("pandad")
ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0]
sm = messaging.SubMaster(['deviceState', 'carParams'],
poll=['deviceState'])
pm = messaging.PubMaster(['managerState'])
ensure_running(managed_processes.values(),
False,
params=params,
CP=sm['carParams'],
not_run=ignore)
while True:
sm.update()
started = sm['deviceState'].started
ensure_running(managed_processes.values(),
started,
params=params,
CP=sm['carParams'],
not_run=ignore)
running = ' '.join(
"%s%s\u001b[0m" %
("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
for p in managed_processes.values() if p.proc)
print(running)
cloudlog.debug(running)
# send managerState
msg = messaging.new_message('managerState')
msg.managerState.processes = [
p.get_process_state_msg() for p in managed_processes.values()
]
pm.send('managerState', msg)
# Exit main loop when uninstall/shutdown/reboot is needed
shutdown = False
for param in ("DoUninstall", "DoShutdown", "DoReboot"):
if params.get_bool(param):
shutdown = True
params.put("LastManagerExitReason", param)
cloudlog.warning(f"Shutting down manager - {param} set")
if shutdown:
break
def disable_logs(sig, frame):
os.system(
"mmcli -m 0 --location-disable-gps-raw --location-disable-gps-nmea"
)
cloudlog.warning("rawgpsd: shutting down")
try_setup_logs(diag, [])
cloudlog.warning("rawgpsd: logs disabled")
sys.exit(0)
def uploader_fn(exit_event):
try:
set_core_affinity([0, 1, 2, 3])
except Exception:
cloudlog.exception("failed to set core affinity")
clear_locks(ROOT)
params = Params()
dongle_id = params.get("DongleId", encoding='utf8')
if dongle_id is None:
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
if TICI and not Path("/data/media").is_mount():
cloudlog.warning("NVME not mounted")
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['uploaderState'])
uploader = Uploader(dongle_id, ROOT)
backoff = 0.1
while not exit_event.is_set():
sm.update(0)
offroad = params.get_bool("IsOffroad")
network_type = sm[
'deviceState'].networkType if not force_wifi else NetworkType.wifi
if network_type == NetworkType.none:
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
d = uploader.next_file_to_upload()
if d is None: # Nothing to upload
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
name, key, fn = d
# qlogs and bootlogs need to be compressed before uploading
if key.endswith(('qlog', 'rlog')) or (key.startswith('boot/')
and not key.endswith('.bz2')):
key += ".bz2"
success = uploader.upload(name, key, fn,
sm['deviceState'].networkType.raw,
sm['deviceState'].networkMetered)
if success:
backoff = 0.1
elif allow_sleep:
cloudlog.info("upload backoff %r", backoff)
time.sleep(backoff + random.uniform(0, backoff))
backoff = min(backoff * 2, 120)
pm.send("uploaderState", uploader.get_msg())
def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types):
astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types)
cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}")
start_time = time.monotonic()
try:
astro_dog.get_orbit_data(t, only_predictions=True)
cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.1f}s")
return astro_dog.orbits, astro_dog.orbit_fetched_times, t
except (RuntimeError, ValueError, IOError) as e:
cloudlog.warning(f"No orbit data found or parsing failure: {e}")
return None, None, t
def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, debug=False):
"""Silence an ECU by disabling sending and receiving messages using UDS 0x28.
The ECU will stay silent as long as openpilot keeps sending Tester Present.
This is used to disable the radar in some cars. Openpilot will emulate the radar.
WARNING: THIS DISABLES AEB!"""
cloudlog.warning(f"ecu disable {hex(addr)} ...")
for i in range(retry):
try:
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
for _, _ in query.get_data(timeout).items():
cloudlog.warning("communication control disable tx/rx ...")
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [com_cont_req], [COM_CONT_RESPONSE], debug=debug)
query.get_data(0)
cloudlog.warning("ecu disabled")
return True
except Exception:
cloudlog.exception("ecu disable exception")
print(f"ecu disable retry ({i+1}) ...")
cloudlog.warning("ecu disable failed")
return False
def update(self):
self.sm.update(0)
if self.sm.updated["managerState"]:
ui_pid = [
p.pid for p in self.sm["managerState"].processes
if p.name == "ui" and p.running
]
if ui_pid:
if self.ui_pid and self.ui_pid != ui_pid[0]:
cloudlog.warning("UI restarting, sending route")
threading.Timer(5.0, self.send_route).start()
self.ui_pid = ui_pid[0]
self.update_location()
self.recompute_route()
self.send_instruction()
def get_car(logcan, sendcan):
candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan)
if candidate is None:
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
candidate = "mock"
disable_radar = Params().get_bool("DisableRadar")
CarInterface, CarController, CarState = interfaces[candidate]
CP = CarInterface.get_params(candidate, fingerprints, car_fw, disable_radar)
CP.carVin = vin
CP.carFw = car_fw
CP.fingerprintSource = source
CP.fuzzyFingerprint = not exact_match
return CarInterface(CP, CarController, CarState), CP
def get_d_path(self, v_ego, path_t, path_xyz):
# Reduce reliance on lanelines that are too far apart or
# will be in a few seconds
path_xyz[:, 1] += self.path_offset
l_prob, r_prob = self.lll_prob, self.rll_prob
width_pts = self.rll_y - self.lll_y
prob_mods = []
for t_check in (0.0, 1.5, 3.0):
width_at_t = interp(t_check * (v_ego + 7), self.ll_x, width_pts)
prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
mod = min(prob_mods)
l_prob *= mod
r_prob *= mod
# Reduce reliance on uncertain lanelines
l_std_mod = interp(self.lll_std, [.15, .3], [1.0, 0.0])
r_std_mod = interp(self.rll_std, [.15, .3], [1.0, 0.0])
l_prob *= l_std_mod
r_prob *= r_std_mod
# Find current lanewidth
self.lane_width_certainty.update(l_prob * r_prob)
current_lane_width = abs(self.rll_y[0] - self.lll_y[0])
self.lane_width_estimate.update(current_lane_width)
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
self.lane_width = self.lane_width_certainty.x * self.lane_width_estimate.x + \
(1 - self.lane_width_certainty.x) * speed_lane_width
clipped_lane_width = min(4.0, self.lane_width)
path_from_left_lane = self.lll_y + clipped_lane_width / 2.0
path_from_right_lane = self.rll_y - clipped_lane_width / 2.0
self.d_prob = l_prob + r_prob - l_prob * r_prob
lane_path_y = (l_prob * path_from_left_lane + r_prob *
path_from_right_lane) / (l_prob + r_prob + 0.0001)
safe_idxs = np.isfinite(self.ll_t)
if safe_idxs[0]:
lane_path_y_interp = np.interp(path_t, self.ll_t[safe_idxs],
lane_path_y[safe_idxs])
path_xyz[:, 1] = self.d_prob * lane_path_y_interp + (
1.0 - self.d_prob) * path_xyz[:, 1]
else:
cloudlog.warning("Lateral mpc - NaNs in laneline times, ignoring")
return path_xyz
def can_init():
global handle, context
handle = None
cloudlog.info("attempting can init")
context = usb1.USBContext()
#context.setDebug(9)
for device in context.getDeviceList(skip_on_error=True):
if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc:
handle = device.open()
handle.claimInterface(0)
handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'')
if handle is None:
cloudlog.warning("CAN NOT FOUND")
exit(-1)
cloudlog.info("got handle")
cloudlog.info("can init done")
def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types,
cache_dir):
astro_dog = AstroDog(valid_const=valid_const,
auto_update=auto_update,
valid_ephem_types=valid_ephem_types,
cache_dir=cache_dir)
cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}")
start_time = time.monotonic()
try:
astro_dog.get_orbit_data(t, only_predictions=True)
cloudlog.info(
f"Done parsing orbits. Took {time.monotonic() - start_time:.1f}s")
cloudlog.debug(
f"Downloaded orbits ({sum([len(v) for v in astro_dog.orbits])}): {list(astro_dog.orbits.keys())}"
+
f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in astro_dog.orbit_fetched_times._ranges]}"
)
return astro_dog.orbits, astro_dog.orbit_fetched_times, t
except (DownloadFailed, RuntimeError, ValueError, IOError) as e:
cloudlog.warning(f"No orbit data found or parsing failure: {e}")
return None, None, t
def main() -> None:
prepare_only = os.getenv("PREPAREONLY") is not None
manager_init()
# Start UI early so prepare can happen in the background
if not prepare_only:
managed_processes['ui'].start()
manager_prepare()
if prepare_only:
return
# SystemExit on sigterm
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1))
try:
manager_thread()
except Exception:
traceback.print_exc()
sentry.capture_exception()
finally:
manager_cleanup()
params = Params()
if params.get_bool("DoUninstall"):
cloudlog.warning("uninstalling")
HARDWARE.uninstall()
elif params.get_bool("DoReboot"):
cloudlog.warning("reboot")
HARDWARE.reboot()
elif params.get_bool("DoShutdown"):
cloudlog.warning("shutdown")
HARDWARE.shutdown()
def get_ecu_addrs(logcan: messaging.SubSocket,
sendcan: messaging.PubSocket,
queries: Set[Tuple[int, Optional[int], int]],
responses: Set[Tuple[int, Optional[int], int]],
timeout: float = 1,
debug: bool = False) -> Set[Tuple[int, Optional[int], int]]:
ecu_responses: Set[Tuple[int, Optional[int],
int]] = set() # set((addr, subaddr, bus),)
try:
msgs = [
make_tester_present_msg(addr, bus, subaddr)
for addr, subaddr, bus in queries
]
messaging.drain_sock_raw(logcan)
sendcan.send(can_list_to_can_capnp(msgs, msgtype='sendcan'))
start_time = time.monotonic()
while time.monotonic() - start_time < timeout:
can_packets = messaging.drain_sock(logcan, wait_for_one=True)
for packet in can_packets:
for msg in packet.can:
subaddr = None if (msg.address, None,
msg.src) in responses else msg.dat[0]
if (msg.address, subaddr, msg.src
) in responses and is_tester_present_response(
msg, subaddr):
if debug:
print(
f"CAN-RX: {hex(msg.address)} - 0x{bytes.hex(msg.dat)}"
)
if (msg.address, subaddr,
msg.src) in ecu_responses:
print(
f"Duplicate ECU address: {hex(msg.address)}"
)
ecu_responses.add((msg.address, subaddr, msg.src))
except Exception:
cloudlog.warning(f"ECU addr scan exception: {traceback.format_exc()}")
return ecu_responses
def flash_panda(panda_serial: str) -> Panda:
panda = Panda(panda_serial)
fw_signature = get_expected_signature(panda)
internal_panda = panda.is_internal() and not panda.bootstub
panda_version = "bootstub" if panda.bootstub else panda.get_version()
panda_signature = b"" if panda.bootstub else panda.get_signature()
cloudlog.warning(
f"Panda {panda_serial} connected, version: {panda_version}, signature {panda_signature.hex()[:16]}, expected {fw_signature.hex()[:16]}"
)
if panda.bootstub or panda_signature != fw_signature:
cloudlog.info("Panda firmware out of date, update required")
panda.flash()
cloudlog.info("Done flashing")
if panda.bootstub:
bootstub_version = panda.get_version()
cloudlog.info(
f"Flashed firmware not booting, flashing development bootloader. Bootstub version: {bootstub_version}"
)
if internal_panda:
HARDWARE.recover_internal_panda()
panda.recover(reset=(not internal_panda))
cloudlog.info("Done flashing bootloader")
if panda.bootstub:
cloudlog.info("Panda still not booting, exiting")
raise AssertionError
panda_signature = panda.get_signature()
if panda_signature != fw_signature:
cloudlog.info("Version mismatch after flashing, exiting")
raise AssertionError
return panda
def run(self):
# t0 = sec_since_boot()
# reset = 0
for i in range(N+1):
self.solver.set(i, 'p', self.params[i])
self.solver.constraints_set(0, "lbx", self.x0)
self.solver.constraints_set(0, "ubx", self.x0)
self.solution_status = self.solver.solve()
self.solve_time = float(self.solver.get_stats('time_tot')[0])
self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
self.time_linearization = float(self.solver.get_stats('time_lin')[0])
self.time_integrator = float(self.solver.get_stats('time_sim')[0])
# qp_iter = self.solver.get_stats('statistics')[-1][-1] # SQP_RTI specific
# print(f"long_mpc timings: tot {self.solve_time:.2e}, qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e}, integrator {self.time_integrator:.2e}, qp_iter {qp_iter}")
# res = self.solver.get_residuals()
# print(f"long_mpc residuals: {res[0]:.2e}, {res[1]:.2e}, {res[2]:.2e}, {res[3]:.2e}")
# self.solver.print_statistics()
for i in range(N+1):
self.x_sol[i] = self.solver.get(i, 'x')
for i in range(N):
self.u_sol[i] = self.solver.get(i, 'u')
self.v_solution = self.x_sol[:,1]
self.a_solution = self.x_sol[:,2]
self.j_solution = self.u_sol[:,0]
self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
t = sec_since_boot()
if self.solution_status != 0:
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}")
self.reset()
def launcher(proc: str, name: str) -> None:
try:
# import the process
mod = importlib.import_module(proc)
# rename the process
setproctitle(proc)
# create new context since we forked
messaging.context = messaging.Context()
# add daemon name tag to logs
cloudlog.bind(daemon=name)
sentry.set_tag("daemon", name)
# exec the process
getattr(mod, 'main')()
except KeyboardInterrupt:
cloudlog.warning(f"child {proc} got SIGINT")
except Exception:
# can't install the crash handler because sys.excepthook doesn't play nice
# with threads, so catch it here.
sentry.capture_exception()
raise
def get_fw_versions(logcan,
sendcan,
query_brand=None,
extra=None,
timeout=0.1,
debug=False,
progress=False):
versions = get_interface_attr('FW_VERSIONS', ignore_none=True)
if query_brand is not None:
versions = {query_brand: versions[query_brand]}
if extra is not None:
versions.update(extra)
# Extract ECU addresses to query from fingerprints
# ECUs using a subaddress need be queried one by one, the rest can be done in parallel
addrs = []
parallel_addrs = []
ecu_types = {}
for brand, brand_versions in versions.items():
for c in brand_versions.values():
for ecu_type, addr, sub_addr in c.keys():
a = (brand, addr, sub_addr)
if a not in ecu_types:
ecu_types[a] = ecu_type
if sub_addr is None:
if a not in parallel_addrs:
parallel_addrs.append(a)
else:
if [a] not in addrs:
addrs.append([a])
addrs.insert(0, parallel_addrs)
# Get versions and build capnp list to put into CarParams
car_fw = []
requests = [
r for r in REQUESTS if query_brand is None or r.brand == query_brand
]
for addr in tqdm(addrs, disable=not progress):
for addr_chunk in chunks(addr):
for r in requests:
try:
addrs = [(a, s) for (b, a, s) in addr_chunk
if b in (r.brand, 'any') and (
len(r.whitelist_ecus) == 0 or ecu_types[
(b, a, s)] in r.whitelist_ecus)]
if addrs:
query = IsoTpParallelQuery(sendcan,
logcan,
r.bus,
addrs,
r.request,
r.response,
r.rx_offset,
debug=debug)
for (addr, rx_addr
), version in query.get_data(timeout).items():
f = car.CarParams.CarFw.new_message()
f.ecu = ecu_types.get((r.brand, addr[0], addr[1]),
Ecu.unknown)
f.fwVersion = version
f.address = addr[0]
f.responseAddress = rx_addr
f.request = r.request
f.brand = r.brand
f.bus = r.bus
if addr[1] is not None:
f.subAddress = addr[1]
car_fw.append(f)
except Exception:
cloudlog.warning(
f"FW query exception: {traceback.format_exc()}")
return car_fw
def hw_state_thread(end_event, hw_queue):
"""Handles non critical hardware state, and sends over queue"""
count = 0
registered_count = 0
prev_hw_state = None
modem_version = None
modem_nv = None
modem_configured = False
while not end_event.is_set():
# these are expensive calls. update every 10s
if (count % int(10. / DT_TRML)) == 0:
try:
network_type = HARDWARE.get_network_type()
modem_temps = HARDWARE.get_modem_temperatures()
if len(modem_temps) == 0 and prev_hw_state is not None:
modem_temps = prev_hw_state.modem_temps
# Log modem version once
if AGNOS and ((modem_version is None) or (modem_nv is None)):
modem_version = HARDWARE.get_modem_version() # pylint: disable=assignment-from-none
modem_nv = HARDWARE.get_modem_nv() # pylint: disable=assignment-from-none
if (modem_version is not None) and (modem_nv is not None):
cloudlog.event("modem version",
version=modem_version,
nv=modem_nv)
hw_state = HardwareState(
network_type=network_type,
network_metered=HARDWARE.get_network_metered(network_type),
network_strength=HARDWARE.get_network_strength(
network_type),
network_info=HARDWARE.get_network_info(),
nvme_temps=HARDWARE.get_nvme_temperatures(),
modem_temps=modem_temps,
)
try:
hw_queue.put_nowait(hw_state)
except queue.Full:
pass
if AGNOS and (hw_state.network_info
is not None) and (hw_state.network_info.get(
'state', None) == "REGISTERED"):
registered_count += 1
else:
registered_count = 0
if registered_count > 10:
cloudlog.warning(
f"Modem stuck in registered state {hw_state.network_info}. nmcli conn up lte"
)
os.system("nmcli conn up lte")
registered_count = 0
# TODO: remove this once the config is in AGNOS
if not modem_configured and len(HARDWARE.get_sim_info().get(
'sim_id', '')) > 0:
cloudlog.warning("configuring modem")
HARDWARE.configure_modem()
modem_configured = True
prev_hw_state = hw_state
except Exception:
cloudlog.exception("Error getting hardware state")
count += 1
time.sleep(DT_TRML)
def send_instruction(self):
msg = messaging.new_message('navInstruction')
if self.step_idx is None:
msg.valid = False
self.pm.send('navInstruction', msg)
return
step = self.route[self.step_idx]
geometry = self.route_geometry[self.step_idx]
along_geometry = distance_along_geometry(geometry, self.last_position)
distance_to_maneuver_along_geometry = step['distance'] - along_geometry
# Current instruction
msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry
parse_banner_instructions(msg.navInstruction,
step['bannerInstructions'],
distance_to_maneuver_along_geometry)
# Compute total remaining time and distance
remaning = 1.0 - along_geometry / max(step['distance'], 1)
total_distance = step['distance'] * remaning
total_time = step['duration'] * remaning
total_time_typical = step['duration_typical'] * remaning
# Add up totals for future steps
for i in range(self.step_idx + 1, len(self.route)):
total_distance += self.route[i]['distance']
total_time += self.route[i]['duration']
total_time_typical += self.route[i]['duration_typical']
msg.navInstruction.distanceRemaining = total_distance
msg.navInstruction.timeRemaining = total_time
msg.navInstruction.timeRemainingTypical = total_time_typical
# Speed limit
closest_idx, closest = min(
enumerate(geometry),
key=lambda p: p[1].distance_to(self.last_position))
if closest_idx > 0:
# If we are not past the closest point, show previous
if along_geometry < distance_along_geometry(
geometry, geometry[closest_idx]):
closest = geometry[closest_idx - 1]
if ('maxspeed' in closest.annotations) and self.localizer_valid:
msg.navInstruction.speedLimit = closest.annotations['maxspeed']
# Speed limit sign type
if 'speedLimitSign' in step:
if step['speedLimitSign'] == 'mutcd':
msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.mutcd
elif step['speedLimitSign'] == 'vienna':
msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.vienna
self.pm.send('navInstruction', msg)
# Transition to next route segment
if distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD:
if self.step_idx + 1 < len(self.route):
self.step_idx += 1
self.recompute_backoff = 0
self.recompute_countdown = 0
else:
cloudlog.warning("Destination reached")
Params().delete("NavDestination")
# Clear route if driving away from destination
dist = self.nav_destination.distance_to(self.last_position)
if dist > REROUTE_DISTANCE:
self.clear_route()
def calculate_route(self, destination):
cloudlog.warning(
f"Calculating route {self.last_position} -> {destination}")
self.nav_destination = destination
params = {
'access_token': self.mapbox_token,
'annotations': 'maxspeed',
'geometries': 'geojson',
'overview': 'full',
'steps': 'true',
'banner_instructions': 'true',
'alternatives': 'false',
}
if self.last_bearing is not None:
params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90;"
url = self.mapbox_host + f'/directions/v5/mapbox/driving-traffic/{self.last_position.longitude},{self.last_position.latitude};{destination.longitude},{destination.latitude}'
try:
resp = requests.get(url, params=params)
resp.raise_for_status()
r = resp.json()
if len(r['routes']):
self.route = r['routes'][0]['legs'][0]['steps']
self.route_geometry = []
maxspeed_idx = 0
maxspeeds = r['routes'][0]['legs'][0]['annotation']['maxspeed']
# Convert coordinates
for step in self.route:
coords = []
for c in step['geometry']['coordinates']:
coord = Coordinate.from_mapbox_tuple(c)
# Last step does not have maxspeed
if (maxspeed_idx < len(maxspeeds)):
maxspeed = maxspeeds[maxspeed_idx]
if ('unknown'
not in maxspeed) and ('none'
not in maxspeed):
coord.annotations['maxspeed'] = maxspeed_to_ms(
maxspeed)
coords.append(coord)
maxspeed_idx += 1
self.route_geometry.append(coords)
maxspeed_idx -= 1 # Every segment ends with the same coordinate as the start of the next
self.step_idx = 0
else:
cloudlog.warning("Got empty route response")
self.clear_route()
except requests.exceptions.RequestException:
cloudlog.exception("failed to get route")
self.clear_route()
self.send_route()
def thermald_thread(end_event, hw_queue):
pm = messaging.PubMaster(['deviceState'])
sm = messaging.SubMaster([
"peripheralState", "gpsLocationExternal", "controlsState",
"pandaStates"
],
poll=["pandaStates"])
count = 0
onroad_conditions: Dict[str, bool] = {
"ignition": False,
}
startup_conditions: Dict[str, bool] = {}
startup_conditions_prev: Dict[str, bool] = {}
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
last_hw_state = HardwareState(
network_type=NetworkType.none,
network_metered=False,
network_strength=NetworkStrength.unknown,
network_info=None,
nvme_temps=[],
modem_temps=[],
)
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML)
should_start_prev = False
in_car = False
engaged_prev = False
params = Params()
power_monitor = PowerMonitoring()
HARDWARE.initialize_hardware()
thermal_config = HARDWARE.get_thermal_config()
fan_controller = None
while not end_event.is_set():
sm.update(PANDA_STATES_TIMEOUT)
pandaStates = sm['pandaStates']
peripheralState = sm['peripheralState']
msg = read_thermal(thermal_config)
if sm.updated['pandaStates'] and len(pandaStates) > 0:
# Set ignition based on any panda connected
onroad_conditions["ignition"] = any(
ps.ignitionLine or ps.ignitionCan for ps in pandaStates
if ps.pandaType != log.PandaState.PandaType.unknown)
pandaState = pandaStates[0]
in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected
# Setup fan handler on first connect to panda
if fan_controller is None and peripheralState.pandaType != log.PandaState.PandaType.unknown:
if TICI:
fan_controller = TiciFanController()
elif (sec_since_boot() -
sm.rcv_time['pandaStates']) > DISCONNECT_TIMEOUT:
if onroad_conditions["ignition"]:
onroad_conditions["ignition"] = False
cloudlog.error("panda timed out onroad")
try:
last_hw_state = hw_queue.get_nowait()
except queue.Empty:
pass
msg.deviceState.freeSpacePercent = get_available_percent(default=100.0)
msg.deviceState.memoryUsagePercent = int(
round(psutil.virtual_memory().percent))
msg.deviceState.cpuUsagePercent = [
int(round(n)) for n in psutil.cpu_percent(percpu=True)
]
msg.deviceState.gpuUsagePercent = int(
round(HARDWARE.get_gpu_usage_percent()))
msg.deviceState.networkType = last_hw_state.network_type
msg.deviceState.networkMetered = last_hw_state.network_metered
msg.deviceState.networkStrength = last_hw_state.network_strength
if last_hw_state.network_info is not None:
msg.deviceState.networkInfo = last_hw_state.network_info
msg.deviceState.nvmeTempC = last_hw_state.nvme_temps
msg.deviceState.modemTempC = last_hw_state.modem_temps
msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness(
)
msg.deviceState.usbOnline = HARDWARE.get_usb_present()
current_filter.update(msg.deviceState.batteryCurrent / 1e6)
max_comp_temp = temp_filter.update(
max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC,
max(msg.deviceState.gpuTempC)))
if fan_controller is not None:
msg.deviceState.fanSpeedPercentDesired = fan_controller.update(
max_comp_temp, onroad_conditions["ignition"])
is_offroad_for_5_min = (started_ts is None) and (
(not started_seen) or (off_ts is None) or
(sec_since_boot() - off_ts > 60 * 5))
if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP:
# If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107
thermal_status = ThermalStatus.danger
else:
current_band = THERMAL_BANDS[thermal_status]
band_idx = list(THERMAL_BANDS.keys()).index(thermal_status)
if current_band.min_temp is not None and max_comp_temp < current_band.min_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1]
elif current_band.max_temp is not None and max_comp_temp > current_band.max_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1]
# **** starting logic ****
# Ensure date/time are valid
now = datetime.datetime.utcnow()
startup_conditions["time_valid"] = (now.year > 2020) or (
now.year == 2020 and now.month >= 10)
set_offroad_alert_if_changed("Offroad_InvalidTime",
(not startup_conditions["time_valid"]))
startup_conditions["up_to_date"] = params.get(
"Offroad_ConnectivityNeeded") is None or params.get_bool(
"DisableUpdates") or params.get_bool("SnoozeUpdate")
startup_conditions["not_uninstalling"] = not params.get_bool(
"DoUninstall")
startup_conditions["accepted_terms"] = params.get(
"HasAcceptedTerms") == terms_version
# with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
params.get_bool("Passive")
startup_conditions["not_driver_view"] = not params.get_bool(
"IsDriverViewEnabled")
startup_conditions["not_taking_snapshot"] = not params.get_bool(
"IsTakingSnapshot")
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
onroad_conditions[
"device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed(
"Offroad_TemperatureTooHigh",
(not onroad_conditions["device_temp_good"]))
# TODO: this should move to TICI.initialize_hardware, but we currently can't import params there
if TICI:
if not os.path.isfile("/persist/comma/living-in-the-moment"):
if not Path("/data/media").is_mount():
set_offroad_alert_if_changed("Offroad_StorageMissing",
True)
else:
# check for bad NVMe
try:
with open("/sys/block/nvme0n1/device/model") as f:
model = f.read().strip()
if not model.startswith(
"Samsung SSD 980") and params.get(
"Offroad_BadNvme") is None:
set_offroad_alert_if_changed(
"Offroad_BadNvme", True)
cloudlog.event("Unsupported NVMe",
model=model,
error=True)
except Exception:
pass
# Handle offroad/onroad transition
should_start = all(onroad_conditions.values())
if started_ts is None:
should_start = should_start and all(startup_conditions.values())
if should_start != should_start_prev or (count == 0):
params.put_bool("IsOnroad", should_start)
params.put_bool("IsOffroad", not should_start)
params.put_bool("IsEngaged", False)
engaged_prev = False
HARDWARE.set_power_save(not should_start)
if sm.updated['controlsState']:
engaged = sm['controlsState'].enabled
if engaged != engaged_prev:
params.put_bool("IsEngaged", engaged)
engaged_prev = engaged
try:
with open('/dev/kmsg', 'w') as kmsg:
kmsg.write(f"<3>[thermald] engaged: {engaged}\n")
except Exception:
pass
if should_start:
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
else:
if onroad_conditions["ignition"] and (startup_conditions !=
startup_conditions_prev):
cloudlog.event("Startup blocked",
startup_conditions=startup_conditions,
onroad_conditions=onroad_conditions)
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
# Offroad power monitoring
power_monitor.calculate(peripheralState, onroad_conditions["ignition"])
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(
0, power_monitor.get_car_battery_capacity())
current_power_draw = HARDWARE.get_current_power_draw()
statlog.sample("power_draw", current_power_draw)
msg.deviceState.powerDrawW = current_power_draw
som_power_draw = HARDWARE.get_som_power_draw()
statlog.sample("som_power_draw", som_power_draw)
msg.deviceState.somPowerDrawW = som_power_draw
# Check if we need to disable charging (handled by boardd)
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(
onroad_conditions["ignition"], in_car, off_ts)
# Check if we need to shut down
if power_monitor.should_shutdown(peripheralState,
onroad_conditions["ignition"], in_car,
off_ts, started_seen):
cloudlog.warning(f"shutting device down, offroad since {off_ts}")
params.put_bool("DoShutdown", True)
msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.deviceState.started = started_ts is not None
msg.deviceState.startedMonoTime = int(1e9 * (started_ts or 0))
last_ping = params.get("LastAthenaPingTime")
if last_ping is not None:
msg.deviceState.lastAthenaPingTime = int(last_ping)
msg.deviceState.thermalStatus = thermal_status
pm.send("deviceState", msg)
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# Log to statsd
statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent)
statlog.gauge("gpu_usage_percent", msg.deviceState.gpuUsagePercent)
statlog.gauge("memory_usage_percent",
msg.deviceState.memoryUsagePercent)
for i, usage in enumerate(msg.deviceState.cpuUsagePercent):
statlog.gauge(f"cpu{i}_usage_percent", usage)
for i, temp in enumerate(msg.deviceState.cpuTempC):
statlog.gauge(f"cpu{i}_temperature", temp)
for i, temp in enumerate(msg.deviceState.gpuTempC):
statlog.gauge(f"gpu{i}_temperature", temp)
statlog.gauge("memory_temperature", msg.deviceState.memoryTempC)
statlog.gauge("ambient_temperature", msg.deviceState.ambientTempC)
for i, temp in enumerate(msg.deviceState.pmicTempC):
statlog.gauge(f"pmic{i}_temperature", temp)
for i, temp in enumerate(last_hw_state.nvme_temps):
statlog.gauge(f"nvme_temperature{i}", temp)
for i, temp in enumerate(last_hw_state.modem_temps):
statlog.gauge(f"modem_temperature{i}", temp)
statlog.gauge("fan_speed_percent_desired",
msg.deviceState.fanSpeedPercentDesired)
statlog.gauge("screen_brightness_percent",
msg.deviceState.screenBrightnessPercent)
# report to server once every 10 minutes
if (count % int(600. / DT_TRML)) == 0:
cloudlog.event(
"STATUS_PACKET",
count=count,
pandaStates=[
strip_deprecated_keys(p.to_dict()) for p in pandaStates
],
peripheralState=strip_deprecated_keys(
peripheralState.to_dict()),
location=(strip_deprecated_keys(
sm["gpsLocationExternal"].to_dict())
if sm.alive["gpsLocationExternal"] else None),
deviceState=strip_deprecated_keys(msg.to_dict()))
count += 1
def get_data(self, timeout, total_timeout=60.):
self._drain_rx()
# Create message objects
msgs = {}
request_counter = {}
request_done = {}
for tx_addr, rx_addr in self.msg_addrs.items():
# rx_addr not set when using functional tx addr
id_addr = rx_addr or tx_addr[0]
sub_addr = tx_addr[1]
can_client = CanClient(self._can_tx,
partial(self._can_rx,
id_addr,
sub_addr=sub_addr),
tx_addr[0],
rx_addr,
self.bus,
sub_addr=sub_addr,
debug=self.debug)
max_len = 8 if sub_addr is None else 7
msg = IsoTpMessage(can_client,
timeout=0,
max_len=max_len,
debug=self.debug)
msg.send(self.request[0])
msgs[tx_addr] = msg
request_counter[tx_addr] = 0
request_done[tx_addr] = False
results = {}
start_time = time.monotonic()
response_timeouts = {
tx_addr: start_time + timeout
for tx_addr in self.msg_addrs
}
while True:
self.rx()
if all(request_done.values()):
break
for tx_addr, msg in msgs.items():
try:
dat: Optional[bytes] = msg.recv()
except Exception:
cloudlog.exception("Error processing UDS response")
request_done[tx_addr] = True
continue
if not dat:
continue
counter = request_counter[tx_addr]
expected_response = self.response[counter]
response_valid = dat[:len(expected_response
)] == expected_response
if response_valid:
response_timeouts[tx_addr] = time.monotonic() + timeout
if counter + 1 < len(self.request):
msg.send(self.request[counter + 1])
request_counter[tx_addr] += 1
else:
results[(tx_addr, msg._can_client.rx_addr
)] = dat[len(expected_response):]
request_done[tx_addr] = True
else:
error_code = dat[2] if len(dat) > 2 else -1
if error_code == 0x78:
response_timeouts[tx_addr] = time.monotonic(
) + self.response_pending_timeout
if self.debug:
cloudlog.warning(
f"iso-tp query response pending: {tx_addr}")
else:
response_timeouts[tx_addr] = 0
request_done[tx_addr] = True
cloudlog.warning(
f"iso-tp query bad response: {tx_addr} - 0x{dat.hex()}"
)
cur_time = time.monotonic()
if cur_time - max(response_timeouts.values()) > 0:
for tx_addr in msgs:
if request_counter[tx_addr] > 0 and not request_done[
tx_addr]:
cloudlog.warning(
f"iso-tp query timeout after receiving response: {tx_addr}"
)
break
if cur_time - start_time > total_timeout:
cloudlog.warning("iso-tp query timeout while receiving data")
break
return results
def register(show_spinner=False) -> Optional[str]:
params = Params()
params.put("SubscriberInfo", HARDWARE.get_subscriber_info())
IMEI = params.get("IMEI", encoding='utf8')
HardwareSerial = params.get("HardwareSerial", encoding='utf8')
dongle_id: Optional[str] = params.get("DongleId", encoding='utf8')
needs_registration = None in (IMEI, HardwareSerial, dongle_id)
pubkey = Path(PERSIST + "/comma/id_rsa.pub")
if not pubkey.is_file():
dongle_id = UNREGISTERED_DONGLE_ID
cloudlog.warning(f"missing public key: {pubkey}")
elif needs_registration:
if show_spinner:
spinner = Spinner()
spinner.update("registering device")
# Create registration token, in the future, this key will make JWTs directly
with open(PERSIST +
"/comma/id_rsa.pub") as f1, open(PERSIST +
"/comma/id_rsa") as f2:
public_key = f1.read()
private_key = f2.read()
# Block until we get the imei
serial = HARDWARE.get_serial()
start_time = time.monotonic()
imei1: Optional[str] = None
imei2: Optional[str] = None
while imei1 is None and imei2 is None:
try:
imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1)
except Exception:
cloudlog.exception("Error getting imei, trying again...")
time.sleep(1)
if time.monotonic() - start_time > 60 and show_spinner:
spinner.update(
f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})"
)
params.put("IMEI", imei1)
params.put("HardwareSerial", serial)
backoff = 0
start_time = time.monotonic()
while True:
try:
register_token = jwt.encode(
{
'register': True,
'exp': datetime.utcnow() + timedelta(hours=1)
},
private_key,
algorithm='RS256')
cloudlog.info("getting pilotauth")
resp = api_get("v2/pilotauth/",
method='POST',
timeout=15,
imei=imei1,
imei2=imei2,
serial=serial,
public_key=public_key,
register_token=register_token)
if resp.status_code in (402, 403):
cloudlog.info(
f"Unable to register device, got {resp.status_code}")
dongle_id = UNREGISTERED_DONGLE_ID
else:
dongleauth = json.loads(resp.text)
dongle_id = dongleauth["dongle_id"]
break
except Exception:
cloudlog.exception("failed to authenticate")
backoff = min(backoff + 1, 15)
time.sleep(backoff)
if time.monotonic() - start_time > 60 and show_spinner:
spinner.update(
f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})"
)
if show_spinner:
spinner.close()
if dongle_id:
params.put("DongleId", dongle_id)
set_offroad_alert("Offroad_UnofficialHardware",
(dongle_id == UNREGISTERED_DONGLE_ID) and not PC)
return dongle_id
def update(self, sm):
v_ego = sm['carState'].vEgo
measured_curvature = sm['controlsState'].curvature
# Parse model predictions
md = sm['modelV2']
self.LP.parse_model(md)
if len(md.position.x) == TRAJECTORY_SIZE and len(
md.orientation.x) == TRAJECTORY_SIZE:
self.path_xyz = np.column_stack(
[md.position.x, md.position.y, md.position.z])
self.t_idxs = np.array(md.position.t)
self.plan_yaw = list(md.orientation.z)
if len(md.position.xStd) == TRAJECTORY_SIZE:
self.path_xyz_stds = np.column_stack(
[md.position.xStd, md.position.yStd, md.position.zStd])
# Lane change logic
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
self.DH.update(sm['carState'], sm['controlsState'].active,
lane_change_prob)
# Turn off lanes during lane change
if self.DH.desire == log.LateralPlan.Desire.laneChangeRight or self.DH.desire == log.LateralPlan.Desire.laneChangeLeft:
self.LP.lll_prob *= self.DH.lane_change_ll_prob
self.LP.rll_prob *= self.DH.lane_change_ll_prob
# Calculate final driving path and set MPC costs
if self.use_lanelines:
d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING,
MPC_COST_LAT.STEER_RATE)
else:
d_path_xyz = self.path_xyz
# Heading cost is useful at low speed, otherwise end of plan can be off-heading
heading_cost = interp(v_ego, [5.0, 10.0],
[MPC_COST_LAT.HEADING, 0.15])
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost,
MPC_COST_LAT.STEER_RATE)
y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1],
np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1],
np.linalg.norm(self.path_xyz, axis=1),
self.plan_yaw)
self.y_pts = y_pts
assert len(y_pts) == LAT_MPC_N + 1
assert len(heading_pts) == LAT_MPC_N + 1
# self.x0[4] = v_ego
p = np.array([v_ego, CAR_ROTATION_RADIUS])
self.lat_mpc.run(self.x0, p, y_pts, heading_pts)
# init state for next
# mpc.u_sol is the desired curvature rate given x0 curv state.
# with x0[3] = measured_curvature, this would be the actual desired rate.
# instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control.
self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1],
self.lat_mpc.x_sol[:, 3])
# Check for infeasible MPC solution
mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any()
t = sec_since_boot()
if mpc_nans or self.lat_mpc.solution_status != 0:
self.reset_mpc()
self.x0[3] = measured_curvature
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.lat_mpc.cost > 20000. or mpc_nans:
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
def main() -> None:
params = Params()
if params.get_bool("DisableUpdates"):
cloudlog.warning("updates are disabled by the DisableUpdates param")
exit(0)
ov_lock_fd = open(LOCK_FILE, 'w')
try:
fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
except OSError as e:
raise RuntimeError(
"couldn't get overlay lock; is another instance running?") from e
# Set low io priority
proc = psutil.Process()
if psutil.LINUX:
proc.ionice(psutil.IOPRIO_CLASS_BE, value=7)
# Check if we just performed an update
if Path(os.path.join(STAGING_ROOT, "old_openpilot")).is_dir():
cloudlog.event("update installed")
if not params.get("InstallDate"):
t = datetime.datetime.utcnow().isoformat()
params.put("InstallDate", t.encode('utf8'))
overlay_init = Path(os.path.join(BASEDIR, ".overlay_init"))
overlay_init.unlink(missing_ok=True)
update_failed_count = 0 # TODO: Load from param?
wait_helper = WaitTimeHelper(proc)
# Run the update loop
while not wait_helper.shutdown:
wait_helper.ready_event.clear()
# Attempt an update
exception = None
new_version = False
update_failed_count += 1
try:
init_overlay()
# TODO: still needed? skip this and just fetch?
# Lightweight internt check
internet_ok, update_available = check_for_update()
if internet_ok and not update_available:
update_failed_count = 0
# Fetch update
if internet_ok:
new_version = fetch_update(wait_helper)
update_failed_count = 0
except subprocess.CalledProcessError as e:
cloudlog.event("update process failed",
cmd=e.cmd,
output=e.output,
returncode=e.returncode)
exception = f"command failed: {e.cmd}\n{e.output}"
overlay_init.unlink(missing_ok=True)
except Exception as e:
cloudlog.exception("uncaught updated exception, shouldn't happen")
exception = str(e)
overlay_init.unlink(missing_ok=True)
if not wait_helper.shutdown:
try:
set_params(new_version, update_failed_count, exception)
except Exception:
cloudlog.exception(
"uncaught updated exception while setting params, shouldn't happen"
)
# infrequent attempts if we successfully updated recently
wait_helper.sleep(5 * 60 if update_failed_count > 0 else 90 * 60)
dismount_overlay()
def main() -> NoReturn:
unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report,
True)
unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(
gps_measurement_report_sv, True)
unpack_glonass_meas, size_glonass_meas = dict_unpacker(
glonass_measurement_report, True)
unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(
glonass_measurement_report_sv, True)
unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(
oemdre_measurement_report, True)
unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(
oemdre_measurement_report_sv, True)
log_types = [
LOG_GNSS_GPS_MEASUREMENT_REPORT,
LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
]
pub_types = ['qcomGnss']
if int(os.getenv("PUBLISH_EXTERNAL", "0")) == 1:
unpack_position, _ = dict_unpacker(position_report)
log_types.append(LOG_GNSS_POSITION_REPORT)
pub_types.append("gpsLocationExternal")
# connect to modem
diag = ModemDiag()
# NV enable OEMDRE
# TODO: it has to reboot for this to take effect
DIAG_NV_READ_F = 38
DIAG_NV_WRITE_F = 39
NV_GNSS_OEM_FEATURE_MASK = 7165
opcode, payload = send_recv(diag, DIAG_NV_WRITE_F,
pack('<HI', NV_GNSS_OEM_FEATURE_MASK, 1))
opcode, payload = send_recv(diag, DIAG_NV_READ_F,
pack('<H', NV_GNSS_OEM_FEATURE_MASK))
def try_setup_logs(diag, log_types):
for _ in range(5):
try:
setup_logs(diag, log_types)
break
except Exception:
pass
def disable_logs(sig, frame):
os.system(
"mmcli -m 0 --location-disable-gps-raw --location-disable-gps-nmea"
)
cloudlog.warning("rawgpsd: shutting down")
try_setup_logs(diag, [])
cloudlog.warning("rawgpsd: logs disabled")
sys.exit(0)
signal.signal(signal.SIGINT, disable_logs)
try_setup_logs(diag, log_types)
cloudlog.warning("rawgpsd: setup logs done")
# disable DPO power savings for more accuracy
os.system("mmcli -m 0 --command='AT+QGPSCFG=\"dpoenable\",0'")
os.system(
"mmcli -m 0 --location-enable-gps-raw --location-enable-gps-nmea")
# enable OEMDRE mode
DIAG_SUBSYS_CMD_F = 75
DIAG_SUBSYS_GPS = 13
CGPS_DIAG_PDAPI_CMD = 0x64
CGPS_OEM_CONTROL = 202
GPSDIAG_OEMFEATURE_DRE = 1
GPSDIAG_OEM_DRE_ON = 1
# gpsdiag_OemControlReqType
opcode, payload = send_recv(
diag,
DIAG_SUBSYS_CMD_F,
pack(
'<BHBBIIII',
DIAG_SUBSYS_GPS, # Subsystem Id
CGPS_DIAG_PDAPI_CMD, # Subsystem Command Code
CGPS_OEM_CONTROL, # CGPS Command Code
0, # Version
GPSDIAG_OEMFEATURE_DRE,
GPSDIAG_OEM_DRE_ON,
0,
0))
pm = messaging.PubMaster(pub_types)
while 1:
opcode, payload = diag.recv()
assert opcode == DIAG_LOG_F
(pending_msgs, log_outer_length), inner_log_packet = unpack_from(
'<BH', payload), payload[calcsize('<BH'):]
if pending_msgs > 0:
cloudlog.debug("have %d pending messages" % pending_msgs)
assert log_outer_length == len(inner_log_packet)
(log_inner_length, log_type, log_time), log_payload = unpack_from(
'<HHQ', inner_log_packet), inner_log_packet[calcsize('<HHQ'):]
assert log_inner_length == len(inner_log_packet)
if log_type not in log_types:
continue
if DEBUG:
print("%.4f: got log: %x len %d" %
(time.time(), log_type, len(log_payload)))
if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT:
msg = messaging.new_message('qcomGnss')
gnss = msg.qcomGnss
gnss.logTs = log_time
gnss.init('drMeasurementReport')
report = gnss.drMeasurementReport
dat = unpack_oemdre_meas(log_payload)
for k, v in dat.items():
if k in ["gpsTimeBias", "gpsClockTimeUncertainty"]:
k += "Ms"
if k == "version":
assert v == 2
elif k == "svCount" or k.startswith("cdmaClockInfo["):
# TODO: should we save cdmaClockInfo?
pass
elif k == "systemRtcValid":
setattr(report, k, bool(v))
else:
setattr(report, k, v)
report.init('sv', dat['svCount'])
sats = log_payload[size_oemdre_meas:]
for i in range(dat['svCount']):
sat = unpack_oemdre_meas_sv(
sats[size_oemdre_meas_sv * i:size_oemdre_meas_sv *
(i + 1)])
sv = report.sv[i]
sv.init('measurementStatus')
for k, v in sat.items():
if k in ["unkn", "measurementStatus2"]:
pass
elif k == "multipathEstimateValid":
sv.measurementStatus.multipathEstimateIsValid = bool(v)
elif k == "directionValid":
sv.measurementStatus.directionIsValid = bool(v)
elif k == "goodParity":
setattr(sv, k, bool(v))
elif k == "measurementStatus":
for kk, vv in measurementStatusFields.items():
setattr(sv.measurementStatus, kk,
bool(v & (1 << vv)))
else:
setattr(sv, k, v)
pm.send('qcomGnss', msg)
elif log_type == LOG_GNSS_POSITION_REPORT:
report = unpack_position(log_payload)
if report["u_PosSource"] != 2:
continue
vNED = [
report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"],
-report["q_FltVelEnuMps[2]"]
]
vNEDsigma = [
report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"],
-report["q_FltVelSigmaMps[2]"]
]
msg = messaging.new_message('gpsLocationExternal')
gps = msg.gpsLocationExternal
gps.flags = 1
gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180 / math.pi
gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180 / math.pi
gps.altitude = report["q_FltFinalPosAlt"]
gps.speed = math.sqrt(sum([x**2 for x in vNED]))
gps.bearingDeg = report["q_FltHeadingRad"] * 180 / math.pi
# TODO: this probably isn't right, use laika for this
gps.timestamp = report['w_GpsWeekNumber'] * 604800 * 1000 + report[
'q_GpsFixTimeMs']
gps.source = log.GpsLocationData.SensorSource.qcomdiag
gps.vNED = vNED
gps.verticalAccuracy = report["q_FltVdop"]
gps.bearingAccuracyDeg = report[
"q_FltHeadingUncRad"] * 180 / math.pi
gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma]))
pm.send('gpsLocationExternal', msg)
if log_type in [
LOG_GNSS_GPS_MEASUREMENT_REPORT,
LOG_GNSS_GLONASS_MEASUREMENT_REPORT
]:
msg = messaging.new_message('qcomGnss')
gnss = msg.qcomGnss
gnss.logTs = log_time
gnss.init('measurementReport')
report = gnss.measurementReport
if log_type == LOG_GNSS_GPS_MEASUREMENT_REPORT:
dat = unpack_gps_meas(log_payload)
sats = log_payload[size_gps_meas:]
unpack_meas_sv, size_meas_sv = unpack_gps_meas_sv, size_gps_meas_sv
report.source = 0 # gps
measurement_status_fields = (
measurementStatusFields.items(),
measurementStatusGPSFields.items())
elif log_type == LOG_GNSS_GLONASS_MEASUREMENT_REPORT:
dat = unpack_glonass_meas(log_payload)
sats = log_payload[size_glonass_meas:]
unpack_meas_sv, size_meas_sv = unpack_glonass_meas_sv, size_glonass_meas_sv
report.source = 1 # glonass
measurement_status_fields = (
measurementStatusFields.items(),
measurementStatusGlonassFields.items())
else:
assert False
for k, v in dat.items():
if k == "version":
assert v == 0
elif k == "week":
report.gpsWeek = v
elif k == "svCount":
pass
else:
setattr(report, k, v)
report.init('sv', dat['svCount'])
if dat['svCount'] > 0:
assert len(sats) // dat['svCount'] == size_meas_sv
for i in range(dat['svCount']):
sv = report.sv[i]
sv.init('measurementStatus')
sat = unpack_meas_sv(sats[size_meas_sv * i:size_meas_sv *
(i + 1)])
for k, v in sat.items():
if k == "parityErrorCount":
sv.gpsParityErrorCount = v
elif k == "frequencyIndex":
sv.glonassFrequencyIndex = v
elif k == "hemmingErrorCount":
sv.glonassHemmingErrorCount = v
elif k == "measurementStatus":
for kk, vv in itertools.chain(
*measurement_status_fields):
setattr(sv.measurementStatus, kk,
bool(v & (1 << vv)))
elif k == "miscStatus":
for kk, vv in miscStatusFields.items():
setattr(sv.measurementStatus, kk,
bool(v & (1 << vv)))
elif k == "pad":
pass
else:
setattr(sv, k, v)
pm.send('qcomGnss', msg)
def fingerprint(logcan, sendcan):
fixed_fingerprint = os.environ.get('FINGERPRINT', "")
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
ecu_responses = set()
if 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)
ecu_responses = get_present_ecus(logcan, sendcan)
car_fw = get_fw_versions(logcan, sendcan)
exact_fw_match, fw_candidates = match_fw_to_car(car_fw)
else:
vin = VIN_UNKNOWN
exact_fw_match, fw_candidates, car_fw = True, set(), []
if len(vin) != 17:
cloudlog.event("Malformed VIN", vin=vin, error=True)
vin = VIN_UNKNOWN
cloudlog.warning("VIN %s", vin)
Params().put("CarVin", vin)
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
frame_fingerprint = 25 # 0.25s
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
exact_match = True
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
exact_match = exact_fw_match
if fixed_fingerprint:
car_fingerprint = fixed_fingerprint
source = car.CarParams.FingerprintSource.fixed
cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match,
fw_count=len(car_fw), ecu_responses=ecu_responses, error=True)
return car_fingerprint, finger, vin, car_fw, source, exact_match
