def plannerd_thread(sm=None, pm=None): gc.disable() # start the loop set_realtime_priority(2) cloudlog.info("plannerd is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) PL = Planner(CP) PP = PathPlanner(CP) VM = VehicleModel(CP) if sm is None: sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'liveMapData']) if pm is None: pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc']) sm['liveParameters'].valid = True sm['liveParameters'].sensorValid = True sm['liveParameters'].steerRatio = CP.steerRatio sm['liveParameters'].stiffnessFactor = 1.0 while True: sm.update() if sm.updated['model']: PP.update(sm, pm, CP, VM) if sm.updated['radarState']: PL.update(sm, pm, CP, VM, PP)
def radard_thread(sm=None, pm=None, can_sock=None): set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) mocked = CP.carName == "mock" cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module( 'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface if can_sock is None: can_sock = messaging.sub_sock(service_list['can'].port) if sm is None: sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) # *** publish radarState and liveTracks if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) RI = RadarInterface(CP) rk = Ratekeeper(rate, print_delay_threshold=None) RD = RadarD(mocked) has_radar = not CP.radarOffCan while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) rr = RI.update(can_strings) if rr is None: continue sm.update(0) dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar) dat.radarState.cumLagMs = -rk.remaining * 1000. pm.send('radarState', dat) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(tracks.keys()): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } pm.send('liveTracks', dat) rk.monitor_time()
def calibrationd_thread(sm=None, pm=None): if sm is None: sm = messaging.SubMaster(['cameraOdometry']) if pm is None: pm = messaging.PubMaster(['liveCalibration']) calibrator = Calibrator(param_put=True) # buffer with all the messages that still need to be input into the kalman while 1: sm.update() new_vp = calibrator.handle_cam_odom(sm['cameraOdometry']) if DEBUG and new_vp is not None: print 'got new vp', new_vp calibrator.send_data(pm)
def controlsd_thread(sm=None, pm=None, can_sock=None): gc.disable() # start the loop set_realtime_priority(3) params = Params() is_metric = params.get("IsMetric") == "1" passive = params.get("Passive") != "0" # Pub/Sub Sockets if pm is None: pm = messaging.PubMaster([ 'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams' ]) if sm is None: sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ 'gpsLocation'], ignore_alive=['gpsLocation']) if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout) # wait for health and CAN packets hw_type = messaging.recv_one(sm.sock['health']).health.hwType is_panda_black = hw_type == log.HealthData.HwType.blackPanda print("Waiting for CAN messages...") get_one_can(can_sock) CI, CP = get_car(can_sock, pm.sock['sendcan'], is_panda_black) car_recognized = CP.carName != 'mock' # If stock camera is disconnected, we loaded car controls and it's not chffrplus controller_available = CP.enableCamera and CI.CC is not None and not passive read_only = not car_recognized or not controller_available or CP.dashcamOnly if read_only: CP.safetyModel = CP.safetyModelPassive # Write CarParams for radard and boardd safety mode params.put("CarParams", CP.to_bytes()) params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") CC = car.CarControl.new_message() AM = AlertManager() startup_alert = get_startup_alert(car_recognized, controller_available) AM.add(sm.frame, startup_alert, False) LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) if CP.lateralTuning.which() == 'pid': LaC = LatControlPID(CP) elif CP.lateralTuning.which() == 'indi': LaC = LatControlINDI(CP) elif CP.lateralTuning.which() == 'lqr': LaC = LatControlLQR(CP) driver_status = DriverStatus() is_rhd = params.get("IsRHD") if is_rhd is not None: driver_status.is_rhd = bool(int(is_rhd)) state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 v_cruise_kph_last = 0 overtemp = False free_space = False cal_status = Calibration.INVALID cal_perc = 0 mismatch_counter = 0 low_battery = False events_prev = [] sm['pathPlan'].sensorValid = True sm['pathPlan'].posenetValid = True # detect sound card presence sounds_available = not os.path.isfile('/EON') or ( os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE') # controlsd is driven by can recv, expected at 100Hz rk = Ratekeeper(100, print_delay_threshold=None) prof = Profiler(False) # off by default while True: start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\ data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params) prof.checkpoint("Sample") # Create alerts if not sm.all_alive_and_valid(): events.append( create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not sm['pathPlan'].mpcSolutionValid: events.append( create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sm['pathPlan'].sensorValid: events.append( create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT])) if not sm['pathPlan'].paramsValid: events.append(create_event('vehicleModelInvalid', [ET.WARNING])) if not sm['pathPlan'].posenetValid: events.append( create_event('posenetInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not sm['plan'].radarValid: events.append( create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if sm['plan'].radarCanError: events.append( create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not CS.canValid: events.append( create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sounds_available: events.append( create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT])) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and sm[ 'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: events.append( create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not read_only: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc) prof.checkpoint("State Control") # Publish data CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev) prof.checkpoint("Sent") rk.monitor_time() prof.display()
def radard_thread(sm=None, pm=None, can_sock=None): set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) use_tesla_radar = CarSettings().get_value("useTeslaRadar") mocked = (CP.carName == "mock") or ((CP.carName == "tesla") and not use_tesla_radar) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters', 'pathPlan']) # *** publish radarState and liveTracks if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) icLeads = messaging.pub_sock('uiIcLeads') ahbInfo = messaging.pub_sock('ahbInfo') RI = RadarInterface(CP) rk = Ratekeeper(1.0 / DT_RDR, print_delay_threshold=None) RD = RadarD(mocked, RI, use_tesla_radar,RI.delay) has_radar = not CP.radarOffCan or mocked last_md_ts = 0. v_ego = 0. while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) sm.update(0) if sm.updated['controlsState']: v_ego = sm['controlsState'].vEgo rr,rrext,ahbCarDetected = RI.update(can_strings,v_ego) if rr is None: continue dat,datext = RD.update(rk.frame, sm, rr, has_radar, rrext) dat.radarState.cumLagMs = -rk.remaining*1000. pm.send('radarState', dat) icLeads.send(datext.to_bytes()) ahbInfoMsg = tesla.AHBinfo.new_message() ahbInfoMsg.source = 0 ahbInfoMsg.radarCarDetected = ahbCarDetected ahbInfoMsg.cameraCarDetected = False ahbInfo.send(ahbInfoMsg.to_bytes()) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(sorted(tracks.keys())): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } pm.send('liveTracks', dat) rk.monitor_time()