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()