class RadarInterface(object): tinklaClient = TinklaClient() def __init__(self,CP): # radar self.pts = {} self.extPts = {} self.delay = 0.1 self.useTeslaRadar = CarSettings().get_value("useTeslaRadar") self.TRACK_LEFT_LANE = True self.TRACK_RIGHT_LANE = True self.updated_messages = set() self.canErrorCounter = 0 if self.useTeslaRadar: self.pts = {} self.extPts = {} self.valid_cnt = {key: 0 for key in RADAR_A_MSGS} self.delay = 0.1 # Delay of radar self.rcp = _create_radard_can_parser() self.logcan = messaging.sub_sock(service_list['can'].port) self.radarOffset = CarSettings().get_value("radarOffset") self.trackId = 1 self.trigger_start_msg = RADAR_A_MSGS[0] self.trigger_end_msg = RADAR_B_MSGS[-1] def update(self, can_strings): # radard at 20Hz and return no points if not self.useTeslaRadar: time.sleep(0.05) return car.RadarData.new_message(),self.extPts.values() tm = int(sec_since_boot() * 1e9) if can_strings != None: vls = self.rcp.update_strings(tm, can_strings) self.updated_messages.update(vls) if self.trigger_start_msg not in self.updated_messages: return None,None if self.trigger_end_msg not in self.updated_messages: return None,None rr,rrext = self._update(self.updated_messages) #self.updated_messages.clear() return rr,rrext def _update(self, updated_messages): ret = car.RadarData.new_message() for message in updated_messages: if not(message in RADAR_A_MSGS): if message in self.pts: del self.pts[message] del self.extPts[message] continue cpt = self.rcp.vl[message] cpt2 = self.rcp.vl[message+1] # ensure the two messages are from the same frame reading if cpt['Index'] != cpt2['Index2']: continue if (cpt['LongDist'] >= BOSCH_MAX_DIST) or (cpt['LongDist']==0) or (not cpt['Tracked']): self.valid_cnt[message] = 0 # reset counter if message in self.pts: del self.pts[message] del self.extPts[message] elif cpt['Valid'] and (cpt['LongDist'] < BOSCH_MAX_DIST) and (cpt['LongDist'] > 0) and (cpt['ProbExist'] >= OBJECT_MIN_PROBABILITY): self.valid_cnt[message] += 1 else: self.valid_cnt[message] = max(self.valid_cnt[message] -1, 0) if (self.valid_cnt[message]==0) and (message in self.pts): del self.pts[message] del self.extPts[message] # radar point only valid if it's a valid measurement and score is above 50 # bosch radar data needs to match Index and Index2 for validity # also for now ignore construction elements if (cpt['Valid'] or cpt['Tracked'])and (cpt['LongDist']>0) and (cpt['LongDist'] < BOSCH_MAX_DIST) and \ (self.valid_cnt[message] > VALID_MESSAGE_COUNT_THRESHOLD) and (cpt['ProbExist'] >= OBJECT_MIN_PROBABILITY): if message not in self.pts and ( cpt['Tracked']): self.pts[message] = car.RadarData.RadarPoint.new_message() self.pts[message].trackId = self.trackId self.extPts[message] = tesla.TeslaRadarPoint.new_message() self.extPts[message].trackId = self.trackId self.trackId = (self.trackId + 1) & 0xFFFFFFFFFFFFFFFF if self.trackId ==0: self.trackId = 1 if message in self.pts: self.pts[message].dRel = cpt['LongDist'] # from front of car self.pts[message].yRel = cpt['LatDist'] - self.radarOffset # in car frame's y axis, left is positive self.pts[message].vRel = cpt['LongSpeed'] self.pts[message].aRel = cpt['LongAccel'] self.pts[message].yvRel = cpt2['LatSpeed'] self.pts[message].measured = bool(cpt['Meas']) self.extPts[message].dz = cpt2['dZ'] self.extPts[message].movingState = cpt2['MovingState'] self.extPts[message].length = cpt2['Length'] self.extPts[message].obstacleProb = cpt['ProbObstacle'] self.extPts[message].timeStamp = int(self.rcp.ts[message+1]['Index2']) if self.rcp.vl[message+1]['Class'] >= CLASS_MIN_PROBABILITY: self.extPts[message].objectClass = cpt2['Class'] # for now we will use class 0- unknown stuff to show trucks # we will base that on being a class 1 and length of 2 (hoping they meant width not length, but as germans could not decide) # 0-unknown 1-four wheel vehicle 2-two wheel vehicle 3-pedestrian 4-construction element # going to 0-unknown 1-truck 2-car 3/4-motorcycle/bicycle 5 pedestrian - we have two bits so if self.extPts[message].objectClass == 0: self.extPts[message].objectClass = 1 if (self.extPts[message].objectClass == 1) and ((self.extPts[message].length >= 1.8) or (1.6 < self.extPts[message].dz < 4.5)): self.extPts[message].objectClass = 0 if self.extPts[message].objectClass == 4: self.extPts[message].objectClass = 1 else: self.extPts[message].objectClass = 1 ret.points = self.pts.values() errors = [] if not self.rcp.can_valid: errors.append("canError") self.tinklaClient.logCANErrorEvent(source="radar_interface", canMessage=0, additionalInformation="Invalid CAN Count") self.canErrorCounter += 1 else: self.canErrorCounter = 0 #BB: Only trigger canError for 3 consecutive errors if self.canErrorCounter > 2: ret.errors = errors else: ret.errors = [] return ret,self.extPts.values()
def sendCrashInfoToTinklad(): tinklaClient = TinklaClient() tinklaClient.logCrashStackTraceEvent()
class RadarInterface(RadarInterfaceBase): tinklaClient = TinklaClient() def __init__(self, CP): # radar self.pts = {} self.extPts = {} self.delay = 0 self.useTeslaRadar = CarSettings().get_value("useTeslaRadar") self.TRACK_LEFT_LANE = True self.TRACK_RIGHT_LANE = True self.updated_messages = set() self.canErrorCounter = 0 self.AHB_car_detected = False if self.useTeslaRadar: self.pts = {} self.extPts = {} self.valid_cnt = {key: 0 for key in RADAR_A_MSGS} self.rcp = _create_radard_can_parser() self.radarOffset = CarSettings().get_value("radarOffset") self.trackId = 1 self.trigger_start_msg = RADAR_A_MSGS[0] self.trigger_end_msg = RADAR_B_MSGS[-1] self.radar_ts = CP.radarTimeStep def update(self, can_strings, v_ego): # radard at 20Hz and return no points if not self.useTeslaRadar: time.sleep(0.05) return car.RadarData.new_message(), self.extPts.values( ), self.AHB_car_detected if can_strings is not None: vls = self.rcp.update_strings(can_strings) self.updated_messages.update(vls) if self.trigger_start_msg not in self.updated_messages: return None, None, self.AHB_car_detected if self.trigger_end_msg not in self.updated_messages: return None, None, self.AHB_car_detected rr, rrext, self.AHB_car_detected = self._update( self.updated_messages, v_ego) self.updated_messages.clear() return rr, rrext, self.AHB_car_detected def _update(self, updated_messages, v_ego): ret = car.RadarData.new_message() AHB_car_detected = False for message in updated_messages: if not (message in RADAR_A_MSGS): if message in self.pts: del self.pts[message] del self.extPts[message] continue cpt = self.rcp.vl[message] if not (message + 1 in updated_messages): continue cpt2 = self.rcp.vl[message + 1] # ensure the two messages are from the same frame reading if cpt['Index'] != cpt2['Index2']: continue if (cpt['LongDist'] >= BOSCH_MAX_DIST) or ( cpt['LongDist'] == 0) or (not cpt['Tracked']) or (not cpt['Valid']): self.valid_cnt[message] = 0 # reset counter if message in self.pts: del self.pts[message] del self.extPts[message] elif cpt['Valid'] and (cpt['LongDist'] < BOSCH_MAX_DIST) and ( cpt['LongDist'] > 0) and (cpt['ProbExist'] >= OBJECT_MIN_PROBABILITY): self.valid_cnt[message] += 1 else: self.valid_cnt[message] = max(self.valid_cnt[message] - 20, 0) if (self.valid_cnt[message] == 0) and (message in self.pts): del self.pts[message] del self.extPts[message] # this is the logic used for Auto High Beam (AHB) car detection if (cpt['Valid'] or cpt['Tracked']) and (abs(cpt['LongSpeed']) < 80) and (cpt['LongDist']>0) and (cpt['LongDist'] < AHB_MAX_DISTANCE) and (cpt['LongDist'] < BOSCH_MAX_DIST) and \ (self.valid_cnt[message] > AHB_VALID_MESSAGE_COUNT_THRESHOLD) and (cpt['ProbExist'] >= AHB_OBJECT_MIN_PROBABILITY) and \ (cpt2['Class'] < 4) and (cpt2['ProbClass'] >= AHB_CLASS_MIN_PROBABILITY): # if moving or the relative speed is x% larger than our speed then use to turn high beam off if ((cpt['LongSpeed'] <= -AHB_STATIONARY_MARGIN - v_ego) or (cpt['LongSpeed'] >= AHB_STATIONARY_MARGIN - v_ego)): AHB_car_detected = True if AHB_DEBUG: print(cpt, cpt2) # radar point only valid if it's a valid measurement and score is above 50 # bosch radar data needs to match Index and Index2 for validity # also for now ignore construction elements if (cpt['Valid'] or cpt['Tracked'])and (cpt['LongDist']>0) and (cpt['LongDist'] < BOSCH_MAX_DIST) and \ (self.valid_cnt[message] > VALID_MESSAGE_COUNT_THRESHOLD) and (cpt['ProbExist'] >= OBJECT_MIN_PROBABILITY) and \ (cpt2['Class'] < 4) and ((cpt['LongSpeed'] >= AHB_STATIONARY_MARGIN - v_ego) or (v_ego < 2)): if message not in self.pts and (cpt['Tracked']): self.pts[message] = car.RadarData.RadarPoint.new_message() self.pts[message].trackId = self.trackId self.extPts[message] = tesla.TeslaRadarPoint.new_message() self.extPts[message].trackId = self.trackId self.trackId = (self.trackId + 1) & 0xFFFFFFFFFFFFFFFF if self.trackId == 0: self.trackId = 1 if message in self.pts: self.pts[message].dRel = cpt[ 'LongDist'] # from front of car self.pts[message].yRel = cpt[ 'LatDist'] - self.radarOffset # in car frame's y axis, left is positive self.pts[message].vRel = cpt['LongSpeed'] self.pts[message].aRel = cpt['LongAccel'] self.pts[message].yvRel = cpt2['LatSpeed'] self.pts[message].measured = bool(cpt['Meas']) self.extPts[message].dz = cpt2['dZ'] self.extPts[message].movingState = cpt2['MovingState'] self.extPts[message].length = cpt2['Length'] self.extPts[message].obstacleProb = cpt['ProbObstacle'] self.extPts[message].timeStamp = int( self.rcp.ts[message + 1]['Index2']) if cpt2['ProbClass'] >= CLASS_MIN_PROBABILITY: self.extPts[message].objectClass = cpt2['Class'] # for now we will use class 0- unknown stuff to show trucks # we will base that on being a class 1 and length of 2 (hoping they meant width not length, but as germans could not decide) # 0-unknown 1-four wheel vehicle 2-two wheel vehicle 3-pedestrian 4-construction element # going to 0-unknown 1-truck 2-car 3/4-motorcycle/bicycle 5 pedestrian - we have two bits so if cpt2['Class'] == 0: self.extPts[message].objectClass = 1 if (cpt2['Class'] == 1) and ( (self.extPts[message].length >= 1.8) or (.6 < self.extPts[message].dz < 4.5)): self.extPts[message].objectClass = 0 else: self.extPts[message].objectClass = 1 ret.points = list(self.pts.values()) errors = [] if not self.rcp.can_valid: errors.append("canError") self.tinklaClient.logCANErrorEvent( source="radar_interface", canMessage=0, additionalInformation="Invalid CAN Count") self.canErrorCounter += 1 else: self.canErrorCounter = 0 #BB: Only trigger canError for 3 consecutive errors if self.canErrorCounter > 9: ret.errors = errors else: ret.errors = [] return ret, self.extPts.values(), AHB_car_detected
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, params) # start persistent processes for p in persistent_processes: start_managed_process(p) # start frame pm_apply_packages('enable') system("LD_LIBRARY_PATH= appops set ai.comma.plus.offroad SU allow") system("am start -n ai.comma.plus.frame/.MainActivity") if os.getenv("NOBOARD") is None: start_managed_process("pandad") logger_dead = False # Tinkla interface last_tinklad_send_attempt_time = 0 tinklaClient = TinklaClient() sendUserInfoToTinkla(params=params, tinklaClient=tinklaClient) 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 # Seems this loop runs every second or so, throttle to once every 30s now = time.time() if now - last_tinklad_send_attempt_time >= 30: tinklaClient.attemptToSendPendingMessages() last_tinklad_send_attempt_time = now 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)) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break
def controlsd_thread(gctx=None): gc.disable() # start the loop set_realtime_priority(3) params = Params() tinklaClient = TinklaClient() # Pub Sockets sendcan = messaging.pub_sock(service_list['sendcan'].port) controlsstate = messaging.pub_sock(service_list['controlsState'].port) carstate = messaging.pub_sock(service_list['carState'].port) carcontrol = messaging.pub_sock(service_list['carControl'].port) carevents = messaging.pub_sock(service_list['carEvents'].port) carparams = messaging.pub_sock(service_list['carParams'].port) is_metric = params.get("IsMetric") == "1" passive = params.get("Passive") != "0" sm = messaging.SubMaster([ 'thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', 'liveParameters' ]) logcan = messaging.sub_sock(service_list['can'].port) # 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 wait_for_can(logcan) CI, CP = get_car(logcan, sendcan, is_panda_black) logcan.close() # TODO: Use the logcan socket from above, but that will currenly break the tests 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) 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 if read_only: CP.safetyModel = car.CarParams.SafetyModel.elm327 # diagnostic only # 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() 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])) logAllAliveAndValidInfoToTinklad(sm=sm, tinklaClient=tinklaClient) 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])) tinklaClient.logCANErrorEvent(source="carcontroller", canMessage=0, additionalInformation="Invalid CAN") 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'], sm['liveParameters'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc) rk.keep_time(1. / 10000) # Run at 100Hz prof.checkpoint("State Control") # Publish data CC, events_prev = data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, carevents, carparams, controlsstate, sendcan, 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 manager_thread(): # now loop thermal_sock = messaging.sub_sock('thermal') if os.getenv("GET_CPU_USAGE"): proc_sock = messaging.sub_sock('procLog', conflate=True) 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 # Tinkla interface last_tinklad_send_attempt_time = 0 tinklaClient = TinklaClient() sendUserInfoToTinkla(params=params, tinklaClient=tinklaClient) start_t = time.time() first_proc = None 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) # Attempt to send pending messages if there's any that queued while offline # Seems this loop runs every second or so, throttle to once every 30s now = time.time() if now - last_tinklad_send_attempt_time >= 30: tinklaClient.attemptToSendPendingMessages() last_tinklad_send_attempt_time = now 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 if os.getenv("GET_CPU_USAGE"): dt = time.time() - start_t # Get first sample if dt > 30 and first_proc is None: first_proc = messaging.recv_sock(proc_sock) # Get last sample and exit if dt > 90: first_proc = first_proc last_proc = messaging.recv_sock(proc_sock, wait=True) cleanup_all_processes(None, None) sys.exit(print_cpu_usage(first_proc, last_proc))
def controlsd_thread(sm=None, pm=None, can_sock=None): gc.disable() # start the loop set_realtime_priority(3) params = Params() tinklaClient = TinklaClient() is_metric = params.get("IsMetric", encoding='utf8') == "1" is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1" passive = params.get("Passive", encoding='utf8') == "1" openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1" community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1" passive = passive or not openpilot_enabled_toggle # 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', 'dMonitoringState', 'plan', 'pathPlan', \ 'model']) if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 can_sock = messaging.sub_sock('can', timeout=can_timeout) # wait for health and CAN packets hw_type = messaging.recv_one(sm.sock['health']).health.hwType has_relay = hw_type in [HwType.blackPanda, HwType.uno] print("Waiting for CAN messages...") messaging.get_one_can(can_sock) CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay) 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 community_feature_disallowed = CP.communityFeature and not community_feature_toggle read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed if read_only: CP.safetyModel = car.CarParams.SafetyModel.noOutput # Write CarParams for radard and boardd safety mode cp_bytes = CP.to_bytes() params.put("CarParams", cp_bytes) params.put("CarParamsCache", cp_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) state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 v_cruise_kph_last = 0 mismatch_counter = 0 can_error_counter = 0 last_blinker_frame = 0 events_prev = [] sm['liveCalibration'].calStatus = Calibration.INVALID sm['pathPlan'].sensorValid = True sm['pathPlan'].posenetValid = True sm['thermal'].freeSpace = 1. sm['dMonitoringState'].events = [] sm['dMonitoringState'].awarenessStatus = 1. sm['dMonitoringState'].faceDetected = False # 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) internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not 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_perc, mismatch_counter, can_error_counter = data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter, params) prof.checkpoint("Sample") # Create alerts if not sm.alive['plan'] and sm.alive['pathPlan']: # only plan not being received: radar not communicating events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) elif not sm.all_alive_and_valid(): events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) logAllAliveAndValidInfoToTinklad(sm=sm, tinklaClient=tinklaClient) if not sm['pathPlan'].mpcSolutionValid: events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None: 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.WARNING])) 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])) tinklaClient.logCANErrorEvent(source="carcontroller", canMessage=0, additionalInformation="Invalid CAN") if not sounds_available: events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT])) if internet_needed: events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT])) if community_feature_disallowed: events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT])) if read_only and not passive: events.append(create_event('carUnrecognized', [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, v_acc, a_acc, lac_log, last_blinker_frame = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame) 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, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled, can_error_counter) prof.checkpoint("Sent") rk.monitor_time() prof.display()