def update(self, c): # get basic data from phone and gps since CAN isn't connected sensors = messaging.recv_sock(self.sensor) if sensors is not None: for sensor in sensors.sensorEvents: if sensor.type == 4: # gyro self.yaw_rate_meas = -sensor.gyro.v[0] gps = messaging.recv_sock(self.gps) if gps is not None: self.speed = gps.gpsLocation.speed # create message ret = car.CarState.new_message() # speeds ret.vEgo = self.speed ret.vEgoRaw = self.speed ret.aEgo = 0. self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate ret.yawRate = self.yaw_rate ret.standstill = self.speed < 0.01 ret.wheelSpeeds.fl = self.speed ret.wheelSpeeds.fr = self.speed ret.wheelSpeeds.rl = self.speed ret.wheelSpeeds.rr = self.speed curvature = self.yaw_rate / max(self.speed, 1.) ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG events = [] #events.append(create_event('passive', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) ret.events = events return ret.as_reader()
def read(self): # TODO: refactor the try/excepts to gracefully fail """Read the data from the zmq sockets.""" # the gpsLocation packet is only sent when we're onroad, so don't wait for it try: location_sock = messaging.recv_sock(self.location) if location_sock is not None: self.latitude = round(location_sock.gpsLocation.latitude, 6) self.longitude = round(location_sock.gpsLocation.longitude, 6) self.altitude = round(location_sock.gpsLocation.altitude, 2) self.speed = round(location_sock.gpsLocation.speed, 2) except: print("Location sock failed") # the health packet is only sent every so often, so don't wait for it try: health_sock = messaging.recv_sock(self.health) if health_sock is not None: self.car_voltage = health_sock.health.voltage except: print("Health sock failed") # we can afford to wait for this packet, because it's sent regularly at all times try: thermal_sock = messaging.recv_sock(self.thermal, wait=True) if thermal_sock is not None: self.eon_soc = thermal_sock.thermal.batteryPercent self.bat_temp = round(thermal_sock.thermal.bat * .001, 2) self.usbonline = thermal_sock.thermal.usbOnline self.started = thermal_sock.thermal.started except: print("Thermal sock failed") self.last_read = time.time() return
def data_sample(self): self.prof = Profiler() self.cur_time = sec_since_boot() # first read can and compute car states self.CS = self.CI.update() self.prof.checkpoint("CarInterface") # *** thermal checking logic *** # thermal data, checked every second td = messaging.recv_sock(self.thermal) if td is not None: # Check temperature. self.overtemp = any( t > 950 for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) # under 15% of space free self.free_space = td.thermal.freeSpace # read calibration status cal = messaging.recv_sock(self.cal) if cal is not None: self.cal_status = cal.liveCalibration.calStatus self.cal_perc = cal.liveCalibration.calPerc
def update(self, CS, LoC): #print "===>>> File: controls/lib/planner.py; FUnction: update" cur_time = sec_since_boot() md = messaging.recv_sock(self.model) #print "============ Planner" #print md if md is not None: self.last_md_ts = md.logMonoTime # print "========= frameId" # print md.model.frameId # else: # print "======== None" l20 = messaging.recv_sock(self.live20) if l20 is not None: self.last_l20_ts = l20.logMonoTime self.PP.update(cur_time, CS.vEgo, md) # LoC.v_pid -> CS.vEgo # TODO: is this change okay? self.AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20) # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts # lateral plan plan_send.plan.lateralValid = not self.PP.dead if plan_send.plan.lateralValid: plan_send.plan.dPoly = map(float, self.PP.d_poly) #else: #print "====================No Vision Data========" # longitudal plan plan_send.plan.longitudinalValid = not self.AC.dead if plan_send.plan.longitudinalValid: plan_send.plan.vTarget = float(self.AC.v_target_lead) plan_send.plan.aTargetMin = float(self.AC.a_target[0]) plan_send.plan.aTargetMax = float(self.AC.a_target[1]) plan_send.plan.jerkFactor = float(self.AC.jerk_factor) plan_send.plan.hasLead = self.AC.has_lead # compute risk of collision events: fcw self.FCW.process(CS, self.AC) plan_send.plan.fcw = bool(self.FCW.active) self.plan.send(plan_send.to_bytes()) return plan_send
def calibrationd_thread(gctx): context = zmq.Context() features = messaging.sub_sock(context, service_list['features'].port) live100 = messaging.sub_sock(context, service_list['live100'].port) livecalibration = messaging.pub_sock(context, service_list['liveCalibration'].port) # subscribe to stats about the car VP = VehicleParams(False) v_ego = None calib = load_calibration(gctx) last_cal_cycle = calib.cal_cycle while 1: # calibration at the end so it does not delay radar processing above ft = messaging.recv_sock(features, wait=True) # get latest here l100 = messaging.recv_sock(live100) if l100 is not None: v_ego = l100.live100.vEgo steer_angle = l100.live100.angleSteers if v_ego is None: continue p0 = ft.features.p0 p1 = ft.features.p1 st = ft.features.status calib.calibration(p0, p1, st, v_ego, steer_angle, VP) # write a new calibration every 100 cal cycle if calib.cal_cycle - last_cal_cycle >= 100: print "writing cal", calib.cal_cycle with open(CALIBRATION_FILE, "w") as cal_file: cal_file.write(str(calib.cal_cycle) + '\n') cal_file.write(str(calib.cal_status) + '\n') cal_file.write(str(calib.vp_f[0]) + '\n') cal_file.write(str(calib.vp_f[1]) + '\n') last_cal_cycle = calib.cal_cycle warp_matrix = map(float, calib.warp_matrix.reshape(9).tolist()) dat = messaging.new_message() dat.init('liveCalibration') dat.liveCalibration.warpMatrix = warp_matrix dat.liveCalibration.calStatus = calib.cal_status dat.liveCalibration.calCycle = calib.cal_cycle dat.liveCalibration.calPerc = calib.cal_perc livecalibration.send(dat.to_bytes())
def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space): # *** read can and compute car states *** CS = CI.update(CC) events = list(CS.events) # *** thermal checking logic *** # thermal data, checked every second td = messaging.recv_sock(thermal) if td is not None: # overtemp above 95 deg overtemp = any(t > 950 for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) # under 15% of space free no enable allowed free_space = td.thermal.freeSpace < 0.15 if overtemp: events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if free_space: events.append(create_event('outOfSpace', [ET.NO_ENTRY])) # *** read calibration status *** cal = messaging.recv_sock(cal) if cal is not None: cal_status = cal.liveCalibration.calStatus if cal_status != Calibration.CALIBRATED: if cal_status == Calibration.UNCALIBRATED: events.append( create_event('calibrationInProgress', [ET.NO_ENTRY, ET.SOFT_DISABLE])) else: events.append( create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) # *** health checking logic *** hh = messaging.recv_sock(health) if hh is not None: controls_allowed = hh.health.controlsAllowed if not controls_allowed: events.append( create_event('controlsMismatch', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) return CS, events, cal_status, overtemp, free_space
def update(self,v,a,str_angl): se_list = None min_str_angl = 1.0 str_ratio = 12.45 #car official steer ratio wb =2.964 #wheelbase in meters #compute radius of curvature r = 0. d = 1. lat_a = 0. if abs(str_angl) > min_str_angl: d = str_angl / abs(str_angl) r = wb / math.sqrt(2-2*math.cos(2*str_angl/str_ratio)) lat_a = d * v * v / r for socket, _ in self.poller.poll(0): if socket is self.sensorEvents: se_list = messaging.recv_sock(socket) if se_list is not None: for se in se_list.sensorEvents: if se.which == cereal_SensorEventData_acceleration: self.yaw = self.list_add(self.yaw_list,se.acceleration.v[0]) self.roll = self.list_add(self.roll_list,se.acceleration.v[1]) #+ lat_a self.pitch = self.list_add(self.pitch_list,se.acceleration.v[2]) #- a if se.which == cereal_SensorEventData_magnetic: self.mag_yaw = self.list_add(self.mag_yaw_list,se.magnetic.v[0]) self.mag_roll = self.list_add(self.mag_roll_list,se.magnetic.v[1]) self.mag_pitch = self.list_add(self.mag_pitch_list,se.magnetic.v[2]) if se.which == cereal_SensorEventData_gyro: dp,dr,dy = self.update_gyro_data(se.gyro.v[2],se.gyro.v[1],se.gyro.v[0]) self.gyro_yaw += dy self.gyro_roll += dr self.gyro_pitch += dp return self.vector_to_rad(self.pitch, self.roll, self.yaw),self.vector_to_rad(self.mag_pitch, self.mag_roll, self.mag_yaw),(self.gyro_pitch, self.gyro_roll, self.gyro_yaw)
def main(gctx=None): poller = zmq.Poller() sock = messaging.sub_sock(service_list['controlsState'].port, poller) poller.poll(timeout=1000) last_v_ego = 0. last_active = False env = dict(os.environ) env['LD_LIBRARY_PATH'] = mediaplayer while 1: v_ego = 0 active = False controls_state = messaging.recv_sock(sock, wait=True) if controls_state is not None: v_ego = controls_state.controlsState.vEgo active = controls_state.controlsState.active # we are driving and all of sudden we dont have any speed at all # we better warn the driver before it's too late if last_active and last_v_ego >= 5 and v_ego == 0: subprocess.Popen([mediaplayer + 'mediaplayer', '/data/openpilot/selfdrive/dragonpilot/safeguardd/error.wav'], shell = False, stdin=None, stdout=None, stderr=None, env = env, close_fds=True) last_active = active last_v_ego = v_ego time.sleep(0.1)
def boardd_proxy_loop(rate=100, address="192.168.2.251"): rk = Ratekeeper(rate) can_init() # *** subscribes can logcan = messaging.sub_sock('can', addr=address) # *** publishes to can send sendcan = messaging.pub_sock('sendcan') # drain sendcan to delete any stale messages from previous runs messaging.drain_sock(sendcan) while 1: # recv @ 100hz can_msgs = can_recv() #for m in can_msgs: # print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex"))) # publish to logger # TODO: refactor for speed if len(can_msgs) > 0: dat = can_list_to_can_capnp(can_msgs, "sendcan") sendcan.send(dat) # send can if we have a packet tsc = messaging.recv_sock(logcan) if tsc is not None: cl = can_capnp_to_can_list(tsc.can) #for m in cl: # print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex"))) can_send_many(cl) rk.keep_time()
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 while 1: # get health of board, log this in "thermal" 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 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 boardd_proxy_loop(rate=200, address="192.168.2.251"): rk = Ratekeeper(rate) context = zmq.Context() can_init() # *** subscribes can logcan = messaging.sub_sock(context, service_list['can'].port, addr=address) # *** publishes to can send sendcan = messaging.pub_sock(context, service_list['sendcan'].port) while 1: # recv @ 100hz can_msgs = can_recv() #for m in can_msgs: # print "R:",hex(m[0]), str(m[2]).encode("hex") # publish to logger # TODO: refactor for speed if len(can_msgs) > 0: dat = can_list_to_can_capnp(can_msgs, "sendcan") sendcan.send(dat.to_bytes()) # send can if we have a packet tsc = messaging.recv_sock(logcan) if tsc is not None: cl = can_capnp_to_can_list(tsc.can) #for m in cl: # print "S:",hex(m[0]), str(m[2]).encode("hex") can_send_many(cl) rk.keep_time()
def update(self, cur_time, v_ego): md = messaging.recv_sock(self.model) if md is not None: self.logMonoTime = md.logMonoTime p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line p_prob = 1. # model does not tell this probability yet, so set to 1 for now l_prob = md.model.leftLane.prob # left line prob r_prob = md.model.rightLane.prob # right line prob self.lead_dist = md.model.lead.dist self.lead_prob = md.model.lead.prob self.lead_var = md.model.lead.std**2 # compute target path self.d_poly, _, _ = calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego) self.last_model = cur_time self.dead = False elif cur_time - self.last_model > 0.5: self.dead = True
def speedlimitd_thread(): context = zmq.Context() gps_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True) speedlimit_sock = messaging.pub_sock(context, service_list['speedLimit'].port) while True: gps = messaging.recv_sock(gps_sock) if gps is not None: fix_ok = gps.gpsLocationExternal.flags & 1 if fix_ok: lat = gps.gpsLocationExternal.latitude lon = gps.gpsLocationExternal.longitude try: max_speed = get_max_speed(lat, lon) dat = ui.SpeedLimitData.new_message() if max_speed: dat.speed = max_speed * CV.MPH_TO_MS speedlimit_sock.send(dat.to_bytes()) except Exception as e: print(e)
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) cloudlog.info("manager start") start_managed_process("logmessaged") start_managed_process("logcatd") start_managed_process("uploader") start_managed_process("ui") # *** 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) 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() 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") # start constellation of processes when the car starts if td.health.started: for p in car_started_processes: start_managed_process(p) else: for p in car_started_processes: kill_managed_process(p) # check the status of all processes, did any of them die? for p in running: cloudlog.info(" running %s %s" % (p, running[p]))
def main(gctx=None): if not os.path.exists(dashcam_videos): os.makedirs(dashcam_videos) poller = zmq.Poller() sock = messaging.sub_sock(service_list['thermal'].port, poller) poller.poll(timeout=1000) while 1: if params.get("DragonEnableDashcam") == "1": now = datetime.datetime.now() file_name = now.strftime("%Y-%m-%d_%H-%M-%S") os.system("screenrecord --bit-rate %s --time-limit %s %s%s.mp4 &" % (bit_rates, duration, dashcam_videos, file_name)) used_spaces = get_used_spaces() last_used_spaces = used_spaces # we should clean up files here if use too much spaces # when used spaces greater than max available storage # or when free space is less than 10% # get health of board, log this in "thermal" msg = messaging.recv_sock(sock, wait=True) if used_spaces >= max_storage or ( msg is not None and msg.thermal.freeSpace < freespace_limit): # get all the files in the dashcam_videos path files = [ f for f in sorted(os.listdir(dashcam_videos)) if os.path.isfile(dashcam_videos + f) ] for file in files: msg = messaging.recv_sock(sock, wait=True) # delete file one by one and once it has enough space for 1 video, we stop deleting if used_spaces - last_used_spaces < max_size_per_file or msg.thermal.freeSpace < freespace_limit: os.system("rm -fr %s" % (dashcam_videos + file)) last_used_spaces = get_used_spaces() else: break # we start the process 1 second before screenrecord ended # to make sure there are no missing footage time.sleep(duration - 1) else: time.sleep(1)
def plannerd_thread(gctx): context = zmq.Context() poller = zmq.Poller() carstate = messaging.sub_sock(context, service_list['carState'].port, poller) live20 = messaging.sub_sock(context, service_list['live20'].port) model = messaging.sub_sock(context, service_list['model'].port) plan = messaging.pub_sock(context, service_list['plan'].port) # wait for stats about the car to come in from controls cloudlog.info("plannerd is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("plannerd got CarParams") CS = None PP = PathPlanner(model) AC = AdaptiveCruise(live20) # start the loop set_realtime_priority(2) # this runs whenever we get a packet that can change the plan while True: polld = poller.poll(timeout=1000) for sock, mode in polld: if mode != zmq.POLLIN or sock != carstate: continue cur_time = sec_since_boot() CS = messaging.recv_sock(carstate).carState PP.update(cur_time, CS.vEgo) # LoC.v_pid -> CS.vEgo # TODO: is this change okay? AC.update(cur_time, CS.vEgo, CS.steeringAngle, CS.vEgo, CP) # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') # lateral plan plan_send.plan.lateralValid = not PP.dead if plan_send.plan.lateralValid: plan_send.plan.dPoly = map(float, PP.d_poly) # longitudal plan plan_send.plan.longitudinalValid = not AC.dead if plan_send.plan.longitudinalValid: plan_send.plan.vTarget = float(AC.v_target_lead) plan_send.plan.aTargetMin = float(AC.a_target[0]) plan_send.plan.aTargetMax = float(AC.a_target[1]) plan_send.plan.jerkFactor = float(AC.jerk_factor) plan_send.plan.hasLead = AC.has_lead plan.send(plan_send.to_bytes())
def main(gctx=None): context = zmq.Context() manager_sock = messaging.sub_sock(context, service_list['managerData'].port) NEED_REBOOT = False while True: # try network ping_failed = subprocess.call( ["ping", "-W", "4", "-c", "1", "8.8.8.8"]) if ping_failed: time.sleep(60) continue # download application update try: r = subprocess.check_output(NICE_LOW_PRIORITY + ["git", "fetch"], stderr=subprocess.STDOUT) except subprocess.CalledProcessError as e: cloudlog.event("git fetch failed", cmd=e.cmd, output=e.output, returncode=e.returncode) time.sleep(60) continue cloudlog.info("git fetch success: %s", r) if kegman.get("autoUpdate", True) and not os.path.isfile("/data/no_ota_updates"): try: head_commit = subprocess.check_output( ["git", "rev-parse", "HEAD"]) local_commit = subprocess.check_output( ["git", "rev-parse", "@{u}"]) if head_commit != local_commit: r = subprocess.check_output(NICE_LOW_PRIORITY + ["git", "pull"], stderr=subprocess.STDOUT) NEED_REBOOT = True except subprocess.CalledProcessError as e: cloudlog.event("git pull failed", cmd=e.cmd, output=e.output, returncode=e.returncode) time.sleep(60) continue cloudlog.info("git pull success: %s", r) if NEED_REBOOT: msg = messaging.recv_sock(manager_sock, wait=True) if msg: if "controlsd" not in msg.managerData.runningProcesses: NEED_REBOOT = False os.system('reboot') time.sleep(30 * 60)
def update(self, c): self.rk.keep_time() # get basic data from phone and gps since CAN isn't connected sensors = messaging.recv_sock(self.sensor) if sensors is not None: for sensor in sensors.sensorEvents: if sensor.type == 4: # gyro self.yaw_rate_meas = -sensor.gyro.v[0] gps = messaging.recv_sock(self.gps) if gps is not None: self.prev_speed = self.speed self.speed = gps.gpsLocation.speed # create message ret = car.CarState.new_message() # speeds ret.vEgo = self.speed ret.vEgoRaw = self.speed a = self.speed - self.prev_speed ret.aEgo = a ret.brakePressed = a < -0.5 self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate ret.yawRate = self.yaw_rate ret.standstill = self.speed < 0.01 ret.wheelSpeeds.fl = self.speed ret.wheelSpeeds.fr = self.speed ret.wheelSpeeds.rl = self.speed ret.wheelSpeeds.rr = self.speed curvature = self.yaw_rate / max(self.speed, 1.) ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG events = [] ret.events = events return ret.as_reader()
def manager_thread(): # now loop context = zmq.Context() thermal_sock = messaging.sub_sock(context, service_list['thermal'].port) cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) 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") params = Params() logger_dead = False while 1: # get health of board, log this in "thermal" 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 car_started_processes: kill_managed_process(p) # check the status of all processes, did any of them die? for p in running: cloudlog.debug(" running %s %s" % (p, running[p])) # is this still needed? if params.get("DoUninstall") == "1": break
def update(self, cur_time, v_ego, angle_steers, v_pid, CP): l20 = messaging.recv_sock(self.live20) if l20 is not None: self.l1 = l20.live20.leadOne self.l2 = l20.live20.leadTwo self.logMonoTime = l20.logMonoTime # TODO: no longer has anything to do with calibration self.last_cal = cur_time self.dead = False elif cur_time - self.last_cal > 0.5: self.dead = True self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \ compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, CP) self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE
def boardd_loop(rate=200): rk = Ratekeeper(rate) context = zmq.Context() can_init() # *** publishes can and health logcan = messaging.pub_sock(context, service_list['can'].port) health_sock = messaging.pub_sock(context, service_list['health'].port) # *** subscribes to can send sendcan = messaging.sub_sock(context, service_list['sendcan'].port) # drain sendcan to delete any stale messages from previous runs messaging.drain_sock(sendcan) while 1: # health packet @ 1hz if (rk.frame % rate) == 0: health = can_health() msg = messaging.new_message() msg.init('health') # store the health to be logged msg.health.voltage = health['voltage'] msg.health.current = health['current'] msg.health.started = health['started'] msg.health.controlsAllowed = True health_sock.send(msg.to_bytes()) # recv @ 100hz can_msgs = can_recv() # publish to logger # TODO: refactor for speed if len(can_msgs) > 0: dat = can_list_to_can_capnp(can_msgs).to_bytes() logcan.send(dat) # send can if we have a packet tsc = messaging.recv_sock(sendcan) if tsc is not None: can_send_many(can_capnp_to_can_list(tsc.sendcan)) rk.keep_time()
def handleTrafficEvents(self, trafficEventsSocket): messages = [] self.reset_traffic_events() tr_ev_list = messaging.recv_sock(trafficEventsSocket) if tr_ev_list is not None: for tr_ev in tr_ev_list.trafficEvents: if tr_ev.type == 0x00: if (tr_ev.distance < self.stopSign_distance): self.stopSign_visible = True self.stopSign_distance = tr_ev.distance self.stopSign_action = tr_ev.action self.stopSign_resume = tr_ev.resuming if tr_ev.type == 0x04: if (tr_ev.distance < self.stopLight_distance): self.stopLight_visible = True self.stopLight_distance = tr_ev.distance self.stopLight_action = tr_ev.action self.stopLight_resume = tr_ev.resuming self.stopLight_color = 1. #0-unknown, 1-red, 2-yellow, 3-green if tr_ev.type == 0x01: if (tr_ev.distance < self.stopLight_distance): self.stopLight_visible = True self.stopLight_distance = tr_ev.distance self.stopLight_action = tr_ev.action self.stopLight_resume = tr_ev.resuming self.stopLight_color = 1. #0-unknown, 1-red, 2-yellow, 3-green if tr_ev.type == 0x02: if (tr_ev.distance < self.stopLight_distance): self.stopLight_visible = True self.stopLight_distance = tr_ev.distance self.stopLight_action = tr_ev.action self.stopLight_resume = tr_ev.resuming self.stopLight_color = 2. #0-unknown, 1-red, 2-yellow, 3-green if tr_ev.type == 0x03: if (tr_ev.distance < self.stopLight_distance): self.stopLight_visible = True self.stopLight_distance = tr_ev.distance self.stopLight_action = tr_ev.action self.stopLight_resume = tr_ev.resuming self.stopLight_color = 3. #0-unknown, 1-red, 2-yellow, 3-green self.checkWhichSign() if not ((self.roadSignType_last == self.roadSignType) and (self.roadSignType == 0xFF)): messages.append(teslacan.create_fake_DAS_sign_msg(self.roadSignType,self.roadSignStopDist,self.roadSignColor,self.roadSignControlActive)) return messages
import sys import argparse import zmq from hexdump import hexdump import selfdrive.messaging as messaging from selfdrive.services import service_list if __name__ == "__main__": context = zmq.Context() poller = zmq.Poller() parser = argparse.ArgumentParser(description='Sniff a communcation socket') parser.add_argument('--raw', action='store_true') parser.add_argument("socket", type=str, nargs='*', help="socket name") args = parser.parse_args() for m in args.socket if len(args.socket) > 0 else service_list: messaging.sub_sock(context, service_list[m].port, poller) while 1: polld = poller.poll(timeout=1000) for sock, mode in polld: if mode != zmq.POLLIN: continue if args.raw: hexdump(sock.recv()) else: print messaging.recv_sock(sock)
def thermald_thread(): setup_eon_fan() # prevent LEECO from undervoltage BATT_PERC_OFF = 10 if LEON else 3 # now loop thermal_sock = messaging.pub_sock(service_list['thermal'].port) health_sock = messaging.sub_sock(service_list['health'].port) location_sock = messaging.sub_sock(service_list['gpsLocation'].port) fan_speed = 0 count = 0 shutdown_count = 0 off_ts = None started_ts = None ignition_seen = False started_seen = False thermal_status = ThermalStatus.green health_sock.RCVTIMEO = 1500 current_filter = FirstOrderFilter(0., CURRENT_TAU, 1.) health_prev = None # Make sure charging is enabled charging_disabled = False os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') params = Params() 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() # clear car params when panda gets disconnected if health is None and health_prev is not None: params.panda_disconnect() health_prev = health # loggerd is gated based on free space avail = get_available_percent() / 100.0 # 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() with open("/sys/class/power_supply/battery/current_now") as f: msg.thermal.batteryCurrent = int(f.read()) with open("/sys/class/power_supply/battery/voltage_now") as f: msg.thermal.batteryVoltage = int(f.read()) with open("/sys/class/power_supply/usb/present") as f: msg.thermal.usbOnline = bool(int(f.read())) usb_online = msg.thermal.usbOnline current_filter.update(msg.thermal.batteryCurrent / 1e6) # TODO: add car battery voltage check max_cpu_temp = 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. fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed) msg.thermal.fanSpeed = fan_speed # thermal logic with hysterisis if max_cpu_temp > 107. or bat_temp >= 63.: # onroad not allowed thermal_status = ThermalStatus.danger elif max_comp_temp > 95. 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 > 90.0: # hysteresis between engage not allowed and uploader not allowed thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red) elif max_cpu_temp > 85.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 **** # start constellation of processes when the car starts ignition = health is not None and health.health.started ignition_seen = ignition_seen or ignition # add voltage check for ignition if not ignition_seen and health is not None and health.health.voltage > 13500: ignition = True do_uninstall = params.get("DoUninstall") == "1" accepted_terms = params.get("HasAcceptedTerms") == "1" completed_training = params.get( "CompletedTrainingVersion") == training_version should_start = ignition # have we seen a panda? passive = (params.get("Passive") == "1") # 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 ( passive or completed_training) and (not do_uninstall) # 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: # TODO: Add a better warning when this is happening should_start = False if should_start: 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: 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') #charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled) msg.thermal.chargingDisabled = charging_disabled 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()) print(msg) # report to server once per minute if (count % 60) == 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 # shutdown EON if usb is not present after certain time if not usb_online: shutdown_count += 1 else: shutdown_count = 0 if shutdown_count >= _SHUTDOWN_AT: os.system('LD_LIBRARY_PATH="" svc power shutdown')
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) location_sock = messaging.sub_sock(context, service_list['gpsLocation'].port) cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) for p in persistent_processes: start_managed_process(p) # start frame system("am start -n ai.comma.plus.frame/.MainActivity") # do this before panda flashing setup_eon_fan() if os.getenv("NOBOARD") is None: start_managed_process("pandad") params = Params() passive = params.get("Passive") == "1" passive_starter = LocationStarter() started_ts = None logger_dead = False count = 0 fan_speed = 0 ignition_seen = False battery_was_high = False health_sock.RCVTIMEO = 1500 while 1: # get health of board, log this in "thermal" td = messaging.recv_sock(health_sock, wait=True) location = messaging.recv_sock(location_sock) location = location.gpsLocation if location else None 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() with open("/sys/class/power_supply/usb/online") as f: msg.thermal.usbOnline = bool(int(f.read())) # TODO: add car battery voltage check max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 bat_temp = msg.thermal.bat / 1000. fan_speed = handle_fan(max_temp, bat_temp, fan_speed) msg.thermal.fanSpeed = fan_speed msg.thermal.started = started_ts is not None msg.thermal.startedTs = int(1e9 * (started_ts or 0)) thermal_sock.send(msg.to_bytes()) print msg # uploader is gated based on the phone temperature if max_temp > 85.0: cloudlog.warning("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 ignition = td is not None and td.health.started ignition_seen = ignition_seen or ignition do_uninstall = params.get("DoUninstall") == "1" accepted_terms = params.get("HasAcceptedTerms") == "1" should_start = ignition # start on gps in passive mode if passive and not ignition_seen: should_start = should_start or passive_starter.update( started_ts, location) # with 2% left, we killall, otherwise the phone is bricked should_start = should_start and avail > 0.02 # require usb power should_start = should_start and msg.thermal.usbOnline should_start = should_start and accepted_terms and (not do_uninstall) # if any CPU gets above 107 or the battery gets above 53, kill all processes # controls will warn with CPU above 95 or battery above 50 if max_temp > 107.0 or msg.thermal.bat >= 53000: should_start = False if should_start: if not started_ts: params.car_start() started_ts = sec_since_boot() for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: started_ts = None logger_dead = False for p in car_started_processes: kill_managed_process(p) # shutdown if the battery gets lower than 5%, we aren't running, and we are discharging if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging" and battery_was_high: os.system('LD_LIBRARY_PATH="" svc power shutdown') if msg.thermal.batteryPercent > 10: battery_was_high = True # 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()) if do_uninstall: break count += 1
def update(self, CS, LoC, v_cruise_kph, user_distracted): cur_time = sec_since_boot() v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS md = messaging.recv_sock(self.model) 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) l20 = messaging.recv_sock(self.live20) if md is None else None if l20 is not None: 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 # 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 user_distracted: # if user is not responsive to awareness alerts, then start 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) else: starting = LoC.long_control_state == LongCtrlState.starting self.v_cruise = CS.vEgo self.a_cruise = self.CP.startAccel if starting else CS.aEgo self.v_acc_start = CS.vEgo self.a_acc_start = self.CP.startAccel if starting else CS.aEgo self.v_acc = CS.vEgo self.a_acc = self.CP.startAccel if starting else CS.aEgo self.v_acc_sol = CS.vEgo self.a_acc_sol = self.CP.startAccel if starting else CS.aEgo 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) # 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, self.lead_1.vLead, self.lead_1.yRel, self.lead_1.vLat, self.lead_1.fcw, blinkers) \ and not CS.brakePressed if self.fcw: cloudlog.info("FCW triggered") 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.IMMEDIATE_DISABLE])) if 'fault' in self.radar_errors: events.append( create_event('radarFault', [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 # 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 radard_thread(gctx=None): #print "===>>> File: controls/radard.py; FUnction: radard_thread" set_realtime_priority(1) # 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)) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.radarName) exec('from selfdrive.radar.'+CP.radarName+'.interface import RadarInterface') context = zmq.Context() # *** subscribe to features and model from visiond model = messaging.sub_sock(context, service_list['model'].port) live100 = messaging.sub_sock(context, service_list['live100'].port) PP = PathPlanner() RI = RadarInterface() 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 rdr_delay = 0.10 # radar data delay in s v_len = 20 # how many speed data points to remember for t alignment with rdr data enabled = 0 steer_angle = 0. 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.aRel, None, False, None] # receive the live100s l100 = messaging.recv_sock(live100) if l100 is not None: enabled = l100.live100.enabled v_ego = l100.live100.vEgo steer_angle = l100.live100.angleSteers 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 md = messaging.recv_sock(model) #print "============ RADAR Thread" #print md if md is not None: last_md_ts = md.logMonoTime # *** get path prediction from the model *** PP.update(sec_since_boot(), v_ego, md) # run kalman filter only if prob is high enough if PP.lead_prob > 0.7: ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var)) ekfv.predict(tsv) ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])), float(ekfv.state[SPEEDV]), np.nan, last_md_ts, np.nan, sec_since_boot()) 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 enabled: # use path from model path_poly path_y = np.polyval(PP.d_poly, path_x) else: # use path from steer, set angle_offset to 0 since calibration does not exactly report the physical offset path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, CP, 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 the vision point for now if ids == VISION_POINT and not VISION_ONLY: continue elif ids != VISION_POINT and VISION_ONLY: 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 - rdr_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)) # create the track if it doesn't exist or it's a new track if ids not in tracks or rpt[5] == 1: tracks[ids] = Track() tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned) # allow the vision model to remove the stationary flag if distance and rel speed roughly match if VISION_POINT in ar_pts: dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - rpt[0])) ** 2 + (2*(ar_pts[VISION_POINT][1] - rpt[1])) ** 2) rel_speed_diff = abs(ar_pts[VISION_POINT][2] - rpt[2]) tracks[ids].mix_vision(dist_to_vision, rel_speed_diff) # publish tracks (debugging) dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(tracks.keys()): dat.liveTracks[cnt].trackId = ids dat.liveTracks[cnt].dRel = float(tracks[ids].dRel) dat.liveTracks[cnt].yRel = float(tracks[ids].yRel) dat.liveTracks[cnt].vRel = float(tracks[ids].vRel) dat.liveTracks[cnt].aRel = float(tracks[ids].aRel) dat.liveTracks[cnt].stationary = tracks[ids].stationary dat.liveTracks[cnt].oncoming = tracks[ids].oncoming liveTracks.send(dat.to_bytes()) 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 = [] # *** 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.l100MonoTime = last_l100_ts if lead_len > 0: lead_clusters[0].toLive20(dat.live20.leadOne) if lead2_len > 0: lead2_clusters[0].toLive20(dat.live20.leadTwo) else: dat.live20.leadTwo.status = False else: dat.live20.leadOne.status = False dat.live20.cumLagMs = -rk.remaining*1000. live20.send(dat.to_bytes()) rk.monitor_time()
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) location_sock = messaging.sub_sock(context, service_list['gpsLocation'].port) cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) for p in persistent_processes: start_managed_process(p) manage_baseui(True) # do this before panda flashing setup_eon_fan() if os.getenv("NOBOARD") is None: start_managed_process("pandad") passive = bool(os.getenv("PASSIVE")) passive_starter = LocationStarter() started_ts = None logger_dead = False count = 0 fan_speed = 0 ignition_seen = False health_sock.RCVTIMEO = 1500 while 1: # get health of board, log this in "thermal" td = messaging.recv_sock(health_sock, wait=True) location = messaging.recv_sock(location_sock) location = location.gpsLocation if location else None 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() # TODO: add car battery voltage check max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 fan_speed = handle_fan(max_temp, fan_speed) msg.thermal.fanSpeed = fan_speed thermal_sock.send(msg.to_bytes()) print msg # uploader is gated based on the phone temperature if max_temp > 85.0: cloudlog.warning("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 ignition = td is not None and td.health.started ignition_seen = ignition_seen or ignition params = Params() should_start = ignition and (params.get("HasAcceptedTerms") == "1") # start on gps in passive mode if passive and not ignition_seen: should_start = should_start or passive_starter.update(started_ts, location) # with 2% left, we killall, otherwise the phone is bricked should_start = should_start and avail > 0.02 if should_start: if not started_ts: params.car_start() started_ts = sec_since_boot() 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_ts = None 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 main(gctx=None): params = Params() NEED_REBOOT = False health_sock = messaging.sub_sock(service_list['health'].port, conflate=True) while True: # try network ping_failed = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"]) if ping_failed: time.sleep(60) continue # download application update try: r = subprocess.check_output(NICE_LOW_PRIORITY + ["git", "fetch"], stderr=subprocess.STDOUT).decode('utf8') except subprocess.CalledProcessError as e: cloudlog.event("git fetch failed", cmd=e.cmd, output=e.output, returncode=e.returncode) time.sleep(60) continue cloudlog.info("git fetch success: %s", r) # Write update available param try: cur_hash = subprocess.check_output(["git", "rev-parse", "HEAD"]).rstrip() upstream_hash = subprocess.check_output(["git", "rev-parse", "@{u}"]).rstrip() params.put("UpdateAvailable", str(int(cur_hash != upstream_hash))) try: if autoUpdate and not os.path.isfile("/data/no_ota_updates") and cur_hash != upstream_hash: r = subprocess.check_output(NICE_LOW_PRIORITY + ["git", "pull"], stderr=subprocess.STDOUT) NEED_REBOOT = True except subprocess.CalledProcessError as e: cloudlog.event("git pull failed", cmd=e.cmd, output=e.output, returncode=e.returncode) time.sleep(60) continue if NEED_REBOOT: cloudlog.info("git pull success: %s", r) except: params.put("UpdateAvailable", "0") # Write latest release notes to param try: r = subprocess.check_output(["git", "--no-pager", "show", "@{u}:RELEASES.md"]) r = r[:r.find(b'\n\n')] # Slice latest release notes params.put("ReleaseNotes", r + b"\n") except: params.put("ReleaseNotes", "") t = datetime.datetime.now().isoformat() params.put("LastUpdateTime", t.encode('utf8')) if NEED_REBOOT: health = messaging.recv_sock(health_sock, wait=True) if health is not None: if not health.health.started: NEED_REBOOT = False os.system('reboot') time.sleep(30*60)
def controlsd_thread(gctx, rate=100): #rate in Hz # *** log *** context = zmq.Context() live100 = messaging.pub_sock(context, service_list['live100'].port) carstate = messaging.pub_sock(context, service_list['carState'].port) carcontrol = messaging.pub_sock(context, service_list['carControl'].port) sendcan = messaging.pub_sock(context, service_list['sendcan'].port) thermal = messaging.sub_sock(context, service_list['thermal'].port) live20 = messaging.sub_sock(context, service_list['live20'].port) model = messaging.sub_sock(context, service_list['model'].port) health = messaging.sub_sock(context, service_list['health'].port) logcan = messaging.sub_sock(context, service_list['can'].port) # connects to can and sendcan CI = CarInterface() VP = CI.getVehicleParams() PP = PathPlanner(model) AC = AdaptiveCruise(live20) AM = AlertManager() LoC = LongControl() LaC = LatControl() # controls enabled state enabled = False last_enable_request = 0 # learned angle offset angle_offset = 0 # rear view camera state rear_view_toggle = False v_cruise_kph = 255 # 0.0 - 1.0 awareness_status = 0.0 soft_disable_timer = None # Is cpu temp too high to enable? overtemp = False free_space = 1.0 # start the loop set_realtime_priority(2) rk = Ratekeeper(rate, print_delay_threshold=2. / 1000) while 1: prof = Profiler() cur_time = sec_since_boot() # read CAN # CS = CI.update() CS = car.CarState.new_message() CS.vEgo = 13 for a in messaging.drain_sock(logcan): CS.steeringAngle = a.carState.steeringAngle # broadcast carState cs_send = messaging.new_message() cs_send.init('carState') cs_send.carState = CS # copy? carstate.send(cs_send.to_bytes()) prof.checkpoint("CarInterface") # did it request to enable? enable_request, enable_condition = False, False if enabled: # gives the user 6 minutes # awareness_status -= 1.0/(100*60*6) if awareness_status <= 0.: # AM.add("driverDistracted", enabled) awareness_status = 1.0 # reset awareness status on steering if CS.steeringPressed: awareness_status = 1.0 # handle button presses for b in CS.buttonEvents: print b # reset awareness on any user action awareness_status = 1.0 # button presses for rear view if b.type == "leftBlinker" or b.type == "rightBlinker": if b.pressed: rear_view_toggle = True else: rear_view_toggle = False if b.type == "altButton1" and b.pressed: rear_view_toggle = not rear_view_toggle if not VP.brake_only and enabled and not b.pressed: if b.type == "accelCruise": v_cruise_kph = v_cruise_kph - ( v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA elif b.type == "decelCruise": v_cruise_kph = v_cruise_kph - ( v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX) if not enabled and b.type in ["accelCruise", "decelCruise" ] and not b.pressed: enable_request = True # do disable on button down if b.type == "cancel" and b.pressed: AM.add("disable", enabled) # Hack-hack if not enabled: enable_request = True prof.checkpoint("Buttons") # *** health checking logic *** hh = messaging.recv_sock(health) if hh is not None: # if the board isn't allowing controls but somehow we are enabled! if not hh.health.controlsAllowed and enabled: AM.add("controlsMismatch", enabled) # *** thermal checking logic *** # thermal data, checked every second td = messaging.recv_sock(thermal) if False and td is not None: # Check temperature. overtemp = any(t > 950 for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) # under 15% of space free free_space = td.thermal.freeSpace prof.checkpoint("Health") # *** getting model logic *** PP.update(cur_time, CS.vEgo) if rk.frame % 5 == 2: # *** run this at 20hz again *** angle_offset = learn_angle_offset(enabled, CS.vEgo, angle_offset, np.asarray(PP.d_poly), LaC.y_des, CS.steeringPressed) # disable if the pedals are pressed while engaged, this is a user disable if enabled: if CS.gasPressed or CS.brakePressed: AM.add("disable", enabled) if enable_request: # check for pressed pedals if CS.gasPressed or CS.brakePressed: AM.add("pedalPressed", enabled) enable_request = False else: print "enabled pressed at", cur_time last_enable_request = cur_time # don't engage with less than 15% free if free_space < 0.15: AM.add("outOfSpace", enabled) enable_request = False if VP.brake_only: enable_condition = ((cur_time - last_enable_request) < 0.2) and CS.cruiseState.enabled else: enable_condition = enable_request if enable_request or enable_condition or enabled: # add all alerts from car for alert in CS.errors: AM.add(alert, enabled) if False and AC.dead: AM.add("radarCommIssue", enabled) if PP.dead: AM.add("modelCommIssue", enabled) if overtemp: AM.add("overheat", enabled) prof.checkpoint("Model") if enable_condition and not enabled and not AM.alertPresent(): print "*** enabling controls" # beep for enabling AM.add("enable", enabled) # enable both lateral and longitudinal controls enabled = True # on activation, let's always set v_cruise from where we are, even if PCM ACC is active v_cruise_kph = int( round( max(CS.vEgo * CV.MS_TO_KPH * VP.ui_speed_fudge, V_CRUISE_ENABLE_MIN))) # 6 minutes driver you're on awareness_status = 1.0 # reset the PID loops LaC.reset() # start long control at actual speed LoC.reset(v_pid=CS.vEgo) if VP.brake_only and CS.cruiseState.enabled: v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH # *** put the adaptive in adaptive cruise control *** AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, awareness_status, VP) prof.checkpoint("AdaptiveCruise") # *** gas/brake PID loop *** final_gas, final_brake = LoC.update(enabled, CS.vEgo, v_cruise_kph, AC.v_target_lead, AC.a_target, AC.jerk_factor, VP) # *** steering PID loop *** final_steer, sat_flag = LaC.update(enabled, CS.vEgo, CS.steeringAngle, CS.steeringPressed, PP.d_poly, angle_offset, VP) prof.checkpoint("PID") # ***** handle alerts **** # send a "steering required alert" if saturation count has reached the limit if False and sat_flag: AM.add("steerSaturated", enabled) if enabled and AM.alertShouldDisable(): print "DISABLING IMMEDIATELY ON ALERT" enabled = False if enabled and AM.alertShouldSoftDisable(): if soft_disable_timer is None: soft_disable_timer = 3 * rate elif soft_disable_timer == 0: print "SOFT DISABLING ON ALERT" enabled = False else: soft_disable_timer -= 1 else: soft_disable_timer = None # *** push the alerts to current *** alert_text_1, alert_text_2, visual_alert, audible_alert = AM.process_alerts( cur_time) # ***** control the car ***** CC = car.CarControl.new_message() CC.enabled = enabled CC.gas = float(final_gas) CC.brake = float(final_brake) CC.steeringTorque = float(final_steer) CC.cruiseControl.override = True CC.cruiseControl.cancel = bool( (not VP.brake_only) or (not enabled and CS.cruiseState.enabled )) # always cancel if we have an interceptor CC.cruiseControl.speedOverride = float((LoC.v_pid - .3) if ( VP.brake_only and final_brake == 0.) else 0.0) CC.cruiseControl.accelOverride = float(AC.a_pcm) CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = enabled CC.hudControl.lanesVisible = enabled CC.hudControl.leadVisible = bool(AC.has_lead) CC.hudControl.visualAlert = visual_alert CC.hudControl.audibleAlert = audible_alert # this alert will apply next controls cycle #if not CI.apply(CC): # AM.add("controlsFailed", enabled) # broadcast carControl cc_send = messaging.new_message() cc_send.init('carControl') cc_send.carControl = CC # copy? #carcontrol.send(cc_send.to_bytes()) sendcan.send(cc_send.to_bytes()) prof.checkpoint("CarControl") # ***** publish state to logger ***** # publish controls state at 100Hz dat = messaging.new_message() dat.init('live100') # show rear view camera on phone if in reverse gear or when button is pressed dat.live100.rearViewCam = ('reverseGear' in CS.errors) or rear_view_toggle dat.live100.alertText1 = alert_text_1 dat.live100.alertText2 = alert_text_2 dat.live100.awarenessStatus = max(awareness_status, 0.0) if enabled else 0.0 # what packets were used to process dat.live100.canMonoTimes = list(CS.canMonoTimes) dat.live100.mdMonoTime = PP.logMonoTime dat.live100.l20MonoTime = AC.logMonoTime # if controls is enabled dat.live100.enabled = enabled # car state dat.live100.vEgo = CS.vEgo dat.live100.angleSteers = CS.steeringAngle dat.live100.steerOverride = CS.steeringPressed # longitudinal control state dat.live100.vPid = float(LoC.v_pid) dat.live100.vCruise = float(v_cruise_kph) dat.live100.upAccelCmd = float(LoC.Up_accel_cmd) dat.live100.uiAccelCmd = float(LoC.Ui_accel_cmd) # lateral control state dat.live100.yActual = float(LaC.y_actual) dat.live100.yDes = float(LaC.y_des) dat.live100.upSteer = float(LaC.Up_steer) dat.live100.uiSteer = float(LaC.Ui_steer) # processed radar state, should add a_pcm? dat.live100.vTargetLead = float(AC.v_target_lead) dat.live100.aTargetMin = float(AC.a_target[0]) dat.live100.aTargetMax = float(AC.a_target[1]) dat.live100.jerkFactor = float(AC.jerk_factor) # lag dat.live100.cumLagMs = -rk.remaining * 1000. live100.send(dat.to_bytes()) prof.checkpoint("Live100") # *** run loop at fixed rate *** if rk.keep_time(): prof.display()