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