def register(show_spinner=False) -> str: params = Params() params.put("SubscriberInfo", HARDWARE.get_subscriber_info()) IMEI = params.get("IMEI", encoding='utf8') HardwareSerial = params.get("HardwareSerial", encoding='utf8') dongle_id = params.get("DongleId", encoding='utf8') needs_registration = None in (IMEI, HardwareSerial, dongle_id) # create a key for auth # your private key is kept on your device persist partition and never sent to our servers # do not erase your persist partition if not os.path.isfile(PERSIST + "/comma/id_rsa.pub"): needs_registration = True cloudlog.warning("generating your personal RSA key") mkdirs_exists_ok(PERSIST + "/comma") assert os.system("openssl genrsa -out " + PERSIST + "/comma/id_rsa.tmp 2048") == 0 assert os.system("openssl rsa -in " + PERSIST + "/comma/id_rsa.tmp -pubout -out " + PERSIST + "/comma/id_rsa.tmp.pub") == 0 os.rename(PERSIST + "/comma/id_rsa.tmp", PERSIST + "/comma/id_rsa") os.rename(PERSIST + "/comma/id_rsa.tmp.pub", PERSIST + "/comma/id_rsa.pub") if 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 imei1, imei2 = None, 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) serial = HARDWARE.get_serial() params.put("IMEI", imei1) params.put("HardwareSerial", serial) backoff = 0 try_count = 0 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=10, imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token) if resp.status_code in (402, 403, 404): 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: try_count += 1 if try_count >= 2: dongle_id = UNREGISTERED_DONGLE_ID break cloudlog.exception("failed to authenticate") backoff = min(backoff + 1, 15) time.sleep(backoff) if show_spinner: spinner.close() if dongle_id: params.put("DongleId", dongle_id) set_offroad_alert("Offroad_UnofficialHardware", dongle_id == UNREGISTERED_DONGLE_ID) return dongle_id
def radard_thread(gctx=None): gc.disable() set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) mocked = CP.carName == "mock" VM = VehicleModel(CP) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module( 'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface context = zmq.Context() # *** subscribe to features and model from visiond poller = zmq.Poller() model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller) live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller) PP = PathPlanner() RI = RadarInterface(CP) last_md_ts = 0 last_l100_ts = 0 # *** publish live20 and liveTracks live20 = messaging.pub_sock(context, service_list['live20'].port) liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port) path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max # Time-alignment rate = 20. # model and radar are both at 20Hz tsv = 1. / rate v_len = 20 # how many speed data points to remember for t alignment with rdr data active = 0 steer_angle = 0. steer_override = False tracks = defaultdict(dict) # Kalman filter stuff: ekfv = EKFV1D() speedSensorV = SimpleSensor(XV, 1, 2) # v_ego v_ego = None v_ego_array = np.zeros([2, v_len]) v_ego_t_aligned = 0. rk = Ratekeeper(rate, print_delay_threshold=np.inf) while 1: rr = RI.update() ar_pts = {} for pt in rr.points: ar_pts[pt.trackId] = [ pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured ] # receive the live100s l100 = None md = None for socket, event in poller.poll(0): if socket is live100: l100 = messaging.recv_one(socket) elif socket is model: md = messaging.recv_one(socket) if l100 is not None: active = l100.live100.active v_ego = l100.live100.vEgo steer_angle = l100.live100.angleSteers steer_override = l100.live100.steerOverride v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame) / rate]], 1) v_ego_array = v_ego_array[:, 1:] last_l100_ts = l100.logMonoTime if v_ego is None: continue if md is not None: last_md_ts = md.logMonoTime # *** get path prediction from the model *** PP.update(v_ego, md) # run kalman filter only if prob is high enough if PP.lead_prob > 0.7: reading = speedSensorV.read(PP.lead_dist, covar=np.matrix(PP.lead_var)) ekfv.update_scalar(reading) ekfv.predict(tsv) # When changing lanes the distance to the lead car can suddenly change, # which makes the Kalman filter output large relative acceleration if mocked and abs(PP.lead_dist - ekfv.state[XV]) > 2.0: ekfv.state[XV] = PP.lead_dist ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init])) ekfv.state[SPEEDV] = 0. ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])), float(ekfv.state[SPEEDV]), False) else: ekfv.state[XV] = PP.lead_dist ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init])) ekfv.state[SPEEDV] = 0. if VISION_POINT in ar_pts: del ar_pts[VISION_POINT] # *** compute the likely path_y *** if (active and not steer_override) or mocked: # use path from model (always when mocking as steering is too noisy) path_y = np.polyval(PP.d_poly, path_x) else: # use path from steer, set angle_offset to 0 it does not only report the physical offset path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0] # *** remove missing points from meta data *** for ids in tracks.keys(): if ids not in ar_pts: tracks.pop(ids, None) # *** compute the tracks *** for ids in ar_pts: # ignore standalone vision point, unless we are mocking the radar if ids == VISION_POINT and not mocked: continue rpt = ar_pts[ids] # align v_ego by a fixed time to align it with the radar measurement cur_time = float(rk.frame) / rate v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_array[1], v_ego_array[0]) d_path = np.sqrt( np.amin((path_x - rpt[0])**2 + (path_y - rpt[1])**2)) # add sign d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y)) # create the track if it doesn't exist or it's a new track if ids not in tracks: tracks[ids] = Track() tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override) # allow the vision model to remove the stationary flag if distance and rel speed roughly match if VISION_POINT in ar_pts: fused_id = None best_score = NO_FUSION_SCORE for ids in tracks: dist_to_vision = np.sqrt( (0.5 * (ar_pts[VISION_POINT][0] - tracks[ids].dRel))**2 + (2 * (ar_pts[VISION_POINT][1] - tracks[ids].yRel))**2) rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel) tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff) if best_score > tracks[ids].vision_score: fused_id = ids best_score = tracks[ids].vision_score if fused_id is not None: tracks[fused_id].vision_cnt += 1 tracks[fused_id].update_vision_fusion() if DEBUG: print("NEW CYCLE") if VISION_POINT in ar_pts: print("vision", ar_pts[VISION_POINT]) idens = tracks.keys() track_pts = np.array( [tracks[iden].get_key_for_cluster() for iden in idens]) # If we have multiple points, cluster them if len(track_pts) > 1: link = linkage_vector(track_pts, method='centroid') cluster_idxs = fcluster(link, 2.5, criterion='distance') clusters = [None] * max(cluster_idxs) for idx in xrange(len(track_pts)): cluster_i = cluster_idxs[idx] - 1 if clusters[cluster_i] == None: clusters[cluster_i] = Cluster() clusters[cluster_i].add(tracks[idens[idx]]) elif len(track_pts) == 1: # TODO: why do we need this? clusters = [Cluster()] clusters[0].add(tracks[idens[0]]) else: clusters = [] if DEBUG: for i in clusters: print(i) # *** extract the lead car *** lead_clusters = [c for c in clusters if c.is_potential_lead(v_ego)] lead_clusters.sort(key=lambda x: x.dRel) lead_len = len(lead_clusters) # *** extract the second lead from the whole set of leads *** lead2_clusters = [ c for c in lead_clusters if c.is_potential_lead2(lead_clusters) ] lead2_clusters.sort(key=lambda x: x.dRel) lead2_len = len(lead2_clusters) # *** publish live20 *** dat = messaging.new_message() dat.init('live20') dat.live20.mdMonoTime = last_md_ts dat.live20.canMonoTimes = list(rr.canMonoTimes) dat.live20.radarErrors = list(rr.errors) dat.live20.l100MonoTime = last_l100_ts if lead_len > 0: dat.live20.leadOne = lead_clusters[0].toLive20() if lead2_len > 0: dat.live20.leadTwo = lead2_clusters[0].toLive20() else: dat.live20.leadTwo.status = False else: dat.live20.leadOne.status = False dat.live20.cumLagMs = -rk.remaining * 1000. live20.send(dat.to_bytes()) # *** publish tracks for UI debugging (keep last) *** dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(tracks.keys()): if DEBUG: print("id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \ (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel, tracks[ids].dPath, tracks[ids].vLat, tracks[ids].vLead, tracks[ids].vLeadK, tracks[ids].aLeadK, tracks[ids].stationary, tracks[ids].measured)) dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), "aRel": float(tracks[ids].aRel), "stationary": bool(tracks[ids].stationary), "oncoming": bool(tracks[ids].oncoming), } liveTracks.send(dat.to_bytes()) rk.monitor_time()
def update(self, CS, CP, VM, LaC, LoC, v_cruise_kph, force_slow_decel): cur_time = sec_since_boot() v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS md = None l20 = None gps_planner_plan = None for socket, event in self.poller.poll(0): if socket is self.model: md = messaging.recv_one(socket) elif socket is self.live20: l20 = messaging.recv_one(socket) elif socket is self.gps_planner_plan: gps_planner_plan = messaging.recv_one(socket) elif socket is self.live_map_data: self.last_live_map_data = messaging.recv_one(socket).liveMapData if gps_planner_plan is not None: self.last_gps_planner_plan = gps_planner_plan if md is not None: self.last_md_ts = md.logMonoTime self.last_model = cur_time self.model_dead = False self.PP.update(CS.vEgo, md) if self.last_gps_planner_plan is not None: plan = self.last_gps_planner_plan.gpsPlannerPlan self.gps_planner_active = plan.valid if plan.valid: self.PP.d_poly = plan.poly self.PP.p_poly = plan.poly self.PP.c_poly = plan.poly self.PP.l_prob = 0.0 self.PP.r_prob = 0.0 self.PP.c_prob = 1.0 if l20 is not None: self.perception_state = copy(l20.live20) self.last_l20_ts = l20.logMonoTime self.last_l20 = cur_time self.radar_dead = False self.radar_errors = list(l20.live20.radarErrors) self.v_acc_start = self.v_acc_sol self.a_acc_start = self.a_acc_sol self.acc_start_time = cur_time self.lead_1 = l20.live20.leadOne self.lead_2 = l20.live20.leadTwo enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping) following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0 if self.last_live_map_data: self.v_speedlimit = NO_CURVATURE_SPEED # Speed limit if self.last_live_map_data.speedLimitValid: speed_limit = self.last_live_map_data.speedLimit set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None if set_speed_limit_active: offset = float(self.params.get("SpeedLimitOffset")) self.v_speedlimit = speed_limit + offset v_cruise_setpoint = min([v_cruise_setpoint, self.v_speedlimit]) # Calculate speed for normal cruise control if enabled: accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following)) # TODO: make a separate lookup for jerk tuning jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], _DT_MPC) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = LoC.long_control_state == LongCtrlState.starting a_ego = min(CS.aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else CS.vEgo reset_accel = self.CP.startAccel if starting else a_ego self.v_acc = reset_speed self.a_acc = reset_accel self.v_acc_start = reset_speed self.a_acc_start = reset_accel self.v_cruise = reset_speed self.a_cruise = reset_accel self.v_acc_sol = reset_speed self.a_acc_sol = reset_accel self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(CS, self.lead_1, v_cruise_setpoint) self.mpc2.update(CS, self.lead_2, v_cruise_setpoint) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = CS.leftBlinker or CS.rightBlinker self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo, self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK, self.lead_1.yRel, self.lead_1.vLat, self.lead_1.fcw, blinkers) \ and not CS.brakePressed if self.fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) if cur_time - self.last_model > 0.5: self.model_dead = True if cur_time - self.last_l20 > 0.5: self.radar_dead = True # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') events = [] if self.model_dead: events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.radar_dead or 'commIssue' in self.radar_errors: events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if 'fault' in self.radar_errors: events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if LaC.mpc_solution[0].cost > 10000. or LaC.mpc_nans: # TODO: find a better way to detect when MPC did not converge events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # Interpolation of trajectory dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0 plan_send.plan.events = events plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts # lateral plan plan_send.plan.lateralValid = not self.model_dead plan_send.plan.dPoly = map(float, self.PP.d_poly) plan_send.plan.laneWidth = float(self.PP.lane_width) # longitudal plan plan_send.plan.longitudinalValid = not self.radar_dead plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vTarget = self.v_acc_sol plan_send.plan.aTarget = self.a_acc_sol plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.gpsPlannerActive = self.gps_planner_active # Send out fcw fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) return plan_send
def manager_thread(): global baseui_running # now loop context = zmq.Context() thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) health_sock = messaging.sub_sock(context, service_list['health'].port) cloudlog.info("manager start") cloudlog.info(dict(os.environ)) start_managed_process("logmessaged") start_managed_process("logcatd") start_managed_process("tombstoned") start_managed_process("uploader") start_managed_process("ui") manage_baseui(True) panda = False if os.getenv("NOBOARD") is None: # *** wait for the board *** panda = wait_for_device() == 0x2300 # flash the device if os.getenv("NOPROG") is None: # checkout the matching panda repo rootdir = os.path.dirname(os.path.abspath(__file__)) ret = os.system("cd %s && git submodule init && git submodule update" % rootdir) cloudlog.info("git submodule update panda returned %d" % ret) # flash the board boarddir = os.path.dirname( os.path.abspath(__file__)) + "/../panda/board/" mkfile = "Makefile" if panda else "Makefile.legacy" print "using", mkfile ret = os.system("cd %s && make -f %s" % (boarddir, mkfile)) cloudlog.info("flash board returned %d" % ret) start_managed_process("boardd") started = False logger_dead = False count = 0 # set 5 second timeout on health socket # 5x slower than expected health_sock.RCVTIMEO = 5000 while 1: # get health of board, log this in "thermal" td = messaging.recv_sock(health_sock, wait=True) print td # replace thermald msg = read_thermal() # loggerd is gated based on free space statvfs = os.statvfs(ROOT) avail = (statvfs.f_bavail * 1.0) / statvfs.f_blocks # thermal message now also includes free space msg.thermal.freeSpace = avail with open("/sys/class/power_supply/battery/capacity") as f: msg.thermal.batteryPercent = int(f.read()) with open("/sys/class/power_supply/battery/status") as f: msg.thermal.batteryStatus = f.read().strip() thermal_sock.send(msg.to_bytes()) print msg # TODO: add car battery voltage check max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 # uploader is gated based on the phone temperature if max_temp > 85.0: cloudlog.info("over temp: %r", max_temp) kill_managed_process("uploader") elif max_temp < 70.0: start_managed_process("uploader") if avail < 0.05: logger_dead = True # start constellation of processes when the car starts # with 2% left, we killall, otherwise the phone is bricked if td is not None and td.health.started and avail > 0.02: if not started: Params().car_start() started = True for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) manage_baseui(False) else: manage_baseui(True) started = False logger_dead = False for p in car_started_processes: kill_managed_process(p) # shutdown if the battery gets lower than 10%, we aren't running, and we are discharging if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging": os.system('LD_LIBRARY_PATH="" svc power shutdown') # check the status of baseui baseui_running = 'com.baseui' in subprocess.check_output(["ps"]) # check the status of all processes, did any of them die? for p in running: cloudlog.debug(" running %s %s" % (p, running[p])) # report to server once per minute if (count % 60) == 0: cloudlog.event("STATUS_PACKET", running=running.keys(), count=count, health=(td.to_dict() if td else None), thermal=msg.to_dict()) count += 1
def thermald_thread(): # prevent LEECO from undervoltage BATT_PERC_OFF = 10 if LEON else 3 health_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected health frequency # now loop thermal_sock = messaging.pub_sock('thermal') health_sock = messaging.sub_sock('health', timeout=health_timeout) location_sock = messaging.sub_sock('gpsLocation') ignition = False fan_speed = 0 count = 0 off_ts = None started_ts = None started_seen = False thermal_status = ThermalStatus.green thermal_status_prev = ThermalStatus.green usb_power = True usb_power_prev = True network_type = NetworkType.none network_strength = NetworkStrength.unknown wifiIpAddress = '--' current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML) health_prev = None fw_version_match_prev = True current_connectivity_alert = None time_valid_prev = True should_start_prev = False handle_fan = None is_uno = False has_relay = False params = Params() pm = PowerMonitoring() no_panda_cnt = 0 while 1: health = messaging.recv_sock(health_sock, wait=True) location = messaging.recv_sock(location_sock) location = location.gpsLocation if location else None msg = read_thermal() if health is not None: usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client # If we lose connection to the panda, wait 5 seconds before going offroad if health.health.hwType == log.HealthData.HwType.unknown: no_panda_cnt += 1 if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: if ignition: cloudlog.error("Lost panda connection while onroad") ignition = False else: no_panda_cnt = 0 ignition = health.health.ignitionLine or health.health.ignitionCan # Setup fan handler on first connect to panda if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown: is_uno = health.health.hwType == log.HealthData.HwType.uno has_relay = health.health.hwType in [ log.HealthData.HwType.blackPanda, log.HealthData.HwType.uno, log.HealthData.HwType.dos ] if is_uno or not ANDROID: cloudlog.info("Setting up UNO fan handler") handle_fan = handle_fan_uno else: cloudlog.info("Setting up EON fan handler") setup_eon_fan() handle_fan = handle_fan_eon # Handle disconnect if health_prev is not None: if health.health.hwType == log.HealthData.HwType.unknown and \ health_prev.health.hwType != log.HealthData.HwType.unknown: params.panda_disconnect() health_prev = health # get_network_type is an expensive call. update every 10s if (count % int(10. / DT_TRML)) == 0: try: network_type = get_network_type() network_strength = get_network_strength(network_type) wifiIpAddress = get_ip_address() except Exception: cloudlog.exception("Error getting network status") msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0 msg.thermal.memUsedPercent = int(round( psutil.virtual_memory().percent)) msg.thermal.cpuPerc = int(round(psutil.cpu_percent())) msg.thermal.networkType = network_type msg.thermal.networkStrength = network_strength msg.thermal.wifiIpAddress = wifiIpAddress msg.thermal.batteryPercent = get_battery_capacity() msg.thermal.batteryStatus = get_battery_status() msg.thermal.batteryCurrent = get_battery_current() msg.thermal.batteryVoltage = get_battery_voltage() msg.thermal.usbOnline = get_usb_present() # Fake battery levels on uno for frame if is_uno: msg.thermal.batteryPercent = 100 msg.thermal.batteryStatus = "Charging" current_filter.update(msg.thermal.batteryCurrent / 1e6) # TODO: add car battery voltage check max_cpu_temp = cpu_temp_filter.update( max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0) max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.) bat_temp = msg.thermal.bat / 1000. if handle_fan is not None: fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, ignition) msg.thermal.fanSpeed = fan_speed # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 # We only do this if there is a relay that prevents the car from faulting if max_cpu_temp > 107. or bat_temp >= 63. or (has_relay and (started_ts is None) and max_cpu_temp > 70.0): # onroad not allowed thermal_status = ThermalStatus.danger elif max_comp_temp > 96.0 or bat_temp > 60.: # hysteresis between onroad not allowed and engage not allowed thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger) elif max_cpu_temp > 94.0: # hysteresis between engage not allowed and uploader not allowed thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red) elif max_cpu_temp > 80.0: # uploader not allowed thermal_status = ThermalStatus.yellow elif max_cpu_temp > 75.0: # hysteresis between uploader not allowed and all good thermal_status = clip(thermal_status, ThermalStatus.green, ThermalStatus.yellow) else: # all good thermal_status = ThermalStatus.green # **** starting logic **** # Check for last update time and display alerts if needed now = datetime.datetime.utcnow() # show invalid date/time alert time_valid = now.year >= 2019 if time_valid and not time_valid_prev: params.delete("Offroad_InvalidTime") if not time_valid and time_valid_prev: put_nonblocking("Offroad_InvalidTime", json.dumps(OFFROAD_ALERTS["Offroad_InvalidTime"])) time_valid_prev = time_valid # Show update prompt try: last_update = datetime.datetime.fromisoformat( params.get("LastUpdateTime", encoding='utf8')) except (TypeError, ValueError): last_update = now dt = now - last_update update_failed_count = params.get("UpdateFailedCount") update_failed_count = 0 if update_failed_count is None else int( update_failed_count) if dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1: if current_connectivity_alert != "expired": current_connectivity_alert = "expired" params.delete("Offroad_ConnectivityNeededPrompt") put_nonblocking( "Offroad_ConnectivityNeeded", json.dumps(OFFROAD_ALERTS["Offroad_ConnectivityNeeded"])) elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0)) if current_connectivity_alert != "prompt" + remaining_time: current_connectivity_alert = "prompt" + remaining_time alert_connectivity_prompt = copy.copy( OFFROAD_ALERTS["Offroad_ConnectivityNeededPrompt"]) alert_connectivity_prompt["text"] += remaining_time + " days." params.delete("Offroad_ConnectivityNeeded") put_nonblocking("Offroad_ConnectivityNeededPrompt", json.dumps(alert_connectivity_prompt)) elif current_connectivity_alert is not None: current_connectivity_alert = None params.delete("Offroad_ConnectivityNeeded") params.delete("Offroad_ConnectivityNeededPrompt") do_uninstall = params.get("DoUninstall") == b"1" accepted_terms = params.get("HasAcceptedTerms") == terms_version completed_training = params.get( "CompletedTrainingVersion") == training_version panda_signature = params.get("PandaFirmware") fw_version_match = (panda_signature is None) or ( panda_signature == FW_SIGNATURE ) # don't show alert is no panda is connected (None) should_start = ignition # with 2% left, we killall, otherwise the phone will take a long time to boot should_start = should_start and msg.thermal.freeSpace > 0.02 # confirm we have completed training and aren't uninstalling should_start = should_start and accepted_terms and completed_training and ( not do_uninstall) # check for firmware mismatch should_start = should_start and fw_version_match # check if system time is valid should_start = should_start and time_valid # don't start while taking snapshot if not should_start_prev: is_viewing_driver = params.get("IsDriverViewEnabled") == b"1" is_taking_snapshot = params.get("IsTakingSnapshot") == b"1" should_start = should_start and (not is_taking_snapshot) and ( not is_viewing_driver) if fw_version_match and not fw_version_match_prev: params.delete("Offroad_PandaFirmwareMismatch") if not fw_version_match and fw_version_match_prev: put_nonblocking( "Offroad_PandaFirmwareMismatch", json.dumps(OFFROAD_ALERTS["Offroad_PandaFirmwareMismatch"])) # 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 if thermal_status >= ThermalStatus.danger: should_start = False if thermal_status_prev < ThermalStatus.danger: put_nonblocking( "Offroad_TemperatureTooHigh", json.dumps(OFFROAD_ALERTS["Offroad_TemperatureTooHigh"])) else: if thermal_status_prev >= ThermalStatus.danger: params.delete("Offroad_TemperatureTooHigh") if should_start: if not should_start_prev: params.delete("IsOffroad") off_ts = None if started_ts is None: started_ts = sec_since_boot() started_seen = True os.system( 'echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor' ) else: if should_start_prev or (count == 0): put_nonblocking("IsOffroad", "1") started_ts = None if off_ts is None: off_ts = sec_since_boot() os.system( 'echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor' ) # shutdown if the battery gets lower than 3%, it's discharging, we aren't running for # more than a minute but we were running if msg.thermal.batteryPercent < BATT_PERC_OFF and msg.thermal.batteryStatus == "Discharging" and \ started_seen and (sec_since_boot() - off_ts) > 60: os.system('LD_LIBRARY_PATH="" svc power shutdown') # Offroad power monitoring pm.calculate(health) msg.thermal.offroadPowerUsage = pm.get_power_used() msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged msg.thermal.started = started_ts is not None msg.thermal.startedTs = int(1e9 * (started_ts or 0)) msg.thermal.thermalStatus = thermal_status thermal_sock.send(msg.to_bytes()) if usb_power_prev and not usb_power: put_nonblocking( "Offroad_ChargeDisabled", json.dumps(OFFROAD_ALERTS["Offroad_ChargeDisabled"])) elif usb_power and not usb_power_prev: params.delete("Offroad_ChargeDisabled") thermal_status_prev = thermal_status usb_power_prev = usb_power fw_version_match_prev = fw_version_match should_start_prev = should_start # report to server once per minute if (count % int(60. / DT_TRML)) == 0: cloudlog.event("STATUS_PACKET", count=count, health=(health.to_dict() if health else None), location=(location.to_dict() if location else None), thermal=msg.to_dict()) count += 1
def manager_update(): if os.path.exists(os.path.join(BASEDIR, "vpn")): cloudlog.info("installing vpn") os.system(os.path.join(BASEDIR, "vpn", "install.sh")) update_apks()
def manager_thread(): Process(name="shutdownd", target=launcher, args=("selfdrive.shutdownd", )).start() update_apks() os.chmod(BASEDIR, 0o755) os.chmod("/dev/shm", 0o777) os.chmod(os.path.join(BASEDIR, "cereal"), 0o755) os.chmod(os.path.join(BASEDIR, "cereal", "libmessaging_shared.so"), 0o755) pm_grant("com.neokii.openpilot", "android.permission.ACCESS_FINE_LOCATION") appops_set("com.neokii.optool", "SU", "allow") system("am startservice com.neokii.optool/.MainService") system("am startservice com.neokii.openpilot/.MainService") cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log #subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) ignore = [] if os.getenv("NOBOARD") is not None: ignore.append("pandad") if os.getenv("BLOCK") is not None: ignore += os.getenv("BLOCK").split(",") ensure_running(managed_processes.values(), started=False, not_run=ignore) started_prev = False params = Params() sm = messaging.SubMaster(['deviceState']) pm = messaging.PubMaster(['managerState']) while True: sm.update() not_run = ignore[:] if sm['deviceState'].freeSpacePercent < 5: not_run.append("loggerd") started = sm['deviceState'].started driverview = params.get_bool("IsDriverViewEnabled") ensure_running(managed_processes.values(), started, driverview, not_run) # trigger an update after going offroad if started_prev and not started and 'updated' in managed_processes: os.sync() managed_processes['updated'].signal(signal.SIGHUP) started_prev = started running_list = [ "%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 ] cloudlog.debug(' '.join(running_list)) # 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) # TODO: let UI handle this # Exit main loop when uninstall is needed if params.get_bool("DoUninstall"): break
def fetch_update(wait_helper: WaitTimeHelper) -> bool: cloudlog.info("attempting git fetch inside staging overlay") setup_git_options(OVERLAY_MERGED) git_fetch_output = run(["git", "fetch"], OVERLAY_MERGED, low_priority=True) cloudlog.info("git fetch success: %s", git_fetch_output) cur_hash = run(["git", "rev-parse", "HEAD"], OVERLAY_MERGED).rstrip() upstream_hash = run(["git", "rev-parse", "@{u}"], OVERLAY_MERGED).rstrip() new_version = cur_hash != upstream_hash git_fetch_result = check_git_fetch_result(git_fetch_output) cloudlog.info("comparing %s to %s" % (cur_hash, upstream_hash)) if new_version or git_fetch_result: cloudlog.info("Running update") if new_version: cloudlog.info("git reset in progress") r = [ run(["git", "reset", "--hard", "@{u}"], OVERLAY_MERGED, low_priority=True), run(["git", "clean", "-xdf"], OVERLAY_MERGED, low_priority=True), run(["git", "submodule", "init"], OVERLAY_MERGED, low_priority=True), run(["git", "submodule", "update"], OVERLAY_MERGED, low_priority=True), ] cloudlog.info("git reset success: %s", '\n'.join(r)) if EON: handle_neos_update(wait_helper) elif TICI: handle_agnos_update(wait_helper) # Create the finalized, ready-to-swap update finalize_update() cloudlog.info("openpilot update successful!") else: cloudlog.info("nothing new from git at this time") return new_version
def main(): params = Params() if params.get_bool("DisableUpdates"): raise RuntimeError("updates are disabled by the DisableUpdates param") if EON and os.geteuid() != 0: raise RuntimeError("updated must be launched as root!") # Set low io priority proc = psutil.Process() if psutil.LINUX: proc.ionice(psutil.IOPRIO_CLASS_BE, value=7) ov_lock_fd = open(LOCK_FILE, 'w') try: fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB) except IOError as e: raise RuntimeError( "couldn't get overlay lock; is another updated running?") from e # Wait for IsOffroad to be set before our first update attempt wait_helper = WaitTimeHelper(proc) wait_helper.sleep(30) overlay_init = Path(os.path.join(BASEDIR, ".overlay_init")) overlay_init.unlink(missing_ok=True) first_run = True last_fetch_time = 0 update_failed_count = 0 # Run the update loop # * every 1m, do a lightweight internet/update check # * every 10m, do a full git fetch while not wait_helper.shutdown: update_now = wait_helper.ready_event.is_set() wait_helper.ready_event.clear() # Don't run updater while onroad or if the time's wrong time_wrong = datetime.datetime.utcnow().year < 2019 is_onroad = not params.get_bool("IsOffroad") if is_onroad or time_wrong: wait_helper.sleep(30) cloudlog.info("not running updater, not offroad") continue # Attempt an update exception = None new_version = False update_failed_count += 1 try: init_overlay() internet_ok, update_available = check_for_update() if internet_ok and not update_available: update_failed_count = 0 # Fetch updates at most every 10 minutes if internet_ok and (update_now or time.monotonic() - last_fetch_time > 60 * 10): new_version = fetch_update(wait_helper) update_failed_count = 0 last_fetch_time = time.monotonic() if first_run and not new_version and os.path.isdir( NEOSUPDATE_DIR): shutil.rmtree(NEOSUPDATE_DIR) first_run = False 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) set_params(new_version, update_failed_count, exception) wait_helper.sleep(60) dismount_overlay()
def manager_thread(): cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) # start daemon processes for p in daemon_processes: start_daemon_process(p) # start persistent processes for p in persistent_processes: start_managed_process(p) # start offroad if EON: pm_apply_packages('enable') start_offroad() if os.getenv("NOBOARD") is not None: del managed_processes["pandad"] if os.getenv("BLOCK") is not None: for k in os.getenv("BLOCK").split(","): del managed_processes[k] started_prev = False logger_dead = False params = Params() device_state_sock = messaging.sub_sock('deviceState') pm = messaging.PubMaster(['managerState']) while 1: msg = messaging.recv_sock(device_state_sock, wait=True) if msg.deviceState.freeSpacePercent < 5: logger_dead = True if msg.deviceState.started: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: logger_dead = False driver_view = params.get("IsDriverViewEnabled") == b"1" # TODO: refactor how manager manages processes for p in reversed(car_started_processes): if p not in driver_view_processes or not driver_view: kill_managed_process(p) for p in driver_view_processes: if driver_view: start_managed_process(p) else: kill_managed_process(p) # trigger an update after going offroad if started_prev: os.sync() send_managed_process_signal("updated", signal.SIGHUP) started_prev = msg.deviceState.started # check the status of all processes, did any of them die? running_list = [ "%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running ] cloudlog.debug(' '.join(running_list)) # send managerState states = [] for p in managed_processes: state = log.ManagerState.ProcessState.new_message() state.name = p if p in running: state.running = running[p].is_alive() state.pid = running[p].pid state.exitCode = running[p].exitcode or 0 states.append(state) msg = messaging.new_message('managerState') msg.managerState.processes = states pm.send('managerState', msg) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break
def init_overlay() -> None: overlay_init_file = Path(os.path.join(BASEDIR, ".overlay_init")) # Re-create the overlay if BASEDIR/.git has changed since we created the overlay if overlay_init_file.is_file(): git_dir_path = os.path.join(BASEDIR, ".git") new_files = run( ["find", git_dir_path, "-newer", str(overlay_init_file)]) if not len(new_files.splitlines()): # A valid overlay already exists return else: cloudlog.info(".git directory changed, recreating overlay") cloudlog.info("preparing new safe staging area") params = Params() params.put_bool("UpdateAvailable", False) set_consistent_flag(False) dismount_overlay() if TICI: run(["sudo", "rm", "-rf", STAGING_ROOT]) if os.path.isdir(STAGING_ROOT): shutil.rmtree(STAGING_ROOT) for dirname in [ STAGING_ROOT, OVERLAY_UPPER, OVERLAY_METADATA, OVERLAY_MERGED ]: os.mkdir(dirname, 0o755) if os.lstat(BASEDIR).st_dev != os.lstat(OVERLAY_MERGED).st_dev: raise RuntimeError( "base and overlay merge directories are on different filesystems; not valid for overlay FS!" ) # Leave a timestamped canary in BASEDIR to check at startup. The device clock # should be correct by the time we get here. If the init file disappears, or # critical mtimes in BASEDIR are newer than .overlay_init, continue.sh can # assume that BASEDIR has used for local development or otherwise modified, # and skips the update activation attempt. consistent_file = Path(os.path.join(BASEDIR, ".overlay_consistent")) if consistent_file.is_file(): consistent_file.unlink() overlay_init_file.touch() os.sync() overlay_opts = f"lowerdir={BASEDIR},upperdir={OVERLAY_UPPER},workdir={OVERLAY_METADATA}" mount_cmd = [ "mount", "-t", "overlay", "-o", overlay_opts, "none", OVERLAY_MERGED ] if TICI: run(["sudo"] + mount_cmd) run(["sudo", "chmod", "755", os.path.join(OVERLAY_METADATA, "work")]) else: run(mount_cmd) git_diff = run(["git", "diff"], OVERLAY_MERGED, low_priority=True) params.put("GitDiff", git_diff) cloudlog.info(f"git diff output:\n{git_diff}")
def manager_thread(): # now loop context = zmq.Context() thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) health_sock = messaging.sub_sock(context, service_list['health'].port) version = open( os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "version.h")).read().split('"')[1] cloudlog.info("manager start %s" % version) cloudlog.info(dict(os.environ)) start_managed_process("logmessaged") start_managed_process("logcatd") start_managed_process("uploader") start_managed_process("ui") if os.getenv("NOBOARD") is None: # *** wait for the board *** wait_for_device() # flash the device if os.getenv("NOPROG") is None: boarddir = os.path.dirname(os.path.abspath(__file__)) + "/../board/" os.system("cd %s && make" % boarddir) start_managed_process("boardd") if os.getenv("STARTALL") is not None: for p in car_started_processes: start_managed_process(p) # logger_dead = False logger_dead = True count = 0 # set 5 second timeout on health socket # 5x slower than expected health_sock.RCVTIMEO = 5000 while 1: # get health of board, log this in "thermal" td = messaging.recv_sock(health_sock, wait=True) print td # replace thermald msg = read_thermal() # loggerd is gated based on free space statvfs = os.statvfs(ROOT) avail = (statvfs.f_bavail * 1.0) / statvfs.f_blocks # thermal message now also includes free space msg.thermal.freeSpace = avail with open("/sys/class/power_supply/battery/capacity") as f: msg.thermal.batteryPercent = int(f.read()) with open("/sys/class/power_supply/battery/status") as f: msg.thermal.batteryStatus = f.read().strip() thermal_sock.send(msg.to_bytes()) print msg # TODO: add car battery voltage check max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 # uploader is gated based on the phone temperature if max_temp > 85.0: cloudlog.info("over temp: %r", max_temp) kill_managed_process("uploader") elif max_temp < 70.0: start_managed_process("uploader") # if avail < 0.05: # logger_dead = True # start constellation of processes when the car starts if not os.getenv("STARTALL"): # with 2% left, we killall, otherwise the phone is bricked if td is not None and td.health.started and avail > 0.02: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: logger_dead = False for p in car_started_processes: kill_managed_process(p) # shutdown if the battery gets lower than 10%, we aren't running, and we are discharging if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging": os.system('LD_LIBRARY_PATH="" svc power shutdown') # check the status of all processes, did any of them die? for p in running: cloudlog.debug(" running %s %s" % (p, running[p])) # report to server once per minute if (count % 60) == 0: cloudlog.event("STATUS_PACKET", running=running.keys(), count=count, health=(td.to_dict() if td else None), thermal=msg.to_dict(), version=version) count += 1
def manager_thread(): cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) params = Params() ignore = [] if params.get("DongleId", encoding='utf8') == UNREGISTERED_DONGLE_ID: ignore += ["manage_athenad", "uploader"] if os.getenv("NOBOARD") is not None: ignore.append("pandad") if os.getenv("BLOCK") is not None: ignore += os.getenv("BLOCK").split(",") ensure_running(managed_processes.values(), started=False, not_run=ignore) started_prev = False sm = messaging.SubMaster(['deviceState']) pm = messaging.PubMaster(['managerState']) while True: sm.update() not_run = ignore[:] if sm['deviceState'].freeSpacePercent < 5: not_run.append("loggerd") started = sm['deviceState'].started driverview = params.get_bool("IsDriverViewEnabled") ensure_running(managed_processes.values(), started, driverview, not_run) # trigger an update after going offroad if started_prev and not started and 'updated' in managed_processes: os.sync() managed_processes['updated'].signal(signal.SIGHUP) started_prev = started running_list = [ "%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 ] cloudlog.debug(' '.join(running_list)) # 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) # TODO: let UI handle this # Exit main loop when uninstall is needed if params.get_bool("DoUninstall"): break
def thermald_thread(): health_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected health frequency # now loop thermal_sock = messaging.pub_sock('thermal') health_sock = messaging.sub_sock('health', timeout=health_timeout) location_sock = messaging.sub_sock('gpsLocation') fan_speed = 0 count = 0 startup_conditions = { "ignition": False, } startup_conditions_prev = startup_conditions.copy() off_ts = None started_ts = None started_seen = False thermal_status = ThermalStatus.green usb_power = True current_branch = get_git_branch() network_type = NetworkType.none network_strength = NetworkStrength.unknown current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML) health_prev = None should_start_prev = False handle_fan = None is_uno = False has_relay = False params = Params() pm = PowerMonitoring() no_panda_cnt = 0 thermal_config = get_thermal_config() while 1: health = messaging.recv_sock(health_sock, wait=True) location = messaging.recv_sock(location_sock) location = location.gpsLocation if location else None msg = read_thermal(thermal_config) if health is not None: usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client # If we lose connection to the panda, wait 5 seconds before going offroad if health.health.hwType == log.HealthData.HwType.unknown: no_panda_cnt += 1 if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: if startup_conditions["ignition"]: cloudlog.error("Lost panda connection while onroad") startup_conditions["ignition"] = False else: no_panda_cnt = 0 startup_conditions[ "ignition"] = health.health.ignitionLine or health.health.ignitionCan # Setup fan handler on first connect to panda if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown: is_uno = health.health.hwType == log.HealthData.HwType.uno has_relay = health.health.hwType in [ log.HealthData.HwType.blackPanda, log.HealthData.HwType.uno, log.HealthData.HwType.dos ] if (not EON) or is_uno: cloudlog.info("Setting up UNO fan handler") handle_fan = handle_fan_uno else: cloudlog.info("Setting up EON fan handler") setup_eon_fan() handle_fan = handle_fan_eon # Handle disconnect if health_prev is not None: if health.health.hwType == log.HealthData.HwType.unknown and \ health_prev.health.hwType != log.HealthData.HwType.unknown: params.panda_disconnect() health_prev = health # get_network_type is an expensive call. update every 10s if (count % int(10. / DT_TRML)) == 0: try: network_type = HARDWARE.get_network_type() network_strength = HARDWARE.get_network_strength(network_type) except Exception: cloudlog.exception("Error getting network status") msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0 msg.thermal.memUsedPercent = int(round( psutil.virtual_memory().percent)) msg.thermal.cpuPerc = int(round(psutil.cpu_percent())) msg.thermal.networkType = network_type msg.thermal.networkStrength = network_strength msg.thermal.batteryPercent = get_battery_capacity() msg.thermal.batteryStatus = get_battery_status() msg.thermal.batteryCurrent = get_battery_current() msg.thermal.batteryVoltage = get_battery_voltage() msg.thermal.usbOnline = get_usb_present() # Fake battery levels on uno for frame if (not EON) or is_uno: msg.thermal.batteryPercent = 100 msg.thermal.batteryStatus = "Charging" msg.thermal.bat = 0 current_filter.update(msg.thermal.batteryCurrent / 1e6) # TODO: add car battery voltage check max_cpu_temp = cpu_temp_filter.update(max(msg.thermal.cpu)) max_comp_temp = max(max_cpu_temp, msg.thermal.mem, max(msg.thermal.gpu)) bat_temp = msg.thermal.bat if handle_fan is not None: fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, startup_conditions["ignition"]) msg.thermal.fanSpeed = fan_speed # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 # We only do this if there is a relay that prevents the car from faulting 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 max_cpu_temp > 107. or bat_temp >= 63. or (has_relay and is_offroad_for_5_min and max_cpu_temp > 70.0): # onroad not allowed thermal_status = ThermalStatus.danger elif max_comp_temp > 96.0 or bat_temp > 60.: # hysteresis between onroad not allowed and engage not allowed thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger) elif max_cpu_temp > 94.0: # hysteresis between engage not allowed and uploader not allowed thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red) elif max_cpu_temp > 80.0: # uploader not allowed thermal_status = ThermalStatus.yellow elif max_cpu_temp > 75.0: # hysteresis between uploader not allowed and all good thermal_status = clip(thermal_status, ThermalStatus.green, ThermalStatus.yellow) else: # all good thermal_status = ThermalStatus.green # **** starting logic **** # Check for last update time and display alerts if needed now = datetime.datetime.utcnow() # show invalid date/time alert startup_conditions["time_valid"] = now.year >= 2019 set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"])) # Show update prompt try: last_update = datetime.datetime.fromisoformat( params.get("LastUpdateTime", encoding='utf8')) except (TypeError, ValueError): last_update = now dt = now - last_update update_failed_count = params.get("UpdateFailedCount") update_failed_count = 0 if update_failed_count is None else int( update_failed_count) last_update_exception = params.get("LastUpdateException", encoding='utf8') if update_failed_count > 15 and last_update_exception is not None: if current_branch in ["release2", "dashcam"]: extra_text = "Ensure the software is correctly installed" else: extra_text = last_update_exception set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text) elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1: set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True) elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0)) set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.") else: set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) startup_conditions["not_uninstalling"] = not params.get( "DoUninstall") == b"1" startup_conditions["accepted_terms"] = params.get( "HasAcceptedTerms") == terms_version completed_training = params.get( "CompletedTrainingVersion") == training_version panda_signature = params.get("PandaFirmware") startup_conditions["fw_version_match"] = (panda_signature is None) or ( panda_signature == FW_SIGNATURE ) # don't show alert is no panda is connected (None) set_offroad_alert_if_changed( "Offroad_PandaFirmwareMismatch", (not startup_conditions["fw_version_match"])) # with 2% left, we killall, otherwise the phone will take a long time to boot startup_conditions["free_space"] = msg.thermal.freeSpace > 0.02 startup_conditions["completed_training"] = completed_training or ( current_branch in ['dashcam', 'dashcam-staging']) startup_conditions["not_driver_view"] = not params.get( "IsDriverViewEnabled") == b"1" startup_conditions["not_taking_snapshot"] = not params.get( "IsTakingSnapshot") == b"1" # 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 startup_conditions[ "device_temp_good"] = thermal_status < ThermalStatus.danger set_offroad_alert_if_changed( "Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"])) should_start = all(startup_conditions.values()) startup_conditions[ "hardware_supported"] = True # health is not None and health.health.hwType not in [log.HealthData.HwType.whitePanda, # log.HealthData.HwType.greyPanda] set_offroad_alert_if_changed( "Offroad_HardwareUnsupported", health is not None and not startup_conditions["hardware_supported"]) if should_start: if not should_start_prev: params.delete("IsOffroad") off_ts = None if started_ts is None: started_ts = sec_since_boot() started_seen = True os.system( 'echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor' ) else: if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev): cloudlog.event("Startup blocked", startup_conditions=startup_conditions) if should_start_prev or (count == 0): params.put("IsOffroad", "1") started_ts = None if off_ts is None: off_ts = sec_since_boot() os.system( 'echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor' ) # Offroad power monitoring pm.calculate(health) msg.thermal.offroadPowerUsage = pm.get_power_used() msg.thermal.carBatteryCapacity = max(0, pm.get_car_battery_capacity()) # Check if we need to disable charging (handled by boardd) msg.thermal.chargingDisabled = pm.should_disable_charging( health, off_ts) # Check if we need to shut down if pm.should_shutdown(health, off_ts, started_seen, LEON): cloudlog.info(f"shutting device down, offroad since {off_ts}") # TODO: add function for blocking cloudlog instead of sleep time.sleep(10) os.system('LD_LIBRARY_PATH="" svc power shutdown') msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged msg.thermal.started = started_ts is not None msg.thermal.startedTs = int(1e9 * (started_ts or 0)) msg.thermal.thermalStatus = thermal_status thermal_sock.send(msg.to_bytes()) set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power)) should_start_prev = should_start startup_conditions_prev = startup_conditions.copy() # report to server once per minute if (count % int(60. / DT_TRML)) == 0: cloudlog.event("STATUS_PACKET", count=count, health=(health.to_dict() if health else None), location=(location.to_dict() if location else None), thermal=msg.to_dict()) count += 1
def update(self, sm, pm, CP, VM, PP): """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = sm['carState'].vEgo long_control_state = sm['controlsState'].longControlState v_cruise_kph = sm['controlsState'].vCruise force_slow_decel = sm['controlsState'].forceDecel v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS lead_1 = sm['radarState'].leadOne lead_2 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 # Calculate speed for normal cruise control if enabled and not self.first_loop and not sm['carState'].gasPressed: accel_limits = [ float(x) for x in calc_cruise_accel_limits(v_ego, following) ] jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns( v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1], jerk_limits[0], LON_MPC_STEP) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else v_ego reset_accel = self.CP.startAccel if starting else a_ego self.v_acc = reset_speed self.a_acc = reset_accel self.v_acc_start = reset_speed self.a_acc_start = reset_accel self.v_cruise = reset_speed self.a_cruise = reset_accel self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(pm, sm['carState'], lead_1) self.mpc2.update(pm, sm['carState'], lead_2) distances, speeds, accelerations = self.parse_modelV2_data(sm) self.mpc_model.update(sm['carState'].vEgo, sm['carState'].aEgo, distances, speeds, accelerations) self.choose_solution(v_cruise_setpoint, enabled, sm['modelLongButton'].enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not sm['carState'].brakePressed if fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) radar_dead = not sm.alive['radarState'] radar_errors = list(sm['radarState'].radarErrors) radar_fault = car.RadarData.Error.fault in radar_errors radar_can_error = car.RadarData.Error.canError in radar_errors # **** send the plan **** plan_send = messaging.new_message('plan') plan_send.valid = sm.all_alive_and_valid( service_list=['carState', 'controlsState', 'radarState']) plan_send.plan.mdMonoTime = sm.logMonoTime['model'] plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] # longitudal plan plan_send.plan.vCruise = float(self.v_cruise) plan_send.plan.aCruise = float(self.a_cruise) plan_send.plan.vStart = float(self.v_acc_start) plan_send.plan.aStart = float(self.a_acc_start) plan_send.plan.vTarget = float(self.v_acc) plan_send.plan.aTarget = float(self.a_acc) plan_send.plan.vTargetFuture = float(self.v_acc_future) plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCanError = bool(radar_can_error) plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] # Send out fcw plan_send.plan.fcw = fcw pm.send('plan', plan_send) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * ( self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + CP.radarTimeStep * ( a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol self.first_loop = False
def update_now(self, signum: int, frame) -> None: cloudlog.info("caught SIGHUP, running update check immediately") self.ready_event.set()
def flash_panda(panda_serial: str) -> Panda: panda = Panda(panda_serial) fw_signature = get_expected_signature(panda) panda_version = "bootstub" if panda.bootstub else panda.get_version() panda_signature = b"" if panda.bootstub else panda.get_signature() cloudlog.warning( "Panda %s connected, version: %s, signature %s, expected %s" % ( panda_serial, panda_version, panda_signature.hex()[:16], 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}" ) panda.recover() 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 manager_thread(): # now loop thermal_sock = messaging.sub_sock('thermal') cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) params = Params() # start daemon processes for p in daemon_processes: start_daemon_process(p) # start persistent processes for p in persistent_processes: start_managed_process(p) # start frame pm_apply_packages('enable') start_frame() if os.getenv("NOBOARD") is None: start_managed_process("pandad") logger_dead = False while 1: msg = messaging.recv_sock(thermal_sock, wait=True) # uploader is gated based on the phone temperature if msg.thermal.thermalStatus >= ThermalStatus.yellow: kill_managed_process("uploader") else: start_managed_process("uploader") if msg.thermal.freeSpace < 0.05: logger_dead = True if msg.thermal.started: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: logger_dead = False for p in reversed(car_started_processes): kill_managed_process(p) # check the status of all processes, did any of them die? running_list = [ "%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running ] cloudlog.debug(' '.join(running_list)) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break
def thermald_thread(end_event, hw_queue): pm = messaging.PubMaster(['deviceState']) sm = messaging.SubMaster([ "peripheralState", "gpsLocationExternal", "controlsState", "pandaStates" ], poll=["pandaStates"]) fan_speed = 0 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 usb_power = True last_hw_state = HardwareState( network_type=NetworkType.none, 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 handle_fan = None is_uno = False engaged_prev = False params = Params() power_monitor = PowerMonitoring() HARDWARE.initialize_hardware() thermal_config = HARDWARE.get_thermal_config() # TODO: use PI controller for UNO controller = PIController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) 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: pandaState = pandaStates[0] if pandaState.pandaType != log.PandaState.PandaType.unknown: onroad_conditions[ "ignition"] = pandaState.ignitionLine or pandaState.ignitionCan in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client # Setup fan handler on first connect to panda if handle_fan is None and peripheralState.pandaType != log.PandaState.PandaType.unknown: is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno if TICI: cloudlog.info("Setting up TICI fan handler") handle_fan = handle_fan_tici elif is_uno or PC: cloudlog.info("Setting up UNO fan handler") handle_fan = handle_fan_uno else: cloudlog.info("Setting up EON fan handler") setup_eon_fan() handle_fan = handle_fan_eon 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.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.batteryPercent = HARDWARE.get_battery_capacity() msg.deviceState.batteryCurrent = HARDWARE.get_battery_current() 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 handle_fan is not None: fan_speed = handle_fan(controller, max_comp_temp, fan_speed, onroad_conditions["ignition"]) msg.deviceState.fanSpeedPercentDesired = fan_speed 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"])) if TICI: set_offroad_alert_if_changed("Offroad_StorageMissing", (not Path("/data/media").is_mount())) # 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"[thermald] engaged: {engaged}") 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() # pylint: disable=assignment-from-none msg.deviceState.powerDrawW = current_power_draw if current_power_draw is not None else 0 # 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) if EON and not is_uno: set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power)) 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: if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40: cloudlog.event("High offroad memory usage", mem=msg.deviceState.memoryUsagePercent) 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 thermald_thread(): pm = messaging.PubMaster(['deviceState']) pandaState_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency pandaState_sock = messaging.sub_sock('pandaState', timeout=pandaState_timeout) location_sock = messaging.sub_sock('gpsLocationExternal') managerState_sock = messaging.sub_sock('managerState', conflate=True) fan_speed = 0 count = 0 startup_conditions = { "ignition": False, } startup_conditions_prev = startup_conditions.copy() off_ts = None started_ts = None started_seen = False thermal_status = ThermalStatus.green usb_power = True current_branch = get_git_branch() network_type = NetworkType.none network_strength = NetworkStrength.unknown wifiIpAddress = 'N/A' current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML) pandaState_prev = None charging_disabled = False should_start_prev = False handle_fan = None is_uno = False ui_running_prev = False params = Params() power_monitor = PowerMonitoring() no_panda_cnt = 0 thermal_config = HARDWARE.get_thermal_config() # CPR3 logging if EON: base_path = "/sys/kernel/debug/cpr3-regulator/" cpr_files = [p for p in Path(base_path).glob("**/*") if p.is_file()] cpr_files = ["/sys/kernel/debug/regulator/pm8994_s11/voltage" ] + cpr_files cpr_data = {} for cf in cpr_files: with open(cf, "r") as f: try: cpr_data[str(cf)] = f.read().strip() except Exception: pass cloudlog.event("CPR", data=cpr_data) # sound trigger sound_trigger = 1 opkrAutoShutdown = 0 shutdown_trigger = 1 is_openpilot_view_enabled = 0 env = dict(os.environ) env['LD_LIBRARY_PATH'] = mediaplayer getoff_alert = params.get_bool("OpkrEnableGetoffAlert") hotspot_on_boot = params.get_bool("OpkrHotspotOnBoot") hotspot_run = False if int(params.get("OpkrAutoShutdown", encoding="utf8")) == 0: opkrAutoShutdown = 0 elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 1: opkrAutoShutdown = 5 elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 2: opkrAutoShutdown = 30 elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 3: opkrAutoShutdown = 60 elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 4: opkrAutoShutdown = 180 elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 5: opkrAutoShutdown = 300 elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 6: opkrAutoShutdown = 600 elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 7: opkrAutoShutdown = 1800 elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 8: opkrAutoShutdown = 3600 elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 9: opkrAutoShutdown = 10800 else: opkrAutoShutdown = 18000 while 1: ts = sec_since_boot() pandaState = messaging.recv_sock(pandaState_sock, wait=True) msg = read_thermal(thermal_config) if pandaState is not None: usb_power = pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client # If we lose connection to the panda, wait 5 seconds before going offroad if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown: no_panda_cnt += 1 if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: if startup_conditions["ignition"]: cloudlog.error("Lost panda connection while onroad") startup_conditions["ignition"] = False shutdown_trigger = 1 else: no_panda_cnt = 0 startup_conditions[ "ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan sound_trigger == 1 #startup_conditions["hardware_supported"] = pandaState.pandaState.pandaType not in [log.PandaState.PandaType.whitePanda, # log.PandaState.PandaType.greyPanda] #set_offroad_alert_if_changed("Offroad_HardwareUnsupported", not startup_conditions["hardware_supported"]) # Setup fan handler on first connect to panda if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown: is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno if (not EON) or is_uno: cloudlog.info("Setting up UNO fan handler") handle_fan = handle_fan_uno else: cloudlog.info("Setting up EON fan handler") setup_eon_fan() handle_fan = handle_fan_eon # Handle disconnect if pandaState_prev is not None: if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown and \ pandaState_prev.pandaState.pandaType != log.PandaState.PandaType.unknown: params.clear_all(ParamKeyType.CLEAR_ON_PANDA_DISCONNECT) pandaState_prev = pandaState elif params.get_bool("IsOpenpilotViewEnabled") and not params.get_bool( "IsDriverViewEnabled") and is_openpilot_view_enabled == 0: is_openpilot_view_enabled = 1 startup_conditions["ignition"] = True elif not params.get_bool( "IsOpenpilotViewEnabled") and not params.get_bool( "IsDriverViewEnabled") and is_openpilot_view_enabled == 1: shutdown_trigger = 0 sound_trigger == 0 is_openpilot_view_enabled = 0 startup_conditions["ignition"] = False # get_network_type is an expensive call. update every 10s if (count % int(10. / DT_TRML)) == 0: try: network_type = HARDWARE.get_network_type() network_strength = HARDWARE.get_network_strength(network_type) wifiIpAddress = HARDWARE.get_ip_address() except Exception: cloudlog.exception("Error getting network status") msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) msg.deviceState.memoryUsagePercent = int( round(psutil.virtual_memory().percent)) msg.deviceState.cpuUsagePercent = int(round(psutil.cpu_percent())) msg.deviceState.networkType = network_type msg.deviceState.networkStrength = network_strength msg.deviceState.wifiIpAddress = wifiIpAddress msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity() msg.deviceState.batteryStatus = HARDWARE.get_battery_status() msg.deviceState.batteryCurrent = HARDWARE.get_battery_current() msg.deviceState.batteryVoltage = HARDWARE.get_battery_voltage() msg.deviceState.usbOnline = HARDWARE.get_usb_present() # Fake battery levels on uno for frame if (not EON) or is_uno: msg.deviceState.batteryPercent = 100 msg.deviceState.batteryStatus = "Charging" msg.deviceState.batteryTempC = 0 current_filter.update(msg.deviceState.batteryCurrent / 1e6) # TODO: add car battery voltage check max_cpu_temp = cpu_temp_filter.update(max(msg.deviceState.cpuTempC)) max_comp_temp = max(max_cpu_temp, msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) bat_temp = msg.deviceState.batteryTempC if handle_fan is not None: fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, startup_conditions["ignition"]) msg.deviceState.fanSpeedPercentDesired = fan_speed # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 # We only do this if there is a relay that prevents the car from faulting 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 max_cpu_temp > 107. or bat_temp >= 63. or (is_offroad_for_5_min and max_cpu_temp > 70.0): # onroad not allowed thermal_status = ThermalStatus.danger elif max_comp_temp > 96.0 or bat_temp > 60.: # hysteresis between onroad not allowed and engage not allowed thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger) elif max_cpu_temp > 94.0: # hysteresis between engage not allowed and uploader not allowed thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red) elif max_cpu_temp > 80.0: # uploader not allowed thermal_status = ThermalStatus.yellow elif max_cpu_temp > 75.0: # hysteresis between uploader not allowed and all good thermal_status = clip(thermal_status, ThermalStatus.green, ThermalStatus.yellow) else: thermal_status = ThermalStatus.green # default to good condition # **** starting logic **** # Check for last update time and display alerts if needed now = datetime.datetime.utcnow() # show invalid date/time alert startup_conditions["time_valid"] = True if ( (now.year > 2020) or (now.year == 2020 and now.month >= 10) ) else True # set True for battery less EON otherwise, set False. set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"])) # Show update prompt # try: # last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8')) # except (TypeError, ValueError): # last_update = now # dt = now - last_update # update_failed_count = params.get("UpdateFailedCount") # update_failed_count = 0 if update_failed_count is None else int(update_failed_count) # last_update_exception = params.get("LastUpdateException", encoding='utf8') # if update_failed_count > 15 and last_update_exception is not None: # if current_branch in ["release2", "dashcam"]: # extra_text = "Ensure the software is correctly installed" # else: # extra_text = last_update_exception # set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) # set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) # set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text) # elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1: # set_offroad_alert_if_changed("Offroad_UpdateFailed", False) # set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) # set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True) # elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: # remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0)) # set_offroad_alert_if_changed("Offroad_UpdateFailed", False) # set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) # set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.") # else: # set_offroad_alert_if_changed("Offroad_UpdateFailed", False) # set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) # set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) #startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") startup_conditions["not_uninstalling"] = not params.get_bool( "DoUninstall") startup_conditions["accepted_terms"] = params.get( "HasAcceptedTerms") == terms_version panda_signature = params.get("PandaFirmware") startup_conditions["fw_version_match"] = (panda_signature is None) or ( panda_signature == FW_SIGNATURE ) # don't show alert is no panda is connected (None) set_offroad_alert_if_changed( "Offroad_PandaFirmwareMismatch", (not startup_conditions["fw_version_match"])) # 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 \ (current_branch in ['dashcam', 'dashcam-staging']) 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 startup_conditions[ "device_temp_good"] = thermal_status < ThermalStatus.danger set_offroad_alert_if_changed( "Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"])) # Handle offroad/onroad transition should_start = all(startup_conditions.values()) if should_start != should_start_prev or (count == 0): params.put_bool("IsOffroad", not should_start) HARDWARE.set_power_save(not should_start) if TICI and not params.get_bool("EnableLteOnroad"): fxn = "off" if should_start else "on" os.system(f"nmcli radio wwan {fxn}") if should_start: off_ts = None if started_ts is None: started_ts = sec_since_boot() started_seen = True else: if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev): cloudlog.event("Startup blocked", startup_conditions=startup_conditions) started_ts = None if off_ts is None: off_ts = sec_since_boot() if shutdown_trigger == 1 and sound_trigger == 1 and msg.deviceState.batteryStatus == "Discharging" and started_seen and ( sec_since_boot() - off_ts) > 1 and getoff_alert: subprocess.Popen([ mediaplayer + 'mediaplayer', '/data/openpilot/selfdrive/assets/sounds/eondetach.wav' ], shell=False, stdin=None, stdout=None, stderr=None, env=env, close_fds=True) sound_trigger = 0 # shutdown if the battery gets lower than 3%, it's discharging, we aren't running for # more than a minute but we were running if shutdown_trigger == 1 and msg.deviceState.batteryStatus == "Discharging" and \ started_seen and opkrAutoShutdown and (sec_since_boot() - off_ts) > opkrAutoShutdown and not os.path.isfile(pandaflash_ongoing): os.system('LD_LIBRARY_PATH="" svc power shutdown') charging_disabled = check_car_battery_voltage(should_start, pandaState, charging_disabled, msg) if msg.deviceState.batteryCurrent > 0: msg.deviceState.batteryStatus = "Discharging" else: msg.deviceState.batteryStatus = "Charging" msg.deviceState.chargingDisabled = charging_disabled prebuiltlet = params.get_bool("PutPrebuiltOn") if not os.path.isfile(prebuiltfile) and prebuiltlet: os.system("cd /data/openpilot; touch prebuilt") elif os.path.isfile(prebuiltfile) and not prebuiltlet: os.system("cd /data/openpilot; rm -f prebuilt") sshkeylet = params.get_bool("OpkrSSHLegacy") if not os.path.isfile(sshkeyfile) and sshkeylet: os.system( "cp -f /data/openpilot/selfdrive/assets/addon/key/GithubSshKeys_legacy /data/params/d/GithubSshKeys; chmod 600 /data/params/d/GithubSshKeys; touch /data/public_key" ) elif os.path.isfile(sshkeyfile) and not sshkeylet: os.system( "cp -f /data/openpilot/selfdrive/assets/addon/key/GithubSshKeys_new /data/params/d/GithubSshKeys; chmod 600 /data/params/d/GithubSshKeys; rm -f /data/public_key" ) # opkr hotspot if hotspot_on_boot and not hotspot_run and sec_since_boot() > 120: os.system("service call wifi 37 i32 0 i32 1 &") hotspot_run = True # Offroad power monitoring power_monitor.calculate(pandaState) msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used() msg.deviceState.carBatteryCapacityUwh = max( 0, power_monitor.get_car_battery_capacity()) # # Check if we need to disable charging (handled by boardd) # msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(pandaState, off_ts) # # # Check if we need to shut down # if power_monitor.should_shutdown(pandaState, off_ts, started_seen): # cloudlog.info(f"shutting device down, offroad since {off_ts}") # # TODO: add function for blocking cloudlog instead of sleep # time.sleep(10) # HARDWARE.shutdown() # If UI has crashed, set the brightness to reasonable non-zero value manager_state = messaging.recv_one_or_none(managerState_sock) if manager_state is not None: ui_running = "ui" in (p.name for p in manager_state.managerState.processes if p.running) if ui_running_prev and not ui_running: HARDWARE.set_screen_brightness(20) ui_running_prev = ui_running 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)) msg.deviceState.thermalStatus = thermal_status pm.send("deviceState", msg) if EON and not is_uno: set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power)) should_start_prev = should_start startup_conditions_prev = startup_conditions.copy() # report to server once every 10 minutes if (count % int(600. / DT_TRML)) == 0: if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40: cloudlog.event("High offroad memory usage", mem=msg.deviceState.memoryUsagePercent) location = messaging.recv_sock(location_sock) cloudlog.event("STATUS_PACKET", count=count, pandaState=(strip_deprecated_keys( pandaState.to_dict()) if pandaState else None), location=(strip_deprecated_keys( location.gpsLocationExternal.to_dict()) if location else None), deviceState=strip_deprecated_keys(msg.to_dict())) count += 1
def cleanup_all_processes(signal, frame): cloudlog.info("caught ctrl-c %s %s" % (signal, frame)) manage_baseui(False) for name in running.keys(): kill_managed_process(name) sys.exit(0)
def send_managed_process_signal(name, sig): if name not in running or name not in managed_processes: return cloudlog.info(f"sending signal {sig} to {name}") os.kill(running[name].pid, sig)
def manager_prepare(): # build cereal first subprocess.check_call(["make", "-j4"], cwd="../cereal") # build all processes os.chdir(os.path.dirname(os.path.abspath(__file__))) for p in managed_processes: proc = managed_processes[p] if isinstance(proc, basestring): # import this python cloudlog.info("preimporting %s" % proc) importlib.import_module(proc) else: # build this process cloudlog.info("building %s" % (proc, )) try: subprocess.check_call(["make", "-j4"], cwd=proc[0]) except subprocess.CalledProcessError: # make clean if the build failed cloudlog.info("building %s failed, make clean" % (proc, )) subprocess.check_call(["make", "clean"], cwd=proc[0]) subprocess.check_call(["make", "-j4"], cwd=proc[0]) # install apks installed = get_installed_apks() for app in os.listdir("../apk/"): if ".apk" in app: app = app.split(".apk")[0] if app not in installed: installed[app] = None cloudlog.info("installed apks %s" % (str(installed), )) for app in installed: apk_path = "../apk/" + app + ".apk" if os.path.isfile(apk_path): h1 = hashlib.sha1(open(apk_path).read()).hexdigest() h2 = None if installed[app] is not None: h2 = hashlib.sha1(open(installed[app]).read()).hexdigest() cloudlog.info("comparing version of %s %s vs %s" % (app, h1, h2)) if h2 is None or h1 != h2: cloudlog.info("installing %s" % app) for do_uninstall in [False, True]: if do_uninstall: cloudlog.info("needing to uninstall %s" % app) os.system("pm uninstall %s" % app) ret = os.system( "cp %s /sdcard/%s.apk && pm install -r /sdcard/%s.apk && rm /sdcard/%s.apk" % (apk_path, app, app, app)) if ret == 0: break assert ret == 0
def manager_thread(): shutdownd = Process(name="shutdownd", target=launcher, args=("selfdrive.shutdownd", )) shutdownd.start() # now loop thermal_sock = messaging.sub_sock('thermal') cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) params = Params() # start daemon processes for p in daemon_processes: start_daemon_process(p) # start persistent processes for p in persistent_processes: start_managed_process(p) # start offroad if ANDROID: pm_apply_packages('enable') start_offroad() if os.getenv("NOBOARD") is None: start_managed_process("pandad") if os.getenv("BLOCK") is not None: for k in os.getenv("BLOCK").split(","): del managed_processes[k] started_prev = False logger_dead = False while 1: msg = messaging.recv_sock(thermal_sock, wait=True) # heavyweight batch processes are gated on favorable thermal conditions if msg.thermal.thermalStatus >= ThermalStatus.yellow: for p in green_temp_processes: if p in persistent_processes: kill_managed_process(p) else: for p in green_temp_processes: if p in persistent_processes: start_managed_process(p) if msg.thermal.freeSpace < 0.05: logger_dead = True if msg.thermal.started: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: logger_dead = False driver_view = params.get("IsDriverViewEnabled") == b"1" # TODO: refactor how manager manages processes for p in reversed(car_started_processes): if p not in driver_view_processes or not driver_view: kill_managed_process(p) for p in driver_view_processes: if driver_view: start_managed_process(p) else: kill_managed_process(p) # trigger an update after going offroad if started_prev: send_managed_process_signal("updated", signal.SIGHUP) started_prev = msg.thermal.started # check the status of all processes, did any of them die? running_list = [ "%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running ] cloudlog.debug(' '.join(running_list)) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break
def main(sm=None, pm=None): gc.disable() if sm is None: sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman']) if pm is None: pm = messaging.PubMaster(['liveParameters']) params_reader = Params() # wait for stats about the car to come in from controls cloudlog.info("paramsd is waiting for CarParams") CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True)) cloudlog.info("paramsd got CarParams") min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio params = params_reader.get("LiveParameters") # Check if car model matches if params is not None: params = json.loads(params) if params.get('carFingerprint', None) != CP.carFingerprint: cloudlog.info("Parameter learner found parameters for wrong car.") params = None # Check if starting values are sane if params is not None: try: angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0 steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr params_sane = angle_offset_sane and steer_ratio_sane if not params_sane: cloudlog.info(f"Invalid starting values found {params}") params = None except Exception as e: cloudlog.info(f"Error reading params {params}: {str(e)}") params = None # TODO: cache the params with the capnp struct if params is None: params = { 'carFingerprint': CP.carFingerprint, 'steerRatio': CP.steerRatio, 'stiffnessFactor': 1.0, 'angleOffsetAverageDeg': 0.0, } cloudlog.info("Parameter learner resetting to default values") # When driving in wet conditions the stiffness can go down, and then be too low on the next drive # Without a way to detect this we have to reset the stiffness every drive #params['stiffnessFactor'] = 1.0 params['stiffnessFactor'] = float( int(Params().get("TireStiffnessFactorAdj")) * 0.01) learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg'])) while True: sm.update() for which, updated in sm.updated.items(): if updated: t = sm.logMonoTime[which] * 1e-9 learner.handle_log(t, which, sm[which]) if sm.updated['liveLocationKalman']: x = learner.kf.x if not all(map(math.isfinite, x)): cloudlog.error( "NaN in liveParameters estimate. Resetting to default values" ) learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0) x = learner.kf.x msg = messaging.new_message('liveParameters') msg.logMonoTime = sm.logMonoTime['carState'] msg.liveParameters.posenetValid = True msg.liveParameters.sensorValid = True msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) msg.liveParameters.angleOffsetAverageDeg = math.degrees( x[States.ANGLE_OFFSET]) msg.liveParameters.angleOffsetDeg = msg.liveParameters.angleOffsetAverageDeg + math.degrees( x[States.ANGLE_OFFSET_FAST]) msg.liveParameters.valid = all(( abs(msg.liveParameters.angleOffsetAverageDeg) < 10.0, abs(msg.liveParameters.angleOffsetDeg) < 10.0, 0.2 <= msg.liveParameters.stiffnessFactor <= 5.0, min_sr <= msg.liveParameters.steerRatio <= max_sr, )) if sm.frame % 1200 == 0: # once a minute params = { 'carFingerprint': CP.carFingerprint, 'steerRatio': msg.liveParameters.steerRatio, 'stiffnessFactor': msg.liveParameters.stiffnessFactor, 'angleOffsetAverageDeg': msg.liveParameters.angleOffsetAverageDeg, } put_nonblocking("LiveParameters", json.dumps(params)) pm.send('liveParameters', msg)
def thermald_thread(): pm = messaging.PubMaster(['deviceState']) pandaState_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency pandaState_sock = messaging.sub_sock('pandaState', timeout=pandaState_timeout) location_sock = messaging.sub_sock('gpsLocationExternal') managerState_sock = messaging.sub_sock('managerState', conflate=True) fan_speed = 0 count = 0 startup_conditions = { "ignition": False, } startup_conditions_prev = startup_conditions.copy() off_ts = None started_ts = None started_seen = False thermal_status = ThermalStatus.green usb_power = True current_branch = get_git_branch() network_type = NetworkType.none network_strength = NetworkStrength.unknown wifiIpAddress = 'N/A' current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML) pandaState_prev = None should_start_prev = False handle_fan = None is_uno = False ui_running_prev = False params = Params() power_monitor = PowerMonitoring() no_panda_cnt = 0 thermal_config = HARDWARE.get_thermal_config() # CPR3 logging if EON: base_path = "/sys/kernel/debug/cpr3-regulator/" cpr_files = [p for p in Path(base_path).glob("**/*") if p.is_file()] cpr_data = {} for cf in cpr_files: with open(cf, "r") as f: try: cpr_data[str(cf)] = f.read().strip() except Exception: pass cloudlog.event("CPR", data=cpr_data) while 1: pandaState = messaging.recv_sock(pandaState_sock, wait=True) msg = read_thermal(thermal_config) if pandaState is not None: usb_power = pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client # If we lose connection to the panda, wait 5 seconds before going offroad if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown: no_panda_cnt += 1 if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: if startup_conditions["ignition"]: cloudlog.error("Lost panda connection while onroad") startup_conditions["ignition"] = False else: no_panda_cnt = 0 startup_conditions["ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan startup_conditions["hardware_supported"] = True #pandaState.pandaState.pandaType not in [log.PandaState.PandaType.whitePanda, # log.PandaState.PandaType.greyPanda] set_offroad_alert_if_changed("Offroad_HardwareUnsupported", not startup_conditions["hardware_supported"]) # Setup fan handler on first connect to panda if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown: is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno if (not EON) or is_uno: cloudlog.info("Setting up UNO fan handler") handle_fan = handle_fan_uno else: cloudlog.info("Setting up EON fan handler") setup_eon_fan() handle_fan = handle_fan_eon # Handle disconnect if pandaState_prev is not None: if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown and \ pandaState_prev.pandaState.pandaType != log.PandaState.PandaType.unknown: params.panda_disconnect() pandaState_prev = pandaState # get_network_type is an expensive call. update every 10s if (count % int(10. / DT_TRML)) == 0: try: network_type = HARDWARE.get_network_type() network_strength = HARDWARE.get_network_strength(network_type) wifiIpAddress = HARDWARE.get_ip_address() except Exception: cloudlog.exception("Error getting network status") msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent)) msg.deviceState.cpuUsagePercent = int(round(psutil.cpu_percent())) msg.deviceState.networkType = network_type msg.deviceState.networkStrength = network_strength msg.deviceState.wifiIpAddress = wifiIpAddress msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity() msg.deviceState.batteryStatus = HARDWARE.get_battery_status() msg.deviceState.batteryCurrent = HARDWARE.get_battery_current() msg.deviceState.batteryVoltage = HARDWARE.get_battery_voltage() msg.deviceState.usbOnline = HARDWARE.get_usb_present() # Fake battery levels on uno for frame if (not EON) or is_uno: msg.deviceState.batteryPercent = 100 msg.deviceState.batteryStatus = "Charging" msg.deviceState.batteryTempC = 0 current_filter.update(msg.deviceState.batteryCurrent / 1e6) # TODO: add car battery voltage check max_cpu_temp = cpu_temp_filter.update(max(msg.deviceState.cpuTempC)) max_comp_temp = max(max_cpu_temp, msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) bat_temp = msg.deviceState.batteryTempC if handle_fan is not None: fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, startup_conditions["ignition"]) msg.deviceState.fanSpeedPercentDesired = fan_speed # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 # We only do this if there is a relay that prevents the car from faulting 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 max_cpu_temp > 107. or bat_temp >= 63. or (is_offroad_for_5_min and max_cpu_temp > 70.0): # onroad not allowed thermal_status = ThermalStatus.danger elif max_comp_temp > 96.0 or bat_temp > 60.: # hysteresis between onroad not allowed and engage not allowed thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger) elif max_cpu_temp > 94.0: # hysteresis between engage not allowed and uploader not allowed thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red) elif max_cpu_temp > 80.0: # uploader not allowed thermal_status = ThermalStatus.yellow elif max_cpu_temp > 75.0: # hysteresis between uploader not allowed and all good thermal_status = clip(thermal_status, ThermalStatus.green, ThermalStatus.yellow) else: thermal_status = ThermalStatus.green # default to good condition # **** starting logic **** # Check for last update time and display alerts if needed now = datetime.datetime.utcnow() # show invalid date/time alert 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"])) # Show update prompt try: last_update = now #datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8')) except (TypeError, ValueError): last_update = now dt = now - last_update update_failed_count = 0 #params.get("UpdateFailedCount") update_failed_count = 0 if update_failed_count is None else int(update_failed_count) last_update_exception = params.get("LastUpdateException", encoding='utf8') if update_failed_count > 15 and last_update_exception is not None: if current_branch in ["release2", "dashcam"]: extra_text = "Ensure the software is correctly installed" else: extra_text = last_update_exception set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text) elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1: set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True) elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0)) set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.") else: set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) #startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get("DisableUpdates") == b"1" startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall") startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version panda_signature = params.get("PandaFirmware") startup_conditions["fw_version_match"] = (panda_signature is None) or (panda_signature == FW_SIGNATURE) # don't show alert is no panda is connected (None) set_offroad_alert_if_changed("Offroad_PandaFirmwareMismatch", (not startup_conditions["fw_version_match"])) # 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 \ (current_branch in ['dashcam', 'dashcam-staging']) 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 startup_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"])) # Handle offroad/onroad transition should_start = all(startup_conditions.values()) if should_start: if not should_start_prev: params.delete("IsOffroad") if TICI and DISABLE_LTE_ONROAD: os.system("sudo systemctl stop --no-block lte") off_ts = None if started_ts is None: started_ts = sec_since_boot() started_seen = True else: if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev): cloudlog.event("Startup blocked", startup_conditions=startup_conditions) if should_start_prev or (count == 0): params.put_bool("IsOffroad", True) if TICI and DISABLE_LTE_ONROAD: os.system("sudo systemctl start --no-block lte") started_ts = None if off_ts is None: off_ts = sec_since_boot() # Offroad power monitoring power_monitor.calculate(pandaState) msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used() msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity()) # Check if we need to disable charging (handled by boardd) msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(pandaState, off_ts) # Check if we need to shut down if power_monitor.should_shutdown(pandaState, off_ts, started_seen): cloudlog.info(f"shutting device down, offroad since {off_ts}") # TODO: add function for blocking cloudlog instead of sleep time.sleep(10) HARDWARE.shutdown() # If UI has crashed, set the brightness to reasonable non-zero value manager_state = messaging.recv_one_or_none(managerState_sock) if manager_state is not None: ui_running = "ui" in (p.name for p in manager_state.managerState.processes if p.running) if ui_running_prev and not ui_running: HARDWARE.set_screen_brightness(20) ui_running_prev = ui_running 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)) msg.deviceState.thermalStatus = thermal_status pm.send("deviceState", msg) if EON and not is_uno: set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power)) should_start_prev = should_start startup_conditions_prev = startup_conditions.copy() # report to server once per minute if (count % int(60. / DT_TRML)) == 0: location = messaging.recv_sock(location_sock) cloudlog.event("STATUS_PACKET", count=count, pandaState=(strip_deprecated_keys(pandaState.to_dict()) if pandaState else None), location=(strip_deprecated_keys(location.gpsLocationExternal.to_dict()) if location else None), deviceState=strip_deprecated_keys(msg.to_dict())) count += 1
def manager_thread(): # now loop thermal_sock = messaging.sub_sock(service_list['thermal'].port) cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) params = Params() # start daemon processes for p in daemon_processes: start_daemon_process(p, params) # start persistent processes for p in persistent_processes: start_managed_process(p) # start frame pm_apply_packages('enable') system("am start -n ai.comma.plus.frame/.MainActivity") if os.getenv("NOBOARD") is None: start_managed_process("pandad") logger_dead = False # Tinkla interface global tinklaClient tinklaClient = TinklaClient() sendUserInfoToTinkla(params) while 1: msg = messaging.recv_sock(thermal_sock, wait=True) # uploader is gated based on the phone temperature if msg.thermal.thermalStatus >= ThermalStatus.yellow: kill_managed_process("uploader") else: start_managed_process("uploader") # Attempt to send pending messages if there's any that queued while offline tinklaClient.attemptToSendPendingMessages() if msg.thermal.freeSpace < 0.05: logger_dead = True if msg.thermal.started: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: logger_dead = False # print "msg.thermal.started is False" for p in car_started_processes: kill_managed_process(p) # check the status of all processes, did any of them die? running_list = [" running %s %s" % (p, running[p]) for p in running] #cloudlog.debug('\n'.join(running_list)) # is this still needed? if params.get("DoUninstall") == "1": break
def manager_thread(): # now loop thermal_sock = messaging.sub_sock('thermal') cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) params = Params() # start daemon processes for p in daemon_processes: start_daemon_process(p) # start persistent processes for p in persistent_processes: start_managed_process(p) # start offroad if ANDROID: pm_apply_packages('enable') start_offroad() if os.getenv("NOBOARD") is None: start_managed_process("pandad") if os.getenv("BLOCK") is not None: for k in os.getenv("BLOCK").split(","): del managed_processes[k] logger_dead = False while 1: msg = messaging.recv_sock(thermal_sock, wait=True) # heavyweight batch processes are gated on favorable thermal conditions if msg.thermal.thermalStatus >= ThermalStatus.yellow: for p in green_temp_processes: if p in persistent_processes: kill_managed_process(p) else: for p in green_temp_processes: if p in persistent_processes: start_managed_process(p) if msg.thermal.freeSpace < 0.05: logger_dead = True if msg.thermal.started and "driverview" not in running: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: logger_dead = False for p in reversed(car_started_processes): kill_managed_process(p) # this is ugly if "driverview" not in running and params.get( "IsDriverViewEnabled") == b"1": start_managed_process("driverview") elif "driverview" in running and params.get( "IsDriverViewEnabled") == b"0": kill_managed_process("driverview") # check the status of all processes, did any of them die? running_list = [ "%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running ] cloudlog.debug(' '.join(running_list)) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break
def manager_cleanup(): for p in managed_processes.values(): p.stop() cloudlog.info("everything is dead")
def update(self, sm, pm, CP, VM, PP): """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = sm['carState'].vEgo long_control_state = sm['controlsState'].longControlState v_cruise_kph = sm['controlsState'].vCruise force_slow_decel = sm['controlsState'].forceDecel v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS lead_1 = sm['radarState'].leadOne lead_2 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) if len(sm['model'].path.poly): path = list(sm['model'].path.poly) # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function # y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b # k = y'' / (1 + y'^2)^1.5 # TODO: compute max speed without using a list of points and without numpy y_p = 3 * path[0] * self.path_x**2 + 2 * path[ 1] * self.path_x + path[2] y_pp = 6 * path[0] * self.path_x + 2 * path[1] curv = y_pp / (1. + y_p**2)**1.5 a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None)) model_speed = np.min(v_curvature) model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph else: model_speed = MAX_SPEED # Calculate speed for normal cruise control if enabled: accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)] jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns( v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1], jerk_limits[0], LON_MPC_STEP) self.v_model, self.a_model = speed_smoother( self.v_acc_start, self.a_acc_start, model_speed, 2 * accel_limits[1], accel_limits[0], 2 * jerk_limits[1], jerk_limits[0], LON_MPC_STEP) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else v_ego reset_accel = self.CP.startAccel if starting else a_ego self.v_acc = reset_speed self.a_acc = reset_accel self.v_acc_start = reset_speed self.a_acc_start = reset_accel self.v_cruise = reset_speed self.a_cruise = reset_accel self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint) self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not sm['carState'].brakePressed if fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) radar_dead = not sm.alive['radarState'] radar_errors = list(sm['radarState'].radarErrors) radar_fault = car.RadarData.Error.fault in radar_errors radar_can_error = car.RadarData.Error.canError in radar_errors # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') plan_send.valid = sm.all_alive_and_valid( service_list=['carState', 'controlsState', 'radarState']) plan_send.plan.mdMonoTime = sm.logMonoTime['model'] plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] # longitudal plan plan_send.plan.vCruise = float(self.v_cruise) plan_send.plan.aCruise = float(self.a_cruise) plan_send.plan.vStart = float(self.v_acc_start) plan_send.plan.aStart = float(self.a_acc_start) plan_send.plan.vTarget = float(self.v_acc) plan_send.plan.aTarget = float(self.a_acc) plan_send.plan.vTargetFuture = float(self.v_acc_future) plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCanError = bool(radar_can_error) plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] # Send out fcw plan_send.plan.fcw = fcw pm.send('plan', plan_send) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (DT_PLAN / LON_MPC_STEP) * ( self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + DT_PLAN * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol