#!/usr/bin/env python3
import time
from cereal import messaging, log
from selfdrive.manager.process_config import managed_processes
if __name__ == "__main__":
procs = ['camerad', 'ui', 'modeld', 'calibrationd']
for p in procs:
managed_processes[p].start()
pm = messaging.PubMaster(
['controlsState', 'deviceState', 'pandaStates', 'carParams'])
msgs = {
s: messaging.new_message(s)
for s in ['controlsState', 'deviceState', 'carParams']
}
msgs['deviceState'].deviceState.started = True
msgs['carParams'].carParams.openpilotLongitudinalControl = True
msgs['pandaStates'] = messaging.new_message('pandaStates', 1)
msgs['pandaStates'].pandaStates[0].ignitionLine = True
msgs['pandaStates'].pandaStates[0].pandaType = log.PandaState.PandaType.uno
try:
while True:
time.sleep(1 / 100) # continually send, rate doesn't matter
for s in msgs:
pm.send(s, msgs[s])
except KeyboardInterrupt:
def dmonitoringd_thread(sm=None, pm=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster(['dMonitoringState'])
if sm is None:
sm = messaging.SubMaster([
'driverState', 'liveCalibration', 'carState', 'model',
'gpsLocation'
],
ignore_alive=['gpsLocation'])
driver_status = DriverStatus()
is_rhd = params.get("IsRHD")
if is_rhd is not None:
driver_status.is_rhd_region = bool(int(is_rhd))
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['carState'].vEgo = 0.
sm['carState'].cruiseState.enabled = False
sm['carState'].cruiseState.speed = 0.
sm['carState'].buttonEvents = []
sm['carState'].steeringPressed = False
sm['carState'].standstill = True
cal_rpy = [0, 0, 0]
v_cruise_last = 0
driver_engaged = False
# 10Hz <- dmonitoringmodeld
while True:
sm.update()
# GPS coords RHD parsing, once every restart
if not driver_status.is_rhd_region_checked and sm.updated[
'gpsLocation']:
is_rhd = is_rhd_region(sm['gpsLocation'].latitude,
sm['gpsLocation'].longitude)
driver_status.is_rhd_region = is_rhd
driver_status.is_rhd_region_checked = True
put_nonblocking("IsRHD", "1" if is_rhd else "0")
# Handle calibration
if sm.updated['liveCalibration']:
if sm['liveCalibration'].calStatus == Calibration.CALIBRATED:
if len(sm['liveCalibration'].rpyCalib) == 3:
cal_rpy = sm['liveCalibration'].rpyCalib
# Get interaction
if sm.updated['carState']:
v_cruise = sm['carState'].cruiseState.speed
driver_engaged = len(sm['carState'].buttonEvents) > 0 or \
v_cruise != v_cruise_last or \
sm['carState'].steeringPressed
if driver_engaged:
_ = driver_status.update([], True,
sm['carState'].cruiseState.enabled,
sm['carState'].standstill)
v_cruise_last = v_cruise
# Get model meta
if sm.updated['model']:
driver_status.set_policy(sm['model'])
# Get data from dmonitoringmodeld
if sm.updated['driverState']:
events = []
driver_status.get_pose(sm['driverState'], cal_rpy,
sm['carState'].vEgo,
sm['carState'].cruiseState.enabled)
# Block any engage after certain distrations
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
# Update events from driver state
events = driver_status.update(events, driver_engaged,
sm['carState'].cruiseState.enabled,
sm['carState'].standstill)
# dMonitoringState packet
dat = messaging.new_message()
dat.init('dMonitoringState')
dat.dMonitoringState = {
"events":
events,
"faceDetected":
driver_status.face_detected,
"isDistracted":
driver_status.driver_distracted,
"awarenessStatus":
driver_status.awareness,
"isRHD":
driver_status.is_rhd_region,
"rhdChecked":
driver_status.is_rhd_region_checked,
"posePitchOffset":
driver_status.pose.pitch_offseter.filtered_stat.mean(),
"posePitchValidCount":
driver_status.pose.pitch_offseter.filtered_stat.n,
"poseYawOffset":
driver_status.pose.yaw_offseter.filtered_stat.mean(),
"poseYawValidCount":
driver_status.pose.yaw_offseter.filtered_stat.n,
"stepChange":
driver_status.step_change,
"awarenessActive":
driver_status.awareness_active,
"awarenessPassive":
driver_status.awareness_passive,
"isLowStd":
driver_status.pose.low_std,
"hiStdCount":
driver_status.hi_stds,
}
pm.send('dMonitoringState', dat)
def thermald_thread(end_event, hw_queue):
pm = messaging.PubMaster(['deviceState'])
sm = messaging.SubMaster([
"peripheralState", "gpsLocationExternal", "controlsState",
"pandaStates"
],
poll=["pandaStates"])
count = 0
onroad_conditions: Dict[str, bool] = {
"ignition": False,
}
startup_conditions: Dict[str, bool] = {}
startup_conditions_prev: Dict[str, bool] = {}
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
usb_power = True
last_hw_state = HardwareState(
network_type=NetworkType.none,
network_metered=False,
network_strength=NetworkStrength.unknown,
network_info=None,
nvme_temps=[],
modem_temps=[],
)
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML)
should_start_prev = False
in_car = False
is_uno = False
engaged_prev = False
params = Params()
power_monitor = PowerMonitoring()
HARDWARE.initialize_hardware()
thermal_config = HARDWARE.get_thermal_config()
fan_controller = None
while not end_event.is_set():
sm.update(PANDA_STATES_TIMEOUT)
pandaStates = sm['pandaStates']
peripheralState = sm['peripheralState']
msg = read_thermal(thermal_config)
if sm.updated['pandaStates'] and len(pandaStates) > 0:
# Set ignition based on any panda connected
onroad_conditions["ignition"] = any(
ps.ignitionLine or ps.ignitionCan for ps in pandaStates
if ps.pandaType != log.PandaState.PandaType.unknown)
pandaState = pandaStates[0]
in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected
usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client
# Setup fan handler on first connect to panda
if fan_controller is None and peripheralState.pandaType != log.PandaState.PandaType.unknown:
is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno
if TICI:
fan_controller = TiciFanController()
elif is_uno or PC:
fan_controller = UnoFanController()
else:
fan_controller = EonFanController()
try:
last_hw_state = hw_queue.get_nowait()
except queue.Empty:
pass
msg.deviceState.freeSpacePercent = get_available_percent(default=100.0)
msg.deviceState.memoryUsagePercent = int(
round(psutil.virtual_memory().percent))
msg.deviceState.cpuUsagePercent = [
int(round(n)) for n in psutil.cpu_percent(percpu=True)
]
msg.deviceState.gpuUsagePercent = int(
round(HARDWARE.get_gpu_usage_percent()))
msg.deviceState.networkType = last_hw_state.network_type
msg.deviceState.networkMetered = last_hw_state.network_metered
msg.deviceState.networkStrength = last_hw_state.network_strength
if last_hw_state.network_info is not None:
msg.deviceState.networkInfo = last_hw_state.network_info
msg.deviceState.nvmeTempC = last_hw_state.nvme_temps
msg.deviceState.modemTempC = last_hw_state.modem_temps
msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness(
)
msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity()
msg.deviceState.batteryCurrent = HARDWARE.get_battery_current()
msg.deviceState.usbOnline = HARDWARE.get_usb_present()
current_filter.update(msg.deviceState.batteryCurrent / 1e6)
max_comp_temp = temp_filter.update(
max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC,
max(msg.deviceState.gpuTempC)))
if fan_controller is not None:
msg.deviceState.fanSpeedPercentDesired = fan_controller.update(
max_comp_temp, onroad_conditions["ignition"])
is_offroad_for_5_min = (started_ts is None) and (
(not started_seen) or (off_ts is None) or
(sec_since_boot() - off_ts > 60 * 5))
if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP:
# If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107
thermal_status = ThermalStatus.danger
else:
current_band = THERMAL_BANDS[thermal_status]
band_idx = list(THERMAL_BANDS.keys()).index(thermal_status)
if current_band.min_temp is not None and max_comp_temp < current_band.min_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1]
elif current_band.max_temp is not None and max_comp_temp > current_band.max_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1]
# **** starting logic ****
# Ensure date/time are valid
now = datetime.datetime.utcnow()
startup_conditions["time_valid"] = (now.year > 2020) or (
now.year == 2020 and now.month >= 10)
set_offroad_alert_if_changed("Offroad_InvalidTime",
(not startup_conditions["time_valid"]))
startup_conditions["up_to_date"] = params.get(
"Offroad_ConnectivityNeeded") is None or params.get_bool(
"DisableUpdates") or params.get_bool("SnoozeUpdate")
startup_conditions["not_uninstalling"] = not params.get_bool(
"DoUninstall")
startup_conditions["accepted_terms"] = params.get(
"HasAcceptedTerms") == terms_version
# with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
params.get_bool("Passive")
startup_conditions["not_driver_view"] = not params.get_bool(
"IsDriverViewEnabled")
startup_conditions["not_taking_snapshot"] = not params.get_bool(
"IsTakingSnapshot")
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
onroad_conditions[
"device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed(
"Offroad_TemperatureTooHigh",
(not onroad_conditions["device_temp_good"]))
# TODO: this should move to TICI.initialize_hardware, but we currently can't import params there
if TICI:
if not os.path.isfile("/persist/comma/living-in-the-moment"):
if not Path("/data/media").is_mount():
set_offroad_alert_if_changed("Offroad_StorageMissing",
True)
else:
# check for bad NVMe
try:
with open("/sys/block/nvme0n1/device/model") as f:
model = f.read().strip()
if not model.startswith(
"Samsung SSD 980") and params.get(
"Offroad_BadNvme") is None:
set_offroad_alert_if_changed(
"Offroad_BadNvme", True)
cloudlog.event("Unsupported NVMe",
model=model,
error=True)
except Exception:
pass
# Handle offroad/onroad transition
should_start = all(onroad_conditions.values())
if started_ts is None:
should_start = should_start and all(startup_conditions.values())
if should_start != should_start_prev or (count == 0):
params.put_bool("IsOnroad", should_start)
params.put_bool("IsOffroad", not should_start)
params.put_bool("IsEngaged", False)
engaged_prev = False
HARDWARE.set_power_save(not should_start)
if sm.updated['controlsState']:
engaged = sm['controlsState'].enabled
if engaged != engaged_prev:
params.put_bool("IsEngaged", engaged)
engaged_prev = engaged
try:
with open('/dev/kmsg', 'w') as kmsg:
kmsg.write(f"<3>[thermald] engaged: {engaged}\n")
except Exception:
pass
if should_start:
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
else:
if onroad_conditions["ignition"] and (startup_conditions !=
startup_conditions_prev):
cloudlog.event("Startup blocked",
startup_conditions=startup_conditions,
onroad_conditions=onroad_conditions)
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
# Offroad power monitoring
power_monitor.calculate(peripheralState, onroad_conditions["ignition"])
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(
0, power_monitor.get_car_battery_capacity())
current_power_draw = HARDWARE.get_current_power_draw() # pylint: disable=assignment-from-none
if current_power_draw is not None:
statlog.sample("power_draw", current_power_draw)
msg.deviceState.powerDrawW = current_power_draw
else:
msg.deviceState.powerDrawW = 0
# Check if we need to disable charging (handled by boardd)
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(
onroad_conditions["ignition"], in_car, off_ts)
# Check if we need to shut down
if power_monitor.should_shutdown(peripheralState,
onroad_conditions["ignition"], in_car,
off_ts, started_seen):
cloudlog.warning(f"shutting device down, offroad since {off_ts}")
params.put_bool("DoShutdown", True)
msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.deviceState.started = started_ts is not None
msg.deviceState.startedMonoTime = int(1e9 * (started_ts or 0))
last_ping = params.get("LastAthenaPingTime")
if last_ping is not None:
msg.deviceState.lastAthenaPingTime = int(last_ping)
msg.deviceState.thermalStatus = thermal_status
pm.send("deviceState", msg)
if EON and not is_uno:
set_offroad_alert_if_changed("Offroad_ChargeDisabled",
(not usb_power))
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# Log to statsd
statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent)
statlog.gauge("gpu_usage_percent", msg.deviceState.gpuUsagePercent)
statlog.gauge("memory_usage_percent",
msg.deviceState.memoryUsagePercent)
for i, usage in enumerate(msg.deviceState.cpuUsagePercent):
statlog.gauge(f"cpu{i}_usage_percent", usage)
for i, temp in enumerate(msg.deviceState.cpuTempC):
statlog.gauge(f"cpu{i}_temperature", temp)
for i, temp in enumerate(msg.deviceState.gpuTempC):
statlog.gauge(f"gpu{i}_temperature", temp)
statlog.gauge("memory_temperature", msg.deviceState.memoryTempC)
statlog.gauge("ambient_temperature", msg.deviceState.ambientTempC)
for i, temp in enumerate(msg.deviceState.pmicTempC):
statlog.gauge(f"pmic{i}_temperature", temp)
for i, temp in enumerate(last_hw_state.nvme_temps):
statlog.gauge(f"nvme_temperature{i}", temp)
for i, temp in enumerate(last_hw_state.modem_temps):
statlog.gauge(f"modem_temperature{i}", temp)
statlog.gauge("fan_speed_percent_desired",
msg.deviceState.fanSpeedPercentDesired)
statlog.gauge("screen_brightness_percent",
msg.deviceState.screenBrightnessPercent)
# report to server once every 10 minutes
if (count % int(600. / DT_TRML)) == 0:
if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40:
cloudlog.event("High offroad memory usage",
mem=msg.deviceState.memoryUsagePercent)
cloudlog.event(
"STATUS_PACKET",
count=count,
pandaStates=[
strip_deprecated_keys(p.to_dict()) for p in pandaStates
],
peripheralState=strip_deprecated_keys(
peripheralState.to_dict()),
location=(strip_deprecated_keys(
sm["gpsLocationExternal"].to_dict())
if sm.alive["gpsLocationExternal"] else None),
deviceState=strip_deprecated_keys(msg.to_dict()))
count += 1
def manager_thread():
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
# save boot log
subprocess.call("./bootlog",
cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
params = Params()
ignore = []
if params.get("DongleId") == UNREGISTERED_DONGLE_ID:
ignore += ["manage_athenad", "uploader"]
if os.getenv("NOBOARD") is not None:
ignore.append("pandad")
if os.getenv("BLOCK") is not None:
ignore += os.getenv("BLOCK").split(",")
ensure_running(managed_processes.values(), started=False, not_run=ignore)
started_prev = False
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['managerState'])
while True:
sm.update()
not_run = ignore[:]
if sm['deviceState'].freeSpacePercent < 5:
not_run.append("loggerd")
started = sm['deviceState'].started
driverview = params.get_bool("IsDriverViewEnabled")
ensure_running(managed_processes.values(), started, driverview,
not_run)
# trigger an update after going offroad
if started_prev and not started and 'updated' in managed_processes:
os.sync()
managed_processes['updated'].signal(signal.SIGHUP)
started_prev = started
running_list = [
"%s%s\u001b[0m" %
("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
for p in managed_processes.values() if p.proc
]
cloudlog.debug(' '.join(running_list))
# send managerState
msg = messaging.new_message('managerState')
msg.managerState.processes = [
p.get_process_state_msg() for p in managed_processes.values()
]
pm.send('managerState', msg)
# TODO: let UI handle this
# Exit main loop when uninstall is needed
if params.get_bool("DoUninstall"):
break
def __init__(self, sm=None, pm=None, can_sock=None):
gc.disable()
set_realtime_priority(3)
self.trace_log = trace1.Loger("controlsd")
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', \
'carControl', 'carEvents', 'carParams'])
self.sm = sm
if self.sm is None:
socks = ['thermal', 'health', 'model', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman']
self.sm = messaging.SubMaster(socks, ignore_alive=['dMonitoringState'])
#self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', \
# 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])
print(" start_Controls messages...1")
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
print(" start_Controls messages...2")
# wait for one health and one CAN packet
hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay)
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None
community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1" or \
internet_needed or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = not os.path.isfile('/EON') or (os.path.isfile('/proc/asound/card0/state') \
and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
put_nonblocking("LongitudinalControl", "1" if self.CP.openpilotLongitudinalControl else "0")
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
print( 'self.CP.lateralTuning.which()={}'.format( self.CP.lateralTuning.which() ) )
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.model_sum = 0
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.consecutive_can_error_count = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.events_prev = []
self.current_alert_types = []
self.sm['liveCalibration'].calStatus = Calibration.INVALID
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized, controller_available, hw_type)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if internet_needed:
self.events.add(EventName.internetConnectivityNeeded, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if self.read_only and not passive:
self.events.add(EventName.carUnrecognized, static=True)
# if hw_type == HwType.whitePanda:
# self.events.add(EventName.whitePandaUnsupported, static=True)
uname = subprocess.check_output(["uname", "-v"], encoding='utf8').strip()
if uname == "#1 SMP PREEMPT Wed Jun 10 12:40:53 PDT 2020":
self.events.add(EventName.neosUpdateRequired, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
self.hyundai_lkas = self.read_only #read_only
self.init_flag = True
self.timer_alloowed = 1500
self.timer_start = 1500
def dmonitoringd_thread(sm=None, pm=None):
if pm is None:
pm = messaging.PubMaster(['driverMonitoringState'])
if sm is None:
sm = messaging.SubMaster([
'driverState', 'liveCalibration', 'carState', 'controlsState',
'modelV2'
],
poll=['driverState'])
driver_status = DriverStatus(rhd=Params().get_bool("IsRHD"))
hands_on_wheel_status = HandsOnWheelStatus()
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['liveCalibration'].rpyCalib = [0, 0, 0]
sm['carState'].buttonEvents = []
sm['carState'].standstill = True
v_cruise_last = 0
driver_engaged = False
steering_wheel_engaged = False
hands_on_wheel_monitoring_enabled = Params().get_bool(
"HandsOnWheelMonitoring")
# 10Hz <- dmonitoringmodeld
while True:
sm.update()
if not sm.updated['driverState']:
continue
# Get interaction
if sm.updated['carState']:
v_cruise = sm['carState'].cruiseState.speed
steering_wheel_engaged = len(sm['carState'].buttonEvents) > 0 or \
v_cruise != v_cruise_last or \
sm['carState'].steeringPressed
driver_engaged = steering_wheel_engaged or sm['carState'].gasPressed
if driver_engaged:
driver_status.update(Events(), True,
sm['controlsState'].enabled,
sm['carState'].standstill)
# Update events and state from hands on wheel monitoring status when steering wheel in engaged
if steering_wheel_engaged and hands_on_wheel_monitoring_enabled:
hands_on_wheel_status.update(Events(), True,
sm['controlsState'].enabled,
sm['carState'].vEgo)
v_cruise_last = v_cruise
if sm.updated['modelV2']:
driver_status.set_policy(sm['modelV2'])
# Get data from dmonitoringmodeld
events = Events()
driver_status.get_pose(sm['driverState'],
sm['liveCalibration'].rpyCalib,
sm['carState'].vEgo,
sm['controlsState'].enabled)
# Block engaging after max number of distrations
if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \
driver_status.terminal_time >= driver_status.settings._MAX_TERMINAL_DURATION:
events.add(car.CarEvent.EventName.tooDistracted)
# Update events from driver state
driver_status.update(events, driver_engaged,
sm['controlsState'].enabled,
sm['carState'].standstill)
# Update events and state from hands on wheel monitoring status
if hands_on_wheel_monitoring_enabled:
hands_on_wheel_status.update(events, steering_wheel_engaged,
sm['controlsState'].enabled,
sm['carState'].vEgo)
# build driverMonitoringState packet
dat = messaging.new_message('driverMonitoringState')
dat.driverMonitoringState = {
"events":
events.to_msg(),
"faceDetected":
driver_status.face_detected,
"isDistracted":
driver_status.driver_distracted,
"awarenessStatus":
driver_status.awareness,
"posePitchOffset":
driver_status.pose.pitch_offseter.filtered_stat.mean(),
"posePitchValidCount":
driver_status.pose.pitch_offseter.filtered_stat.n,
"poseYawOffset":
driver_status.pose.yaw_offseter.filtered_stat.mean(),
"poseYawValidCount":
driver_status.pose.yaw_offseter.filtered_stat.n,
"stepChange":
driver_status.step_change,
"awarenessActive":
driver_status.awareness_active,
"awarenessPassive":
driver_status.awareness_passive,
"isLowStd":
driver_status.pose.low_std,
"hiStdCount":
driver_status.hi_stds,
"isActiveMode":
driver_status.active_monitoring_mode,
"handsOnWheelState":
hands_on_wheel_status.hands_on_wheel_state,
}
pm.send('driverMonitoringState', dat)
def thermald_thread():
pm = messaging.PubMaster(['deviceState'])
pandaState_timeout = int(1000 * 2.5 *
DT_TRML) # 2.5x the expected pandaState frequency
pandaState_sock = messaging.sub_sock('pandaState',
timeout=pandaState_timeout)
location_sock = messaging.sub_sock('gpsLocationExternal')
managerState_sock = messaging.sub_sock('managerState', conflate=True)
fan_speed = 0
count = 0
startup_conditions = {
"ignition": False,
}
startup_conditions_prev = startup_conditions.copy()
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
usb_power = True
current_branch = get_git_branch()
network_type = NetworkType.none
network_strength = NetworkStrength.unknown
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML)
pandaState_prev = None
should_start_prev = False
handle_fan = None
is_uno = False
ui_running_prev = False
params = Params()
power_monitor = PowerMonitoring()
no_panda_cnt = 0
thermal_config = HARDWARE.get_thermal_config()
while 1:
pandaState = messaging.recv_sock(pandaState_sock, wait=True)
msg = read_thermal(thermal_config)
if pandaState is not None:
usb_power = pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client
# If we lose connection to the panda, wait 5 seconds before going offroad
if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown:
no_panda_cnt += 1
if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
if startup_conditions["ignition"]:
cloudlog.error("Lost panda connection while onroad")
startup_conditions["ignition"] = False
else:
no_panda_cnt = 0
startup_conditions[
"ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan
# Setup fan handler on first connect to panda
if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown:
is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno
if (not EON) or is_uno:
cloudlog.info("Setting up UNO fan handler")
handle_fan = handle_fan_uno
else:
cloudlog.info("Setting up EON fan handler")
setup_eon_fan()
handle_fan = handle_fan_eon
# Handle disconnect
if pandaState_prev is not None:
if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown and \
pandaState_prev.pandaState.pandaType != log.PandaState.PandaType.unknown:
params.panda_disconnect()
pandaState_prev = pandaState
# get_network_type is an expensive call. update every 10s
if (count % int(10. / DT_TRML)) == 0:
try:
network_type = HARDWARE.get_network_type()
network_strength = HARDWARE.get_network_strength(network_type)
except Exception:
cloudlog.exception("Error getting network status")
msg.deviceState.freeSpacePercent = get_available_percent(default=100.0)
msg.deviceState.memoryUsagePercent = int(
round(psutil.virtual_memory().percent))
msg.deviceState.cpuUsagePercent = int(round(psutil.cpu_percent()))
msg.deviceState.networkType = network_type
msg.deviceState.networkStrength = network_strength
msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity()
msg.deviceState.batteryStatus = HARDWARE.get_battery_status()
msg.deviceState.batteryCurrent = HARDWARE.get_battery_current()
msg.deviceState.batteryVoltage = HARDWARE.get_battery_voltage()
msg.deviceState.usbOnline = HARDWARE.get_usb_present()
# Fake battery levels on uno for frame
if (not EON) or is_uno:
msg.deviceState.batteryPercent = 100
msg.deviceState.batteryStatus = "Charging"
msg.deviceState.batteryTempC = 0
current_filter.update(msg.deviceState.batteryCurrent / 1e6)
# TODO: add car battery voltage check
max_cpu_temp = cpu_temp_filter.update(max(msg.deviceState.cpuTempC))
max_comp_temp = max(max_cpu_temp, msg.deviceState.memoryTempC,
max(msg.deviceState.gpuTempC))
bat_temp = msg.deviceState.batteryTempC
if handle_fan is not None:
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed,
startup_conditions["ignition"])
msg.deviceState.fanSpeedPercentDesired = fan_speed
# If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107
# We only do this if there is a relay that prevents the car from faulting
is_offroad_for_5_min = (started_ts is None) and (
(not started_seen) or (off_ts is None) or
(sec_since_boot() - off_ts > 60 * 5))
if max_cpu_temp > 107. or bat_temp >= 63. or (is_offroad_for_5_min
and max_cpu_temp > 70.0):
# onroad not allowed
thermal_status = ThermalStatus.danger
elif max_comp_temp > 96.0 or bat_temp > 60.:
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red,
ThermalStatus.danger)
elif max_cpu_temp > 94.0:
# hysteresis between engage not allowed and uploader not allowed
thermal_status = clip(thermal_status, ThermalStatus.yellow,
ThermalStatus.red)
elif max_cpu_temp > 80.0:
# uploader not allowed
thermal_status = ThermalStatus.yellow
elif max_cpu_temp > 75.0:
# hysteresis between uploader not allowed and all good
thermal_status = clip(thermal_status, ThermalStatus.green,
ThermalStatus.yellow)
else:
thermal_status = ThermalStatus.green # default to good condition
# **** starting logic ****
# Check for last update time and display alerts if needed
now = datetime.datetime.utcnow()
# show invalid date/time alert
startup_conditions["time_valid"] = (now.year > 2020) or (
now.year == 2020 and now.month >= 10)
set_offroad_alert_if_changed("Offroad_InvalidTime",
(not startup_conditions["time_valid"]))
# Show update prompt
try:
last_update = datetime.datetime.fromisoformat(
params.get("LastUpdateTime", encoding='utf8'))
except (TypeError, ValueError):
last_update = now
dt = now - last_update
update_failed_count = params.get("UpdateFailedCount")
update_failed_count = 0 if update_failed_count is None else int(
update_failed_count)
last_update_exception = params.get("LastUpdateException",
encoding='utf8')
if update_failed_count > 15 and last_update_exception is not None:
if current_branch in ["release2", "dashcam"]:
extra_text = "Ensure the software is correctly installed"
else:
extra_text = last_update_exception
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
set_offroad_alert_if_changed("Offroad_UpdateFailed",
True,
extra_text=extra_text)
elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True)
elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
True,
extra_text=f"{remaining_time} days.")
else:
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
startup_conditions["up_to_date"] = params.get(
"Offroad_ConnectivityNeeded") is None or params.get(
"DisableUpdates") == b"1"
startup_conditions["not_uninstalling"] = not params.get(
"DoUninstall") == b"1"
startup_conditions["accepted_terms"] = params.get(
"HasAcceptedTerms") == terms_version
panda_signature = params.get("PandaFirmware")
startup_conditions["fw_version_match"] = (panda_signature is None) or (
panda_signature == FW_SIGNATURE
) # don't show alert is no panda is connected (None)
set_offroad_alert_if_changed(
"Offroad_PandaFirmwareMismatch",
(not startup_conditions["fw_version_match"]))
# with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
(current_branch in ['dashcam', 'dashcam-staging'])
startup_conditions["not_driver_view"] = not params.get(
"IsDriverViewEnabled") == b"1"
startup_conditions["not_taking_snapshot"] = not params.get(
"IsTakingSnapshot") == b"1"
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
startup_conditions[
"device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed(
"Offroad_TemperatureTooHigh",
(not startup_conditions["device_temp_good"]))
startup_conditions[
"hardware_supported"] = pandaState is not None and pandaState.pandaState.pandaType not in [
log.PandaState.PandaType.whitePanda,
log.PandaState.PandaType.greyPanda
]
set_offroad_alert_if_changed(
"Offroad_HardwareUnsupported", pandaState is not None
and not startup_conditions["hardware_supported"])
# Handle offroad/onroad transition
should_start = all(startup_conditions.values())
if should_start:
if not should_start_prev:
params.delete("IsOffroad")
if TICI and DISABLE_LTE_ONROAD:
os.system("sudo systemctl stop --no-block lte")
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
else:
if startup_conditions["ignition"] and (startup_conditions !=
startup_conditions_prev):
cloudlog.event("Startup blocked",
startup_conditions=startup_conditions)
if should_start_prev or (count == 0):
params.put("IsOffroad", "1")
if TICI and DISABLE_LTE_ONROAD:
os.system("sudo systemctl start --no-block lte")
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
# Offroad power monitoring
power_monitor.calculate(pandaState)
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(
0, power_monitor.get_car_battery_capacity())
# Check if we need to disable charging (handled by boardd)
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(
pandaState, off_ts)
# Check if we need to shut down
if power_monitor.should_shutdown(pandaState, off_ts, started_seen,
LEON):
cloudlog.info(f"shutting device down, offroad since {off_ts}")
# TODO: add function for blocking cloudlog instead of sleep
time.sleep(10)
HARDWARE.shutdown()
# If UI has crashed, set the brightness to reasonable non-zero value
manager_state = messaging.recv_one_or_none(managerState_sock)
if manager_state is not None:
ui_running = "ui" in (p.name
for p in manager_state.managerState.processes
if p.running)
if ui_running_prev and not ui_running:
HARDWARE.set_screen_brightness(20)
ui_running_prev = ui_running
msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.deviceState.started = started_ts is not None
msg.deviceState.startedMonoTime = int(1e9 * (started_ts or 0))
msg.deviceState.thermalStatus = thermal_status
pm.send("deviceState", msg)
set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power))
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# report to server once per minute
if (count % int(60. / DT_TRML)) == 0:
location = messaging.recv_sock(location_sock)
cloudlog.event(
"STATUS_PACKET",
count=count,
pandaState=(pandaState.to_dict() if pandaState else None),
location=(location.gpsLocationExternal.to_dict()
if location else None),
deviceState=msg.to_dict())
count += 1
def radard_thread(sm=None, pm=None, can_sock=None):
set_realtime_priority(Priority.CTRL_LOW)
# 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.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'])
# *** publish radarState and liveTracks
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
# TODO: always log leads once we can hide them conditionally
enable_lead = CP.openpilotLongitudinalControl or not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
# This looks like a useless tesla hack. See if it can be removed?
if CP.carName == "tesla":
rr, rrext, ahbCarDetected = RI.update(can_strings, v_ego=0)
else:
rr = RI.update(can_strings)
if rr is None:
continue
sm.update(0)
dat = RD.update(rk.frame, sm, rr, enable_lead)
dat.radarState.cumLagMs = -rk.remaining * 1000.
pm.send('radarState', dat)
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message('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()
def main(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', 'carState'])
if pm is None:
pm = messaging.PubMaster(['liveParameters'])
params_reader = Params()
# wait for stats about the car to come in from controls
cloudlog.info("paramsd is waiting for CarParams")
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
cloudlog.info("paramsd got CarParams")
params = params_reader.get("LiveParameters")
# Check if car model matches
if params is not None:
params = json.loads(params)
if params.get('carFingerprint', None) != CP.carFingerprint:
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': CP.steerRatio,
'stiffnessFactor': 1.0,
'angleOffsetAverage': 0.0,
}
cloudlog.info("Parameter learner resetting to default values")
learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage']))
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
i = 0
while True:
sm.update()
for which, updated in sm.updated.items():
if not updated:
continue
t = sm.logMonoTime[which] * 1e-9
learner.handle_log(t, which, sm[which])
# TODO: set valid to false when locationd stops sending
# TODO: make sure controlsd knows when there is no gyro
if sm.updated['carState']:
msg = messaging.new_message('liveParameters')
msg.logMonoTime = sm.logMonoTime['carState']
msg.liveParameters.valid = True # TODO: Check if learned values are sane
msg.liveParameters.posenetValid = True
msg.liveParameters.sensorValid = True
x = learner.kf.x
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET])
msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(x[States.ANGLE_OFFSET_FAST])
msg.liveParameters.valid = all((
abs(msg.liveParameters.angleOffsetAverage) < 10.0,
abs(msg.liveParameters.angleOffset) < 10.0,
0.5 <= msg.liveParameters.stiffnessFactor <= 2.0,
min_sr <= msg.liveParameters.steerRatio <= max_sr,
))
i += 1
if i % 6000 == 0: # once a minute
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': msg.liveParameters.steerRatio,
'stiffnessFactor': msg.liveParameters.stiffnessFactor,
'angleOffsetAverage': msg.liveParameters.angleOffsetAverage,
}
put_nonblocking("LiveParameters", json.dumps(params))
pm.send('liveParameters', msg)
PRINT_DECIMATION = 100
from configparser import ConfigParser
cfg = ConfigParser()
cfg.read("./tweak.cfg")
STEER_RATIO = cfg.getfloat("steer", "STEER_RATIO", fallback=14)
STEER_SPEED_OFFSET = cfg.getfloat("steer", "STEER_SPEED_OFFSET", fallback=11)
BASE_MAX_STEER_ANGLE = cfg.getfloat(
"steer", "BASE_MAX_STEER_ANGLE",
fallback=38.4) # tweak to set base steer ratio/angle
STEER_SPEED_DENOM = cfg.getfloat("steer", "STEER_SPEED_DENOM", fallback=1.46)
print("tweak values: steer ratio:", STEER_RATIO, "steer speed offset:",
STEER_SPEED_OFFSET, "max steer angle:", BASE_MAX_STEER_ANGLE,
"steer speed denominator:", STEER_SPEED_DENOM)
pm = messaging.PubMaster(['frame', 'sensorEvents', 'can'])
sm = messaging.SubMaster(['carControl', 'controlsState'])
class VehicleState:
def __init__(self):
self.speed = 0
self.angle = 0
self.cruise_button = 0
self.is_engaged = False
self.left_blinker = False
self.right_blinker = False
def steer_rate_limit(old, new):
# if True: return new
def manager_thread():
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
params = Params()
ignore = []
if params.get("DongleId", encoding='utf8') == UNREGISTERED_DONGLE_ID:
ignore += ["manage_athenad", "uploader"]
if os.getenv("NOBOARD") is not None:
ignore.append("pandad")
if os.getenv("BLOCK") is not None:
ignore += os.getenv("BLOCK").split(",")
ensure_running(managed_processes.values(), started=False, not_run=ignore)
started_prev = False
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['managerState'])
while True:
sm.update()
not_run = ignore[:]
if sm['deviceState'].freeSpacePercent < 5:
not_run.append("loggerd")
started = sm['deviceState'].started
driverview = params.get_bool("IsDriverViewEnabled")
ensure_running(managed_processes.values(), started, driverview,
not_run)
# trigger an update after going offroad
if started_prev and not started and 'updated' in managed_processes:
os.sync()
managed_processes['updated'].signal(signal.SIGHUP)
started_prev = started
running = ' '.join(
"%s%s\u001b[0m" %
("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
for p in managed_processes.values() if p.proc)
print(running)
cloudlog.debug(running)
# send managerState
msg = messaging.new_message('managerState')
msg.managerState.processes = [
p.get_process_state_msg() for p in managed_processes.values()
]
pm.send('managerState', msg)
# Exit main loop when uninstall/shutdown/reboot is needed
shutdown = False
for param in ("DoUninstall", "DoShutdown", "DoReboot"):
if params.get_bool(param):
cloudlog.warning(f"Shutting down manager - {param} set")
shutdown = True
if shutdown:
break
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.camera_packets = ["roadCameraState", "driverCameraState"]
if TICI:
self.camera_packets.append("wideRoadCameraState")
params = Params()
self.joystick_mode = params.get_bool("JoystickDebugMode")
joystick_packet = ['testJoystick'] if self.joystick_mode else []
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'
] if SIMULATION else None
self.sm = messaging.SubMaster(
[
'deviceState', 'pandaStates', 'peripheralState', 'modelV2',
'liveCalibration', 'driverMonitoringState',
'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState'
] + self.camera_packets + joystick_packet,
ignore_alive=ignore,
ignore_avg_freq=['radarState', 'longitudinalPlan'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
if TICI:
self.log_sock = messaging.sub_sock('androidLog')
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
self.read_only = not car_recognized or not controller_available or self.CP.dashcamOnly
if self.read_only:
safety_config = car.CarParams.SafetyConfig.new_message()
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
self.CP.safetyConfigs = [safety_config]
# Write CarParams for radard
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP, self.CI)
self.initialized = False
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.cruise_mismatch_counter = 0
self.can_rcv_error_counter = 0
self.last_blinker_frame = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.button_timers = {
ButtonEvent.Type.decelCruise: 0,
ButtonEvent.Type.accelCruise: 0
}
self.last_actuators = car.CarControl.Actuators.new_message()
# TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized,
controller_available,
len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
if len(self.CP.carFw) > 0:
set_offroad_alert("Offroad_CarUnrecognized", True)
else:
set_offroad_alert("Offroad_NoFirmware", True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
elif self.joystick_mode:
self.events.add(EventName.joystickDebug, static=True)
self.startup_event = None
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState',
'carControl', 'carEvents', 'carParams'])
self.camera_packets = ["roadCameraState", "driverCameraState"]
if TICI:
self.camera_packets.append("wideRoadCameraState")
params = Params()
self.joystick_mode = params.get_bool("JoystickDebugMode")
joystick_packet = ['testJoystick'] if self.joystick_mode else []
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'] if SIMULATION else None
self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet,
ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
if TICI:
self.log_sock = messaging.sub_sock('androidLog')
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP, candidate = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
self.commIssue_ignored = params.get_bool("ComIssueGone")
self.auto_enabled = params.get_bool("AutoEnable") and params.get_bool("MadModeEnabled")
self.batt_less = params.get_bool("OpkrBattLess")
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature = self.CP.communityFeature or \
self.CP.fingerprintSource == car.CarParams.FingerprintSource.can
community_feature_disallowed = community_feature and (not community_feature_toggle)
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb, candidate)
self.VM = VehicleModel(self.CP)
self.lateral_control_method = 0
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
self.lateral_control_method = 3
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
self.lateral_control_method = 0
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
self.lateral_control_method = 1
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.lateral_control_method = 2
self.controlsAllowed = False
self.initialized = False
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.v_target = 0.0
self.a_target = 0.0
# TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized, controller_available, self.CP.fuzzyFingerprint,
len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
#elif self.read_only:
# self.events.add(EventName.dashcamMode, static=True)
elif self.joystick_mode:
self.events.add(EventName.joystickDebug, static=True)
self.startup_event = None
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
self.hyundai_lkas = self.read_only #read_only
self.mpc_frame = 0
self.mpc_frame_sr = 0
self.steerRatio_Max = float(Decimal(params.get("SteerRatioMaxAdj", encoding="utf8")) * Decimal('0.1'))
self.steer_angle_range = [5, 30]
self.steerRatio_range = [self.CP.steerRatio, self.steerRatio_Max]
self.new_steerRatio = self.CP.steerRatio
self.new_steerRatio_prev = self.CP.steerRatio
self.steerRatio_to_send = 0
self.live_sr = params.get_bool("OpkrLiveSteerRatio")
self.second = 0.0
self.map_enabled = False
self.lane_change_delay = int(Params().get("OpkrAutoLaneChangeDelay", encoding="utf8"))
def main(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', 'carState'],
poll=['liveLocationKalman'])
if pm is None:
pm = messaging.PubMaster(['liveParameters'])
params_reader = Params()
# wait for stats about the car to come in from controls
cloudlog.info("paramsd is waiting for CarParams")
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
cloudlog.info("paramsd got CarParams")
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
params = params_reader.get("LiveParameters")
# Check if car model matches
if params is not None:
params = json.loads(params)
if params.get('carFingerprint', None) != CP.carFingerprint:
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
if (params is not None) and not all(
(abs(params['angleOffsetAverage']) < 10.0,
min_sr <= params['steerRatio'] <= max_sr)):
cloudlog.info(f"Invalid starting values found {params}")
params = None
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': CP.steerRatio,
'stiffnessFactor': 1.0,
'angleOffsetAverage': 0.0,
}
cloudlog.info("Parameter learner resetting to default values")
# When driving in wet conditions the stiffness can go down, and then be too low on the next drive
# Without a way to detect this we have to reset the stiffness every drive
#params['stiffnessFactor'] = 1.0
params['stiffnessFactor'] = int(Params().get("TireStiffnessFactorAdj",
encoding='utf8')) * 0.01
learner = ParamsLearner(CP, params['steerRatio'],
params['stiffnessFactor'],
math.radians(params['angleOffsetAverage']))
while True:
sm.update()
for which, updated in sm.updated.items():
if updated:
t = sm.logMonoTime[which] * 1e-9
learner.handle_log(t, which, sm[which])
if sm.updated['liveLocationKalman']:
msg = messaging.new_message('liveParameters')
msg.logMonoTime = sm.logMonoTime['carState']
msg.liveParameters.posenetValid = True
msg.liveParameters.sensorValid = True
x = learner.kf.x
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.angleOffsetAverage = math.degrees(
x[States.ANGLE_OFFSET])
msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(
x[States.ANGLE_OFFSET_FAST])
msg.liveParameters.valid = all((
abs(msg.liveParameters.angleOffsetAverage) < 10.0,
abs(msg.liveParameters.angleOffset) < 10.0,
0.2 <= msg.liveParameters.stiffnessFactor <= 5.0,
min_sr <= msg.liveParameters.steerRatio <= max_sr,
))
if sm.frame % 1200 == 0: # once a minute
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': msg.liveParameters.steerRatio,
'stiffnessFactor': msg.liveParameters.stiffnessFactor,
'angleOffsetAverage':
msg.liveParameters.angleOffsetAverage,
}
put_nonblocking("LiveParameters", json.dumps(params))
pm.send('liveParameters', msg)
sc = messaging.drain_sock_raw(sendcan)
cp.update_strings(sc, sendcan=True)
if cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
brake = cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248
else:
brake = 0.0
if cp.vl[0x200]['GAS_COMMAND'] > 0:
gas = cp.vl[0x200]['GAS_COMMAND'] / 256.0
else:
gas = 0.0
if cp.vl[0xe4]['STEER_TORQUE_REQUEST']:
steer_torque = cp.vl[0xe4]['STEER_TORQUE'] * 1.0 / 0x1000
else:
steer_torque = 0.0
return (gas, brake, steer_torque)
if __name__ == "__main__":
pm = messaging.PubMaster(['can'])
sendcan = messaging.sub_sock('sendcan')
idx = 0
while 1:
sendcan_function(sendcan)
can_function(pm, 10.0, idx)
time.sleep(0.01)
idx += 1
from cereal.visionipc import VisionIpcServer, VisionStreamType
from common.basedir import BASEDIR
from common.numpy_fast import clip
from common.params import Params
from common.realtime import DT_DMON, Ratekeeper
from selfdrive.car.honda.values import CruiseButtons
from selfdrive.test.helpers import set_params_enabled
from tools.sim.lib.can import can_function
W, H = 1928, 1208
REPEAT_COUNTER = 5
PRINT_DECIMATION = 100
STEER_RATIO = 15.
pm = messaging.PubMaster([
'roadCameraState', 'wideRoadCameraState', 'sensorEvents', 'can',
"gpsLocationExternal"
])
sm = messaging.SubMaster(['carControl', 'controlsState'])
def parse_args(add_args=None):
parser = argparse.ArgumentParser(
description='Bridge between CARLA and openpilot.')
parser.add_argument('--joystick', action='store_true')
parser.add_argument('--high_quality', action='store_true')
parser.add_argument('--dual_camera', action='store_true')
parser.add_argument('--town', type=str, default='Town04_Opt')
parser.add_argument('--spawn_point',
dest='num_selected_spawn_point',
type=int,
default=16)
def dmonitoringd_thread(sm=None, pm=None):
gc.disable()
set_realtime_priority(2)
if pm is None:
pm = messaging.PubMaster(['driverMonitoringState'])
if sm is None:
sm = messaging.SubMaster([
'driverStateV2', 'liveCalibration', 'carState', 'controlsState',
'modelV2'
],
poll=['driverStateV2'])
driver_status = DriverStatus(rhd_saved=Params().get_bool("IsRhdDetected"))
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['liveCalibration'].rpyCalib = [0, 0, 0]
sm['carState'].buttonEvents = []
sm['carState'].standstill = True
v_cruise_last = 0
driver_engaged = False
# 10Hz <- dmonitoringmodeld
while True:
sm.update()
if not sm.updated['driverStateV2']:
continue
# Get interaction
if sm.updated['carState']:
v_cruise = sm['carState'].cruiseState.speed
driver_engaged = len(sm['carState'].buttonEvents) > 0 or \
v_cruise != v_cruise_last or \
sm['carState'].steeringPressed or \
sm['carState'].gasPressed
v_cruise_last = v_cruise
if sm.updated['modelV2']:
driver_status.set_policy(sm['modelV2'], sm['carState'].vEgo)
# Get data from dmonitoringmodeld
events = Events()
driver_status.update_states(sm['driverStateV2'],
sm['liveCalibration'].rpyCalib,
sm['carState'].vEgo,
sm['controlsState'].enabled)
# Block engaging after max number of distrations
if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \
driver_status.terminal_time >= driver_status.settings._MAX_TERMINAL_DURATION:
events.add(car.CarEvent.EventName.tooDistracted)
# Update events from driver state
driver_status.update_events(events, driver_engaged,
sm['controlsState'].enabled,
sm['carState'].standstill)
# build driverMonitoringState packet
dat = messaging.new_message('driverMonitoringState')
dat.driverMonitoringState = {
"events":
events.to_msg(),
"faceDetected":
driver_status.face_detected,
"isDistracted":
driver_status.driver_distracted,
"distractedType":
sum(driver_status.distracted_types),
"awarenessStatus":
driver_status.awareness,
"posePitchOffset":
driver_status.pose.pitch_offseter.filtered_stat.mean(),
"posePitchValidCount":
driver_status.pose.pitch_offseter.filtered_stat.n,
"poseYawOffset":
driver_status.pose.yaw_offseter.filtered_stat.mean(),
"poseYawValidCount":
driver_status.pose.yaw_offseter.filtered_stat.n,
"stepChange":
driver_status.step_change,
"awarenessActive":
driver_status.awareness_active,
"awarenessPassive":
driver_status.awareness_passive,
"isLowStd":
driver_status.pose.low_std,
"hiStdCount":
driver_status.hi_stds,
"isActiveMode":
driver_status.active_monitoring_mode,
"isRHD":
driver_status.wheel_on_right,
}
pm.send('driverMonitoringState', dat)
# save rhd virtual toggle every 5 mins
if (sm['driverStateV2'].frameId % 6000 == 0
and driver_status.wheelpos_learner.filtered_stat.n >
driver_status.settings._WHEELPOS_FILTER_MIN_COUNT
and driver_status.wheel_on_right
== (driver_status.wheelpos_learner.filtered_stat.M >
driver_status.settings._WHEELPOS_THRESHOLD)):
put_bool_nonblocking("IsRhdDetected", driver_status.wheel_on_right)
def __init__(self, threadID, name, sharedParams={}):
# invoke parent constructor
LoggerThread.__init__(self, threadID, name)
self.sharedParams = sharedParams
self.pm = messaging.PubMaster(['liveMapData'])
self.logger.debug("entered mapsd_thread, ... %s" % (str(self.pm)))
def manager_thread():
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
# save boot log
subprocess.call("./bootlog",
cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
# start daemon processes
for p in daemon_processes:
start_daemon_process(p)
# start persistent processes
for p in persistent_processes:
start_managed_process(p)
# start offroad
if EON:
pm_apply_packages('enable')
start_offroad()
if os.getenv("NOBOARD") is not None:
del managed_processes["pandad"]
if os.getenv("BLOCK") is not None:
for k in os.getenv("BLOCK").split(","):
del managed_processes[k]
started_prev = False
logger_dead = False
params = Params()
device_state_sock = messaging.sub_sock('deviceState')
pm = messaging.PubMaster(['managerState'])
while 1:
msg = messaging.recv_sock(device_state_sock, wait=True)
if msg.deviceState.freeSpacePercent < 0.05:
logger_dead = True
if msg.deviceState.started:
for p in car_started_processes:
if p == "loggerd" and logger_dead:
kill_managed_process(p)
else:
start_managed_process(p)
else:
logger_dead = False
driver_view = params.get("IsDriverViewEnabled") == b"1"
# TODO: refactor how manager manages processes
for p in reversed(car_started_processes):
if p not in driver_view_processes or not driver_view:
kill_managed_process(p)
for p in driver_view_processes:
if driver_view:
start_managed_process(p)
else:
kill_managed_process(p)
# trigger an update after going offroad
if started_prev:
os.sync()
send_managed_process_signal("updated", signal.SIGHUP)
started_prev = msg.deviceState.started
# check the status of all processes, did any of them die?
running_list = [
"%s%s\u001b[0m" %
("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p)
for p in running
]
cloudlog.debug(' '.join(running_list))
# send managerState
states = []
for p in managed_processes:
state = log.ManagerState.ProcessState.new_message()
state.name = p
if p in running:
state.running = running[p].is_alive()
state.pid = running[p].pid
state.exitCode = running[p].exitcode or 0
states.append(state)
msg = messaging.new_message('managerState')
msg.managerState.processes = states
pm.send('managerState', msg)
# Exit main loop when uninstall is needed
if params.get("DoUninstall", encoding='utf8') == "1":
break
def main(sm=None, pm=None):
gc.disable()
set_realtime_priority(5)
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', 'carState'],
poll=['liveLocationKalman'])
if pm is None:
pm = messaging.PubMaster(['liveParameters'])
params_reader = Params()
# wait for stats about the car to come in from controls
cloudlog.info("paramsd is waiting for CarParams")
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
cloudlog.info("paramsd got CarParams")
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
params = params_reader.get("LiveParameters")
# Check if car model matches
if params is not None:
params = json.loads(params)
if params.get('carFingerprint', None) != CP.carFingerprint:
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
# Check if starting values are sane
if params is not None:
try:
angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0
steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
params_sane = angle_offset_sane and steer_ratio_sane
if not params_sane:
cloudlog.info(f"Invalid starting values found {params}")
params = None
except Exception as e:
cloudlog.info(f"Error reading params {params}: {str(e)}")
params = None
# TODO: cache the params with the capnp struct
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': CP.steerRatio,
'stiffnessFactor': 1.0,
'angleOffsetAverageDeg': 0.0,
}
cloudlog.info("Parameter learner resetting to default values")
# When driving in wet conditions the stiffness can go down, and then be too low on the next drive
# Without a way to detect this we have to reset the stiffness every drive
params['stiffnessFactor'] = 1.0
learner = ParamsLearner(CP, params['steerRatio'],
params['stiffnessFactor'],
math.radians(params['angleOffsetAverageDeg']))
angle_offset_average = params['angleOffsetAverageDeg']
angle_offset = angle_offset_average
while True:
sm.update()
for which, updated in sm.updated.items():
if updated:
t = sm.logMonoTime[which] * 1e-9
learner.handle_log(t, which, sm[which])
if sm.updated['liveLocationKalman']:
x = learner.kf.x
if not all(map(math.isfinite, x)):
cloudlog.error(
"NaN in liveParameters estimate. Resetting to default values"
)
learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
x = learner.kf.x
angle_offset_average = clip(
math.degrees(x[States.ANGLE_OFFSET]),
angle_offset_average - MAX_ANGLE_OFFSET_DELTA,
angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
angle_offset = clip(
math.degrees(x[States.ANGLE_OFFSET] +
x[States.ANGLE_OFFSET_FAST]),
angle_offset - MAX_ANGLE_OFFSET_DELTA,
angle_offset + MAX_ANGLE_OFFSET_DELTA)
msg = messaging.new_message('liveParameters')
msg.logMonoTime = sm.logMonoTime['carState']
msg.liveParameters.posenetValid = True
msg.liveParameters.sensorValid = True
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.angleOffsetAverageDeg = angle_offset_average
msg.liveParameters.angleOffsetDeg = angle_offset
msg.liveParameters.valid = all((
abs(msg.liveParameters.angleOffsetAverageDeg) < 10.0,
abs(msg.liveParameters.angleOffsetDeg) < 10.0,
0.2 <= msg.liveParameters.stiffnessFactor <= 5.0,
min_sr <= msg.liveParameters.steerRatio <= max_sr,
))
if sm.frame % 1200 == 0: # once a minute
params = {
'carFingerprint':
CP.carFingerprint,
'steerRatio':
msg.liveParameters.steerRatio,
'stiffnessFactor':
msg.liveParameters.stiffnessFactor,
'angleOffsetAverageDeg':
msg.liveParameters.angleOffsetAverageDeg,
}
put_nonblocking("LiveParameters", json.dumps(params))
pm.send('liveParameters', msg)
def thermald_thread():
pm = messaging.PubMaster(['deviceState'])
pandaState_timeout = int(1000 * 2.5 *
DT_TRML) # 2.5x the expected pandaState frequency
pandaState_sock = messaging.sub_sock('pandaState',
timeout=pandaState_timeout)
location_sock = messaging.sub_sock('gpsLocationExternal')
managerState_sock = messaging.sub_sock('managerState', conflate=True)
fan_speed = 0
count = 0
startup_conditions = {
"ignition": False,
}
startup_conditions_prev = startup_conditions.copy()
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
usb_power = True
network_type = NetworkType.none
network_strength = NetworkStrength.unknown
network_info = None
modem_version = None
registered_count = 0
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML)
pandaState_prev = None
should_start_prev = False
handle_fan = None
is_uno = False
ui_running_prev = False
params = Params()
power_monitor = PowerMonitoring()
no_panda_cnt = 0
HARDWARE.initialize_hardware()
thermal_config = HARDWARE.get_thermal_config()
# TODO: use PI controller for UNO
controller = PIController(k_p=0,
k_i=2e-3,
neg_limit=-80,
pos_limit=0,
rate=(1 / DT_TRML))
if params.get_bool("IsOnroad"):
cloudlog.event("onroad flag not cleared")
# CPR3 logging
if EON:
base_path = "/sys/kernel/debug/cpr3-regulator/"
cpr_files = [p for p in Path(base_path).glob("**/*") if p.is_file()]
cpr_files = ["/sys/kernel/debug/regulator/pm8994_s11/voltage"
] + cpr_files
cpr_data = {}
for cf in cpr_files:
with open(cf, "r") as f:
try:
cpr_data[str(cf)] = f.read().strip()
except Exception:
pass
cloudlog.event("CPR", data=cpr_data)
while 1:
pandaState = messaging.recv_sock(pandaState_sock, wait=True)
msg = read_thermal(thermal_config)
if pandaState is not None:
usb_power = pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client
# If we lose connection to the panda, wait 5 seconds before going offroad
if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown:
no_panda_cnt += 1
if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
if startup_conditions["ignition"]:
cloudlog.error("Lost panda connection while onroad")
startup_conditions["ignition"] = False
else:
no_panda_cnt = 0
startup_conditions[
"ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan
startup_conditions[
"hardware_supported"] = pandaState.pandaState.pandaType not in [
log.PandaState.PandaType.whitePanda,
log.PandaState.PandaType.greyPanda
]
set_offroad_alert_if_changed(
"Offroad_HardwareUnsupported",
not startup_conditions["hardware_supported"])
# Setup fan handler on first connect to panda
if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown:
is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno
if TICI:
cloudlog.info("Setting up TICI fan handler")
handle_fan = handle_fan_tici
elif is_uno or PC:
cloudlog.info("Setting up UNO fan handler")
handle_fan = handle_fan_uno
else:
cloudlog.info("Setting up EON fan handler")
setup_eon_fan()
handle_fan = handle_fan_eon
# Handle disconnect
if pandaState_prev is not None:
if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown and \
pandaState_prev.pandaState.pandaType != log.PandaState.PandaType.unknown:
params.clear_all(ParamKeyType.CLEAR_ON_PANDA_DISCONNECT)
pandaState_prev = pandaState
# get_network_type is an expensive call. update every 10s
if (count % int(10. / DT_TRML)) == 0:
try:
network_type = HARDWARE.get_network_type()
network_strength = HARDWARE.get_network_strength(network_type)
network_info = HARDWARE.get_network_info() # pylint: disable=assignment-from-none
# Log modem version once
if modem_version is None:
modem_version = HARDWARE.get_modem_version() # pylint: disable=assignment-from-none
if modem_version is not None:
cloudlog.warning(f"Modem version: {modem_version}")
if TICI and (network_info.get('state', None) == "REGISTERED"):
registered_count += 1
else:
registered_count = 0
if registered_count > 10:
cloudlog.warning(
f"Modem stuck in registered state {network_info}. nmcli conn up lte"
)
os.system("nmcli conn up lte")
registered_count = 0
except Exception:
cloudlog.exception("Error getting network status")
msg.deviceState.freeSpacePercent = get_available_percent(default=100.0)
msg.deviceState.memoryUsagePercent = int(
round(psutil.virtual_memory().percent))
msg.deviceState.cpuUsagePercent = [
int(round(n)) for n in psutil.cpu_percent(percpu=True)
]
msg.deviceState.gpuUsagePercent = int(
round(HARDWARE.get_gpu_usage_percent()))
msg.deviceState.networkType = network_type
msg.deviceState.networkStrength = network_strength
if network_info is not None:
msg.deviceState.networkInfo = network_info
msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity()
msg.deviceState.batteryCurrent = HARDWARE.get_battery_current()
msg.deviceState.usbOnline = HARDWARE.get_usb_present()
current_filter.update(msg.deviceState.batteryCurrent / 1e6)
max_comp_temp = temp_filter.update(
max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC,
max(msg.deviceState.gpuTempC)))
if handle_fan is not None:
fan_speed = handle_fan(controller, max_comp_temp, fan_speed,
startup_conditions["ignition"])
msg.deviceState.fanSpeedPercentDesired = fan_speed
is_offroad_for_5_min = (started_ts is None) and (
(not started_seen) or (off_ts is None) or
(sec_since_boot() - off_ts > 60 * 5))
if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP:
# If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107
thermal_status = ThermalStatus.danger
else:
current_band = THERMAL_BANDS[thermal_status]
band_idx = list(THERMAL_BANDS.keys()).index(thermal_status)
if current_band.min_temp is not None and max_comp_temp < current_band.min_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1]
elif current_band.max_temp is not None and max_comp_temp > current_band.max_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1]
# **** starting logic ****
# Check for last update time and display alerts if needed
now = datetime.datetime.utcnow()
# show invalid date/time alert
startup_conditions["time_valid"] = (now.year > 2020) or (
now.year == 2020 and now.month >= 10)
set_offroad_alert_if_changed("Offroad_InvalidTime",
(not startup_conditions["time_valid"]))
# Show update prompt
try:
last_update = datetime.datetime.fromisoformat(
params.get("LastUpdateTime", encoding='utf8'))
except (TypeError, ValueError):
last_update = now
dt = now - last_update
update_failed_count = params.get("UpdateFailedCount")
update_failed_count = 0 if update_failed_count is None else int(
update_failed_count)
last_update_exception = params.get("LastUpdateException",
encoding='utf8')
if update_failed_count > 15 and last_update_exception is not None:
if tested_branch:
extra_text = "Ensure the software is correctly installed"
else:
extra_text = last_update_exception
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
set_offroad_alert_if_changed("Offroad_UpdateFailed",
True,
extra_text=extra_text)
elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True)
elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
True,
extra_text=f"{remaining_time} days.")
else:
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
startup_conditions["up_to_date"] = params.get(
"Offroad_ConnectivityNeeded") is None or params.get_bool(
"DisableUpdates")
startup_conditions["not_uninstalling"] = not params.get_bool(
"DoUninstall")
startup_conditions["accepted_terms"] = params.get(
"HasAcceptedTerms") == terms_version
panda_signature = params.get("PandaFirmware")
startup_conditions["fw_version_match"] = (panda_signature is None) or (
panda_signature == FW_SIGNATURE
) # don't show alert is no panda is connected (None)
set_offroad_alert_if_changed(
"Offroad_PandaFirmwareMismatch",
(not startup_conditions["fw_version_match"]))
# with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
params.get_bool("Passive")
startup_conditions["not_driver_view"] = not params.get_bool(
"IsDriverViewEnabled")
startup_conditions["not_taking_snapshot"] = not params.get_bool(
"IsTakingSnapshot")
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
startup_conditions[
"device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed(
"Offroad_TemperatureTooHigh",
(not startup_conditions["device_temp_good"]))
if TICI:
set_offroad_alert_if_changed("Offroad_NvmeMissing",
(not Path("/data/media").is_mount()))
# Handle offroad/onroad transition
should_start = all(startup_conditions.values())
if should_start != should_start_prev or (count == 0):
params.put_bool("IsOnroad", should_start)
params.put_bool("IsOffroad", not should_start)
HARDWARE.set_power_save(not should_start)
if should_start:
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
else:
if startup_conditions["ignition"] and (startup_conditions !=
startup_conditions_prev):
cloudlog.event("Startup blocked",
startup_conditions=startup_conditions)
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
# Offroad power monitoring
power_monitor.calculate(pandaState)
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(
0, power_monitor.get_car_battery_capacity())
# Check if we need to disable charging (handled by boardd)
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(
pandaState, off_ts)
# Check if we need to shut down
if power_monitor.should_shutdown(pandaState, off_ts, started_seen):
cloudlog.info(f"shutting device down, offroad since {off_ts}")
# TODO: add function for blocking cloudlog instead of sleep
time.sleep(10)
HARDWARE.shutdown()
# If UI has crashed, set the brightness to reasonable non-zero value
manager_state = messaging.recv_one_or_none(managerState_sock)
if manager_state is not None:
ui_running = "ui" in (p.name
for p in manager_state.managerState.processes
if p.running)
if ui_running_prev and not ui_running:
HARDWARE.set_screen_brightness(20)
ui_running_prev = ui_running
msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.deviceState.started = started_ts is not None
msg.deviceState.startedMonoTime = int(1e9 * (started_ts or 0))
last_ping = params.get("LastAthenaPingTime")
if last_ping is not None:
msg.deviceState.lastAthenaPingTime = int(last_ping)
msg.deviceState.thermalStatus = thermal_status
pm.send("deviceState", msg)
if EON and not is_uno:
set_offroad_alert_if_changed("Offroad_ChargeDisabled",
(not usb_power))
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# report to server once every 10 minutes
if (count % int(600. / DT_TRML)) == 0:
if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40:
cloudlog.event("High offroad memory usage",
mem=msg.deviceState.memoryUsagePercent)
location = messaging.recv_sock(location_sock)
cloudlog.event("STATUS_PACKET",
count=count,
pandaState=(strip_deprecated_keys(
pandaState.to_dict()) if pandaState else None),
location=(strip_deprecated_keys(
location.gpsLocationExternal.to_dict())
if location else None),
deviceState=strip_deprecated_keys(msg.to_dict()))
count += 1
def model_replay(lr, frs):
spinner = Spinner()
spinner.update("starting model replay")
vipc_server = VisionIpcServer("camerad")
vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *(tici_f_frame_size if TICI else eon_f_frame_size))
vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *(tici_d_frame_size if TICI else eon_d_frame_size))
vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *(tici_f_frame_size))
vipc_server.start_listener()
sm = messaging.SubMaster(['modelV2', 'driverState'])
pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan'])
try:
managed_processes['modeld'].start()
managed_processes['dmonitoringmodeld'].start()
time.sleep(5)
sm.update(1000)
log_msgs = []
last_desire = None
recv_cnt = defaultdict(lambda: 0)
frame_idxs = defaultdict(lambda: 0)
# init modeld with valid calibration
cal_msgs = [msg for msg in lr if msg.which() == "liveCalibration"]
for _ in range(5):
pm.send(cal_msgs[0].which(), cal_msgs[0].as_builder())
time.sleep(0.1)
msgs = defaultdict(list)
for msg in lr:
msgs[msg.which()].append(msg)
for cam_msgs in zip_longest(msgs['roadCameraState'], msgs['wideRoadCameraState'], msgs['driverCameraState']):
# need a pair of road/wide msgs
if TICI and None in (cam_msgs[0], cam_msgs[1]):
break
for msg in cam_msgs:
if msg is None:
continue
if SEND_EXTRA_INPUTS:
if msg.which() == "liveCalibration":
last_calib = list(msg.liveCalibration.rpyCalib)
pm.send(msg.which(), replace_calib(msg, last_calib))
elif msg.which() == "lateralPlan":
last_desire = msg.lateralPlan.desire
dat = messaging.new_message('lateralPlan')
dat.lateralPlan.desire = last_desire
pm.send('lateralPlan', dat)
if msg.which() in VIPC_STREAM:
msg = msg.as_builder()
camera_state = getattr(msg, msg.which())
img = frs[msg.which()].get(frame_idxs[msg.which()], pix_fmt="yuv420p")[0]
frame_idxs[msg.which()] += 1
# send camera state and frame
camera_state.frameId = frame_idxs[msg.which()]
pm.send(msg.which(), msg)
vipc_server.send(VIPC_STREAM[msg.which()], img.flatten().tobytes(), camera_state.frameId,
camera_state.timestampSof, camera_state.timestampEof)
recv = None
if msg.which() in ('roadCameraState', 'wideRoadCameraState'):
if not TICI or min(frame_idxs['roadCameraState'], frame_idxs['wideRoadCameraState']) > recv_cnt['modelV2']:
recv = "modelV2"
elif msg.which() == 'driverCameraState':
recv = "driverState"
# wait for a response
with Timeout(15, f"timed out waiting for {recv}"):
if recv:
recv_cnt[recv] += 1
log_msgs.append(messaging.recv_one(sm.sock[recv]))
spinner.update("replaying models: road %d/%d, driver %d/%d" % (frame_idxs['roadCameraState'],
frs['roadCameraState'].frame_count, frame_idxs['driverCameraState'], frs['driverCameraState'].frame_count))
if any(frame_idxs[c] >= frs[c].frame_count for c in frame_idxs.keys()):
break
finally:
spinner.close()
managed_processes['modeld'].stop()
managed_processes['dmonitoringmodeld'].stop()
return log_msgs
def dmonitoringd_thread(sm=None, pm=None):
if pm is None:
pm = messaging.PubMaster(['dMonitoringState'])
if sm is None:
sm = messaging.SubMaster([
'driverState', 'liveCalibration', 'carState', 'controlsState',
'model'
],
poll=['driverState'])
driver_status = DriverStatus()
driver_status.is_rhd_region = Params().get("IsRHD") == b"1"
offroad = Params().get("IsOffroad") == b"1"
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['liveCalibration'].rpyCalib = [0, 0, 0]
sm['carState'].vEgo = 0.
sm['carState'].cruiseState.speed = 0.
sm['carState'].buttonEvents = []
sm['carState'].steeringPressed = False
sm['carState'].gasPressed = False
sm['carState'].standstill = True
v_cruise_last = 0
driver_engaged = False
# 10Hz <- dmonitoringmodeld
standstill = True
while True:
sm.update()
if not sm.updated['driverState']:
continue
# Get interaction
if sm.updated['carState']:
steeringPressed = sm['carState'].steeringPressed
vEgo = sm['carState'].vEgo
v_cruise = sm['carState'].cruiseState.speed
driver_engaged = len(sm['carState'].buttonEvents) > 0 or \
v_cruise != v_cruise_last or \
sm['carState'].steeringPressed or \
sm['carState'].gasPressed
standstill = sm[
'carState'].standstill or steeringPressed #or vEgo < 1
if driver_engaged:
driver_status.update(Events(), True,
sm['controlsState'].enabled, standstill)
v_cruise_last = v_cruise
if sm.updated['model']:
driver_status.set_policy(sm['model'])
# Get data from dmonitoringmodeld
events = Events()
driver_status.get_pose(sm['driverState'],
sm['liveCalibration'].rpyCalib,
sm['carState'].vEgo,
sm['controlsState'].enabled)
# Block engaging after max number of distrations
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
events.add(car.CarEvent.EventName.tooDistracted)
# Update events from driver state
driver_status.update(events, driver_engaged,
sm['controlsState'].enabled, standstill)
# build dMonitoringState packet
dat = messaging.new_message('dMonitoringState')
dat.dMonitoringState = {
"events":
events.to_msg(),
"faceDetected":
driver_status.face_detected,
"isDistracted":
driver_status.driver_distracted,
"awarenessStatus":
driver_status.awareness,
"isRHD":
driver_status.is_rhd_region,
"posePitchOffset":
driver_status.pose.pitch_offseter.filtered_stat.mean(),
"posePitchValidCount":
driver_status.pose.pitch_offseter.filtered_stat.n,
"poseYawOffset":
driver_status.pose.yaw_offseter.filtered_stat.mean(),
"poseYawValidCount":
driver_status.pose.yaw_offseter.filtered_stat.n,
"stepChange":
driver_status.step_change,
"awarenessActive":
driver_status.awareness_active,
"awarenessPassive":
driver_status.awareness_passive,
"isLowStd":
driver_status.pose.low_std,
"hiStdCount":
driver_status.hi_stds,
"isPreview":
offroad,
}
pm.send('dMonitoringState', dat)
def __init__(self, sm=None, pm=None, can_sock=None):
gc.disable()
set_realtime_priority(3)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', \
'carControl', 'carEvents', 'carParams'])
self.sm = sm
if self.sm is None:
self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', \
'dMonitoringState', 'plan', 'pathPlan'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one health and one CAN packet
hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay)
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None
community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1" or \
internet_needed or not openpilot_enabled_toggle
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
put_nonblocking("LongitudinalControl", "1" if self.CP.openpilotLongitudinalControl else "0")
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.events_prev = ""
self.sm['liveCalibration'].calStatus = Calibration.INVALID
self.sm['pathPlan'].sensorValid = True
self.sm['pathPlan'].posenetValid = True
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False
startup_alert = get_startup_alert(car_recognized, controller_available)
self.AM.add(self.sm.frame, startup_alert, False)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
# detect sound card presence and ensure successful init
sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') \
and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
self.static_events = []
if not sounds_available:
self.static_events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
if internet_needed:
self.static_events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT]))
if community_feature_disallowed:
self.static_events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT]))
if self.read_only and not passive:
self.static_events.append(create_event('carUnrecognized', [ET.PERMANENT]))
def thermald_thread():
pm = messaging.PubMaster(['deviceState'])
pandaState_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency
pandaState_sock = messaging.sub_sock('pandaStates', timeout=pandaState_timeout)
sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "managerState"])
fan_speed = 0
count = 0
startup_conditions = {
"ignition": False,
}
startup_conditions_prev = startup_conditions.copy()
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
usb_power = True
network_type = NetworkType.none
network_strength = NetworkStrength.unknown
network_info = None
modem_version = None
registered_count = 0
nvme_temps = None
modem_temps = None
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML)
pandaState_prev = None
should_start_prev = False
in_car = False
handle_fan = None
is_uno = False
ui_running_prev = False
power_monitor = PowerMonitoring()
no_panda_cnt = 0
HARDWARE.initialize_hardware()
thermal_config = HARDWARE.get_thermal_config()
# TODO: use PI controller for UNO
controller = PIController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))
# Leave flag for loggerd to indicate device was left onroad
if params.get_bool("IsOnroad"):
params.put_bool("BootedOnroad", True)
# dp
peripheralStateLast = None
dp_no_batt = params.get_bool("dp_no_batt")
dp_temp_monitor = True
dp_last_modified_temp_monitor = None
dp_auto_shutdown = False
dp_last_modified_auto_shutdown = None
dp_auto_shutdown_in = 90
dp_last_modified_auto_shutdown_in = None
dp_fan_mode = 0
dp_fan_mode_last = None
modified = None
last_modified = None
last_modified_check = None
dp_no_offroad_fix = params.get_bool('dp_no_offroad_fix')
if JETSON:
handle_fan = handle_fan_jetson
while True:
# dp - load temp monitor conf
last_modified_check, modified = get_last_modified(LAST_MODIFIED_THERMALD, last_modified_check, modified)
if last_modified != modified:
dp_temp_monitor, dp_last_modified_temp_monitor = param_get_if_updated("dp_temp_monitor", "bool", dp_temp_monitor, dp_last_modified_temp_monitor)
dp_auto_shutdown, dp_last_modified_auto_shutdown = param_get_if_updated("dp_auto_shutdown", "bool", dp_auto_shutdown, dp_last_modified_auto_shutdown)
dp_auto_shutdown_in, dp_last_modified_auto_shutdown_in = param_get_if_updated("dp_auto_shutdown_in", "int", dp_auto_shutdown_in, dp_last_modified_auto_shutdown_in)
dp_fan_mode, dp_fan_mode_last = param_get_if_updated("dp_fan_mode", "int", dp_fan_mode, dp_fan_mode_last)
last_modified = modified
pandaStates = messaging.recv_sock(pandaState_sock, wait=True)
sm.update(0)
peripheralState = sm['peripheralState']
msg = read_thermal(thermal_config)
if pandaStates is not None and len(pandaStates.pandaStates) > 0:
pandaState = pandaStates.pandaStates[0]
# If we lose connection to the panda, wait 5 seconds before going offroad
if pandaState.pandaType == log.PandaState.PandaType.unknown:
no_panda_cnt += 1
if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
if startup_conditions["ignition"]:
cloudlog.error("Lost panda connection while onroad")
startup_conditions["ignition"] = False
else:
no_panda_cnt = 0
startup_conditions["ignition"] = pandaState.ignitionLine or pandaState.ignitionCan
in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected
usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client
# Setup fan handler on first connect to panda
if not JETSON and handle_fan is None and peripheralState.pandaType != log.PandaState.PandaType.unknown:
is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno
if TICI:
cloudlog.info("Setting up TICI fan handler")
handle_fan = handle_fan_tici
elif is_uno or PC:
cloudlog.info("Setting up UNO fan handler")
handle_fan = handle_fan_uno
else:
cloudlog.info("Setting up EON fan handler")
setup_eon_fan()
handle_fan = handle_fan_eon
# Handle disconnect
if pandaState_prev is not None:
if pandaState.pandaType == log.PandaState.PandaType.unknown and \
pandaState_prev.pandaType != log.PandaState.PandaType.unknown:
params.clear_all(ParamKeyType.CLEAR_ON_PANDA_DISCONNECT)
pandaState_prev = pandaState
# these are expensive calls. update every 10s
if (count % int(10. / DT_TRML)) == 0:
try:
network_type = HARDWARE.get_network_type()
network_strength = HARDWARE.get_network_strength(network_type)
network_info = HARDWARE.get_network_info() # pylint: disable=assignment-from-none
nvme_temps = HARDWARE.get_nvme_temperatures()
modem_temps = HARDWARE.get_modem_temperatures()
# Log modem version once
if modem_version is None:
modem_version = HARDWARE.get_modem_version() # pylint: disable=assignment-from-none
if modem_version is not None:
cloudlog.warning(f"Modem version: {modem_version}")
if TICI and (network_info.get('state', None) == "REGISTERED"):
registered_count += 1
else:
registered_count = 0
if registered_count > 10:
cloudlog.warning(f"Modem stuck in registered state {network_info}. nmcli conn up lte")
os.system("nmcli conn up lte")
registered_count = 0
except Exception:
cloudlog.exception("Error getting network status")
msg.deviceState.freeSpacePercent = get_available_percent(default=100.0)
msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent))
msg.deviceState.cpuUsagePercent = [int(round(n)) for n in psutil.cpu_percent(percpu=True)]
msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent()))
msg.deviceState.networkType = network_type
msg.deviceState.networkStrength = network_strength
if network_info is not None:
msg.deviceState.networkInfo = network_info
if nvme_temps is not None:
msg.deviceState.nvmeTempC = nvme_temps
if modem_temps is not None:
msg.deviceState.modemTempC = modem_temps
msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness()
msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity()
msg.deviceState.batteryCurrent = HARDWARE.get_battery_current()
msg.deviceState.usbOnline = HARDWARE.get_usb_present()
# Fake battery levels on uno for frame
if dp_no_batt:
msg.deviceState.batteryPercent = 100
current_filter.update(msg.deviceState.batteryCurrent / 1e6)
max_comp_temp = temp_filter.update(
max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC))
)
if handle_fan is not None:
fan_speed = handle_fan(dp_fan_mode, controller, max_comp_temp, fan_speed, startup_conditions["ignition"])
msg.deviceState.fanSpeedPercentDesired = fan_speed
is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5))
if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP:
# If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107
thermal_status = ThermalStatus.danger
else:
current_band = THERMAL_BANDS[thermal_status]
band_idx = list(THERMAL_BANDS.keys()).index(thermal_status)
if current_band.min_temp is not None and max_comp_temp < current_band.min_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1]
elif current_band.max_temp is not None and max_comp_temp > current_band.max_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1]
if not dp_temp_monitor and thermal_status in [ThermalStatus.red, ThermalStatus.danger]:
thermal_status = ThermalStatus.yellow
# **** starting logic ****
# Check for last update time and display alerts if needed
# now = datetime.datetime.utcnow()
#
# # show invalid date/time alert
# startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10)
# set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"]))
#
# # Show update prompt
# try:
# last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8'))
# except (TypeError, ValueError):
# last_update = now
# dt = now - last_update
#
# update_failed_count = params.get("UpdateFailedCount")
# update_failed_count = 0 if update_failed_count is None else int(update_failed_count)
# last_update_exception = params.get("LastUpdateException", encoding='utf8')
#
# if update_failed_count > 15 and last_update_exception is not None:
# if tested_branch:
# extra_text = "Ensure the software is correctly installed"
# else:
# extra_text = last_update_exception
#
# set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
# set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text)
# elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
# set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True)
# elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
# remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
# set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.")
# else:
# set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
# set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False)
startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates")
startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version
panda_signature = params.get("PandaFirmware")
startup_conditions["fw_version_match"] = (panda_signature is None) or (panda_signature == FW_SIGNATURE) # don't show alert is no panda is connected (None)
set_offroad_alert_if_changed("Offroad_PandaFirmwareMismatch", (not startup_conditions["fw_version_match"]))
# with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
params.get_bool("Passive")
startup_conditions["not_driver_view"] = not params.get_bool("IsDriverViewEnabled")
startup_conditions["not_taking_snapshot"] = not params.get_bool("IsTakingSnapshot")
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
startup_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"]))
if TICI:
set_offroad_alert_if_changed("Offroad_NvmeMissing", (not Path("/data/media").is_mount()))
# Handle offroad/onroad transition
should_start = all(startup_conditions.values())
# dp - check usb_present to fix not going offroad on "EON/LEON + battery - Comma Power"
if dp_no_offroad_fix:
should_start = should_start and HARDWARE.get_usb_present()
if should_start != should_start_prev or (count == 0):
params.put_bool("IsOnroad", should_start)
params.put_bool("IsOffroad", not should_start)
HARDWARE.set_power_save(not should_start)
if should_start:
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
else:
if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev):
cloudlog.event("Startup blocked", startup_conditions=startup_conditions)
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
# Offroad power monitoring
if not dp_no_batt:
power_monitor.calculate(peripheralState, startup_conditions["ignition"])
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
# Check if we need to disable charging (handled by boardd)
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(startup_conditions["ignition"], in_car, off_ts, dp_auto_shutdown, dp_auto_shutdown_in)
# dp - for battery powered device
# when peripheralState is not changing (panda offline), and usb is not present (not charging)
if dp_no_offroad_fix and (peripheralStateLast == peripheralState) and not msg.deviceState.usbOnline:
if (sec_since_boot() - off_ts) > dp_auto_shutdown_in * 60:
time.sleep(10)
HARDWARE.shutdown()
peripheralStateLast = peripheralState
# Check if we need to shut down
if power_monitor.should_shutdown(peripheralState, startup_conditions["ignition"], in_car, off_ts, started_seen, LEON, dp_auto_shutdown, dp_auto_shutdown_in):
cloudlog.info(f"shutting device down, offroad since {off_ts}")
# TODO: add function for blocking cloudlog instead of sleep
time.sleep(10)
HARDWARE.shutdown()
# If UI has crashed, set the brightness to reasonable non-zero value
ui_running = "ui" in (p.name for p in sm["managerState"].processes if p.running)
if ui_running_prev and not ui_running:
HARDWARE.set_screen_brightness(20)
ui_running_prev = ui_running
msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.deviceState.started = started_ts is not None
msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0))
last_ping = params.get("LastAthenaPingTime")
if last_ping is not None:
msg.deviceState.lastAthenaPingTime = int(last_ping)
msg.deviceState.thermalStatus = thermal_status
pm.send("deviceState", msg)
if EON and not is_uno:
set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power))
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# report to server once every 10 minutes
# if (count % int(600. / DT_TRML)) == 0:
# if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40:
# cloudlog.event("High offroad memory usage", mem=msg.deviceState.memoryUsagePercent)
#
# cloudlog.event("STATUS_PACKET",
# count=count,
# pandaStates=(strip_deprecated_keys(pandaStates.to_dict()) if pandaStates else None),
# peripheralState=strip_deprecated_keys(peripheralState.to_dict()),
# location=(strip_deprecated_keys(sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None),
# deviceState=strip_deprecated_keys(msg.to_dict()))
count += 1
def 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", 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
# Passive if internet needed
internet_needed = params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None
passive = passive or internet_needed
# 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)
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]))
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.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]))
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()
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState',
'carControl', 'carEvents', 'carParams'])
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'] if SIMULATION else None
self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'roadCameraState', 'driverCameraState', 'managerState', 'liveParameters', 'radarState'],
ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one pandaState and one CAN packet
hw_type = messaging.recv_one(self.sm.sock['pandaState']).pandaState.pandaType
has_relay = hw_type in [PandaType.blackPanda, PandaType.uno, PandaType.dos]
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay)
# read params
params = Params()
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
self.enable_lte_onroad = params.get_bool("EnableLteOnroad")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
fuzzy_fingerprint = self.CP.fuzzyFingerprint
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature = self.CP.communityFeature or fuzzy_fingerprint
community_feature_disallowed = community_feature and (not community_feature_toggle)
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.initialized = False
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
# scc smoother
self.is_cruise_enabled = False
self.cruiseVirtualMaxSpeed = 0
self.clu_speed_ms = 0.
self.apply_accel = 0.
self.fused_accel = 0.
self.lead_drel = 0.
self.aReqValue = 0.
self.aReqValueMin = 0.
self.aReqValueMax = 0.
self.angle_steers_des = 0.
# TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized, controller_available, fuzzy_fingerprint)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
#!/usr/bin/env python3
import time
import cereal.messaging as messaging
from selfdrive.manager import start_managed_process, kill_managed_process
services = ['controlsState', 'deviceState', 'radarState'] # the services needed to be spoofed to start ui offroad
procs = ['camerad', 'ui', 'modeld', 'calibrationd']
[start_managed_process(p) for p in procs] # start needed processes
pm = messaging.PubMaster(services)
dat_cs, dat_deviceState, dat_radar = [messaging.new_message(s) for s in services]
dat_cs.controlsState.rearViewCam = False # ui checks for these two messages
dat_deviceState.deviceState.started = True
try:
while True:
pm.send('controlsState', dat_cs)
pm.send('deviceState', dat_deviceState)
pm.send('radarState', dat_radar)
time.sleep(1 / 100) # continually send, rate doesn't matter for deviceState
except KeyboardInterrupt:
[kill_managed_process(p) for p in procs]
def __init__(self, sm=None, pm=None, can_sock=None):
config_rt_process(3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.sm = sm
if self.sm is None:
self.sm = messaging.SubMaster([
'thermal', 'health', 'frame', 'model', 'liveCalibration',
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'
])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one health and one CAN packet
hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno, HwType.dos]
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'],
has_relay)
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled",
encoding='utf8') == "1"
internet_needed = (params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None) and (
params.get("DisableUpdates") != b"1")
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1" or \
internet_needed or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.sm['liveCalibration'].calStatus = Calibration.INVALID
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized,
controller_available)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if internet_needed:
self.events.add(EventName.internetConnectivityNeeded, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if self.read_only and not passive:
self.events.add(EventName.carUnrecognized, static=True)
if hw_type == HwType.whitePanda:
self.events.add(EventName.whitePandaUnsupported, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
def main() -> NoReturn:
unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report,
True)
unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(
gps_measurement_report_sv, True)
unpack_glonass_meas, size_glonass_meas = dict_unpacker(
glonass_measurement_report, True)
unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(
glonass_measurement_report_sv, True)
unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(
oemdre_measurement_report, True)
unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(
oemdre_measurement_report_sv, True)
log_types = [
LOG_GNSS_GPS_MEASUREMENT_REPORT,
LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
]
pub_types = ['qcomGnss']
if int(os.getenv("PUBLISH_EXTERNAL", "0")) == 1:
unpack_position, _ = dict_unpacker(position_report)
log_types.append(LOG_GNSS_POSITION_REPORT)
pub_types.append("gpsLocationExternal")
# connect to modem
diag = ModemDiag()
# NV enable OEMDRE
# TODO: it has to reboot for this to take effect
DIAG_NV_READ_F = 38
DIAG_NV_WRITE_F = 39
NV_GNSS_OEM_FEATURE_MASK = 7165
opcode, payload = send_recv(diag, DIAG_NV_WRITE_F,
pack('<HI', NV_GNSS_OEM_FEATURE_MASK, 1))
opcode, payload = send_recv(diag, DIAG_NV_READ_F,
pack('<H', NV_GNSS_OEM_FEATURE_MASK))
def try_setup_logs(diag, log_types):
for _ in range(5):
try:
setup_logs(diag, log_types)
break
except Exception:
pass
def disable_logs(sig, frame):
os.system(
"mmcli -m 0 --location-disable-gps-raw --location-disable-gps-nmea"
)
cloudlog.warning("rawgpsd: shutting down")
try_setup_logs(diag, [])
cloudlog.warning("rawgpsd: logs disabled")
sys.exit(0)
signal.signal(signal.SIGINT, disable_logs)
try_setup_logs(diag, log_types)
cloudlog.warning("rawgpsd: setup logs done")
# disable DPO power savings for more accuracy
os.system("mmcli -m 0 --command='AT+QGPSCFG=\"dpoenable\",0'")
os.system(
"mmcli -m 0 --location-enable-gps-raw --location-enable-gps-nmea")
# enable OEMDRE mode
DIAG_SUBSYS_CMD_F = 75
DIAG_SUBSYS_GPS = 13
CGPS_DIAG_PDAPI_CMD = 0x64
CGPS_OEM_CONTROL = 202
GPSDIAG_OEMFEATURE_DRE = 1
GPSDIAG_OEM_DRE_ON = 1
# gpsdiag_OemControlReqType
opcode, payload = send_recv(
diag,
DIAG_SUBSYS_CMD_F,
pack(
'<BHBBIIII',
DIAG_SUBSYS_GPS, # Subsystem Id
CGPS_DIAG_PDAPI_CMD, # Subsystem Command Code
CGPS_OEM_CONTROL, # CGPS Command Code
0, # Version
GPSDIAG_OEMFEATURE_DRE,
GPSDIAG_OEM_DRE_ON,
0,
0))
pm = messaging.PubMaster(pub_types)
while 1:
opcode, payload = diag.recv()
assert opcode == DIAG_LOG_F
(pending_msgs, log_outer_length), inner_log_packet = unpack_from(
'<BH', payload), payload[calcsize('<BH'):]
if pending_msgs > 0:
cloudlog.debug("have %d pending messages" % pending_msgs)
assert log_outer_length == len(inner_log_packet)
(log_inner_length, log_type, log_time), log_payload = unpack_from(
'<HHQ', inner_log_packet), inner_log_packet[calcsize('<HHQ'):]
assert log_inner_length == len(inner_log_packet)
if log_type not in log_types:
continue
if DEBUG:
print("%.4f: got log: %x len %d" %
(time.time(), log_type, len(log_payload)))
if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT:
msg = messaging.new_message('qcomGnss')
gnss = msg.qcomGnss
gnss.logTs = log_time
gnss.init('drMeasurementReport')
report = gnss.drMeasurementReport
dat = unpack_oemdre_meas(log_payload)
for k, v in dat.items():
if k in ["gpsTimeBias", "gpsClockTimeUncertainty"]:
k += "Ms"
if k == "version":
assert v == 2
elif k == "svCount" or k.startswith("cdmaClockInfo["):
# TODO: should we save cdmaClockInfo?
pass
elif k == "systemRtcValid":
setattr(report, k, bool(v))
else:
setattr(report, k, v)
report.init('sv', dat['svCount'])
sats = log_payload[size_oemdre_meas:]
for i in range(dat['svCount']):
sat = unpack_oemdre_meas_sv(
sats[size_oemdre_meas_sv * i:size_oemdre_meas_sv *
(i + 1)])
sv = report.sv[i]
sv.init('measurementStatus')
for k, v in sat.items():
if k in ["unkn", "measurementStatus2"]:
pass
elif k == "multipathEstimateValid":
sv.measurementStatus.multipathEstimateIsValid = bool(v)
elif k == "directionValid":
sv.measurementStatus.directionIsValid = bool(v)
elif k == "goodParity":
setattr(sv, k, bool(v))
elif k == "measurementStatus":
for kk, vv in measurementStatusFields.items():
setattr(sv.measurementStatus, kk,
bool(v & (1 << vv)))
else:
setattr(sv, k, v)
pm.send('qcomGnss', msg)
elif log_type == LOG_GNSS_POSITION_REPORT:
report = unpack_position(log_payload)
if report["u_PosSource"] != 2:
continue
vNED = [
report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"],
-report["q_FltVelEnuMps[2]"]
]
vNEDsigma = [
report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"],
-report["q_FltVelSigmaMps[2]"]
]
msg = messaging.new_message('gpsLocationExternal')
gps = msg.gpsLocationExternal
gps.flags = 1
gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180 / math.pi
gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180 / math.pi
gps.altitude = report["q_FltFinalPosAlt"]
gps.speed = math.sqrt(sum([x**2 for x in vNED]))
gps.bearingDeg = report["q_FltHeadingRad"] * 180 / math.pi
# TODO: this probably isn't right, use laika for this
gps.timestamp = report['w_GpsWeekNumber'] * 604800 * 1000 + report[
'q_GpsFixTimeMs']
gps.source = log.GpsLocationData.SensorSource.qcomdiag
gps.vNED = vNED
gps.verticalAccuracy = report["q_FltVdop"]
gps.bearingAccuracyDeg = report[
"q_FltHeadingUncRad"] * 180 / math.pi
gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma]))
pm.send('gpsLocationExternal', msg)
if log_type in [
LOG_GNSS_GPS_MEASUREMENT_REPORT,
LOG_GNSS_GLONASS_MEASUREMENT_REPORT
]:
msg = messaging.new_message('qcomGnss')
gnss = msg.qcomGnss
gnss.logTs = log_time
gnss.init('measurementReport')
report = gnss.measurementReport
if log_type == LOG_GNSS_GPS_MEASUREMENT_REPORT:
dat = unpack_gps_meas(log_payload)
sats = log_payload[size_gps_meas:]
unpack_meas_sv, size_meas_sv = unpack_gps_meas_sv, size_gps_meas_sv
report.source = 0 # gps
measurement_status_fields = (
measurementStatusFields.items(),
measurementStatusGPSFields.items())
elif log_type == LOG_GNSS_GLONASS_MEASUREMENT_REPORT:
dat = unpack_glonass_meas(log_payload)
sats = log_payload[size_glonass_meas:]
unpack_meas_sv, size_meas_sv = unpack_glonass_meas_sv, size_glonass_meas_sv
report.source = 1 # glonass
measurement_status_fields = (
measurementStatusFields.items(),
measurementStatusGlonassFields.items())
else:
assert False
for k, v in dat.items():
if k == "version":
assert v == 0
elif k == "week":
report.gpsWeek = v
elif k == "svCount":
pass
else:
setattr(report, k, v)
report.init('sv', dat['svCount'])
if dat['svCount'] > 0:
assert len(sats) // dat['svCount'] == size_meas_sv
for i in range(dat['svCount']):
sv = report.sv[i]
sv.init('measurementStatus')
sat = unpack_meas_sv(sats[size_meas_sv * i:size_meas_sv *
(i + 1)])
for k, v in sat.items():
if k == "parityErrorCount":
sv.gpsParityErrorCount = v
elif k == "frequencyIndex":
sv.glonassFrequencyIndex = v
elif k == "hemmingErrorCount":
sv.glonassHemmingErrorCount = v
elif k == "measurementStatus":
for kk, vv in itertools.chain(
*measurement_status_fields):
setattr(sv.measurementStatus, kk,
bool(v & (1 << vv)))
elif k == "miscStatus":
for kk, vv in miscStatusFields.items():
setattr(sv.measurementStatus, kk,
bool(v & (1 << vv)))
elif k == "pad":
pass
else:
setattr(sv, k, v)
pm.send('qcomGnss', msg)
