def cpp_replay_process(cfg, lr):
sub_sockets = [s for _, sub in cfg.pub_sub.items()
for s in sub] # We get responses here
pm = messaging.PubMaster(cfg.pub_sub.keys())
sockets = {s: messaging.sub_sock(s, timeout=1000) for s in sub_sockets}
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = [
msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())
]
manager.prepare_managed_process(cfg.proc_name)
manager.start_managed_process(cfg.proc_name)
time.sleep(1) # We give the process time to start
log_msgs = []
for s in sub_sockets:
messaging.recv_one_or_none(sockets[s])
for msg in tqdm(pub_msgs):
pm.send(msg.which(), msg.as_builder())
resp_sockets = sub_sockets if cfg.should_recv_callback is None else cfg.should_recv_callback(
msg)
for s in resp_sockets:
response = messaging.recv_one(sockets[s])
if response is not None:
log_msgs.append(response)
manager.kill_managed_process(cfg.proc_name)
return log_msgs
def cpp_replay_process(cfg, lr, fingerprint=None):
sub_sockets = [s for _, sub in cfg.pub_sub.items()
for s in sub] # We get responses here
pm = messaging.PubMaster(cfg.pub_sub.keys())
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = [
msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())
]
log_msgs = []
# We need to fake SubMaster alive since we can't inject a fake clock
setup_env(simulation=True)
managed_processes[cfg.proc_name].prepare()
managed_processes[cfg.proc_name].start()
try:
with Timeout(TIMEOUT):
while not all(
pm.all_readers_updated(s) for s in cfg.pub_sub.keys()):
time.sleep(0)
# Make sure all subscribers are connected
sockets = {
s: messaging.sub_sock(s, timeout=2000)
for s in sub_sockets
}
for s in sub_sockets:
messaging.recv_one_or_none(sockets[s])
for i, msg in enumerate(tqdm(pub_msgs, disable=False)):
pm.send(msg.which(), msg.as_builder())
resp_sockets = cfg.pub_sub[msg.which(
)] if cfg.should_recv_callback is None else cfg.should_recv_callback(
msg)
for s in resp_sockets:
response = messaging.recv_one(sockets[s])
if response is None:
print(f"Warning, no response received {i}")
else:
response = response.as_builder()
response.logMonoTime = msg.logMonoTime
response = response.as_reader()
log_msgs.append(response)
if not len(
resp_sockets
): # We only need to wait if we didn't already wait for a response
while not pm.all_readers_updated(msg.which()):
time.sleep(0)
finally:
managed_processes[cfg.proc_name].signal(signal.SIGKILL)
managed_processes[cfg.proc_name].stop()
return log_msgs
def gps_planner_plan():
context = zmq.Context()
live_location = messaging.sub_sock(context,
'liveLocation',
conflate=True,
addr=_REMOTE_ADDR)
gps_planner_points = messaging.sub_sock(context,
'gpsPlannerPoints',
conflate=True)
gps_planner_plan = messaging.pub_sock(context, 'gpsPlannerPlan')
points = messaging.recv_one(gps_planner_points).gpsPlannerPoints
target_speed = 100. * CV.MPH_TO_MS
target_accel = 0.
last_ecef = np.array([0., 0., 0.])
while True:
ll = messaging.recv_one(live_location)
ll = ll.liveLocation
p = messaging.recv_one_or_none(gps_planner_points)
if p is not None:
points = p.gpsPlannerPoints
target_speed = p.gpsPlannerPoints.speedLimit
target_accel = p.gpsPlannerPoints.accelTarget
cur_ecef = geodetic2ecef((ll.lat, ll.lon, ll.alt))
# TODO: make NED initialization much faster so we can run this every time step
if np.linalg.norm(last_ecef - cur_ecef) > 200.:
ned_converter = NED(ll.lat, ll.lon, ll.alt)
last_ecef = cur_ecef
cur_heading = np.radians(ll.heading)
if points.valid:
poly, x_lookahead = fit_poly(points, cur_ecef, cur_heading,
ned_converter)
else:
poly, x_lookahead = [0.0, 0.0, 0.0, 0.0], 0.
valid = points.valid
m = messaging.new_message('gpsPlannerPlan')
m.gpsPlannerPlan.valid = valid
m.gpsPlannerPlan.poly = poly
m.gpsPlannerPlan.trackName = points.trackName
r = []
for p in points.points:
point = log.ECEFPoint.new_message()
point.x, point.y, point.z = p.x, p.y, p.z
r.append(point)
m.gpsPlannerPlan.points = r
m.gpsPlannerPlan.speed = target_speed
m.gpsPlannerPlan.acceleration = target_accel
m.gpsPlannerPlan.xLookahead = x_lookahead
gps_planner_plan.send(m.to_bytes())
def cpp_replay_process(cfg, lr, fingerprint=None):
sub_sockets = [s for _, sub in cfg.pub_sub.items()
for s in sub] # We get responses here
pm = messaging.PubMaster(cfg.pub_sub.keys())
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = [
msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())
]
os.environ["SIMULATION"] = "1" # Disable submaster alive checks
managed_processes[cfg.proc_name].prepare()
managed_processes[cfg.proc_name].start()
while not all(pm.all_readers_updated(s) for s in cfg.pub_sub.keys()):
time.sleep(0)
# Make sure all subscribers are connected
sockets = {s: messaging.sub_sock(s, timeout=2000) for s in sub_sockets}
for s in sub_sockets:
messaging.recv_one_or_none(sockets[s])
log_msgs = []
for i, msg in enumerate(tqdm(pub_msgs, disable=CI)):
pm.send(msg.which(), msg.as_builder())
resp_sockets = cfg.pub_sub[msg.which(
)] if cfg.should_recv_callback is None else cfg.should_recv_callback(
msg)
for s in resp_sockets:
response = messaging.recv_one(sockets[s])
if response is None:
print(f"Warning, no response received {i}")
else:
log_msgs.append(response)
if not len(
resp_sockets
): # We only need to wait if we didn't already wait for a response
while not pm.all_readers_updated(msg.which()):
time.sleep(0)
managed_processes[cfg.proc_name].signal(signal.SIGKILL)
managed_processes[cfg.proc_name].stop()
return log_msgs
def recv_timeout(can, addr):
received = False
r = []
t = time.time()
while not received:
c = messaging.recv_one_or_none(can)
if c is not None:
for msg in c.can:
if msg.address == addr:
r.append(msg)
received = True
if time.time() - t > 0.05:
received = True
return r
def update_acc(self, enabled, CS, frame, actuators, pcm_speed,
speed_limit_kph, set_speed_limit_active,
speed_limit_offset):
# Adaptive cruise control
self.prev_speed_limit_kph = self.speed_limit_kph
if set_speed_limit_active and speed_limit_kph > 0:
self.speed_limit_kph = speed_limit_kph + speed_limit_offset
if int(self.prev_speed_limit_kph) != int(self.speed_limit_kph):
self.acc_speed_kph = self.speed_limit_kph
self.fleet_speed.reset_averager()
else: # reset internal speed limit, so double pull doesn't set higher speed than current (e.g. after leaving the highway)
self.speed_limit_kph = 0.
current_time_ms = _current_time_millis()
if CruiseButtons.should_be_throttled(CS.cruise_buttons):
self.human_cruise_action_time = current_time_ms
button_to_press = None
# If ACC is disabled, disengage traditional cruise control.
if ((self.prev_enable_adaptive_cruise)
and (not self.enable_adaptive_cruise)
and (CS.pcm_acc_status == CruiseState.ENABLED)):
button_to_press = CruiseButtons.CANCEL
#if non addaptive and we just engaged ACC but pcm is not engaged, then engage
if (not self.adaptive) and self.enable_adaptive_cruise and (
CS.pcm_acc_status != CruiseState.ENABLED):
button_to_press = CruiseButtons.MAIN
#if plain cc, not adaptive, then just return None or Cancel
if (not self.adaptive) and self.enable_adaptive_cruise:
self.acc_speed_kph = CS.v_cruise_actual #if not CS.imperial_speed_units else CS.v_cruise_actual * CV.MPH_TO_KPH
return button_to_press
#disengage if cruise is canceled
if (not self.enable_adaptive_cruise) and (CS.pcm_acc_status >= 2) and (
CS.pcm_acc_status <= 4):
button_to_press = CruiseButtons.CANCEL
lead_1 = None
#if enabled:
lead = messaging.recv_one_or_none(self.radarState)
if lead is not None:
lead_1 = lead.radarState.leadOne
if lead_1.dRel:
self.lead_last_seen_time_ms = current_time_ms
if self.enable_adaptive_cruise and enabled: # and (button_to_press == None):
if CS.cstm_btns.get_button_label2(
ACCMode.BUTTON_NAME) in ["OP", "AutoOP"]:
button_to_press = self._calc_button(CS, pcm_speed)
self.new_speed = pcm_speed * CV.MS_TO_KPH
else:
# Alternative speed decision logic that uses the lead car's distance
# and speed more directly.
# Bring in the lead car distance from the radarState feed
button_to_press = self._calc_follow_button(
CS, lead_1, speed_limit_kph, set_speed_limit_active,
speed_limit_offset, frame)
if button_to_press:
self.automated_cruise_action_time = current_time_ms
# If trying to slow below the min cruise speed, just cancel cruise.
# This prevents a SCCM crash which is triggered by repeatedly pressing
# stalk-down when already at min cruise speed.
if (CruiseButtons.is_decel(button_to_press)
and CS.v_cruise_actual - 1 <
self.MIN_CRUISE_SPEED_MS * CV.MS_TO_KPH):
button_to_press = CruiseButtons.CANCEL
if button_to_press == CruiseButtons.CANCEL:
self.fast_decel_time = current_time_ms
# Debug logging (disable in production to reduce latency of commands)
#print "***ACC command: %s***" % button_to_press
return button_to_press
def manager_thread():
# now loop
thermal_sock = messaging.sub_sock('thermal')
gps_sock = messaging.sub_sock('gpsLocation', conflate=True)
if os.getenv("GET_CPU_USAGE"):
proc_sock = messaging.sub_sock('procLog', conflate=True)
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
# save boot log
#subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
params = Params()
# start daemon processes
#for p in daemon_processes:
# start_daemon_process(p)
# start persistent processes
for p in persistent_processes:
start_managed_process(p)
# start offroad
if ANDROID:
pm_apply_packages('enable')
start_offroad()
if os.getenv("NOBOARD") is None:
start_managed_process("pandad")
if os.getenv("BLOCK") is not None:
for k in os.getenv("BLOCK").split(","):
del managed_processes[k]
logger_dead = False
start_t = time.time()
first_proc = None
while 1:
gps = messaging.recv_one_or_none(gps_sock)
msg = messaging.recv_sock(thermal_sock, wait=True)
if gps:
if 47.3024876979 < gps.gpsLocation.latitude < 54.983104153 and 5.98865807458 < gps.gpsLocation.longitude < 15.0169958839:
logger_dead = True
else:
logger_dead = True
# heavyweight batch processes are gated on favorable thermal conditions
if msg.thermal.thermalStatus >= ThermalStatus.yellow:
for p in green_temp_processes:
if p in persistent_processes:
kill_managed_process(p)
else:
for p in green_temp_processes:
if p in persistent_processes:
start_managed_process(p)
if msg.thermal.freeSpace < 0.05:
logger_dead = True
if msg.thermal.started and "driverview" not in running:
for p in car_started_processes:
if p == "loggerd" and logger_dead:
kill_managed_process(p)
else:
start_managed_process(p)
else:
logger_dead = False
for p in reversed(car_started_processes):
kill_managed_process(p)
# this is ugly
if "driverview" not in running and params.get(
"IsDriverViewEnabled") == b"1":
start_managed_process("driverview")
elif "driverview" in running and params.get(
"IsDriverViewEnabled") == b"0":
kill_managed_process("driverview")
# check the status of all processes, did any of them die?
running_list = [
"%s%s\u001b[0m" %
("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p)
for p in running
]
cloudlog.debug(' '.join(running_list))
# Exit main loop when uninstall is needed
if params.get("DoUninstall", encoding='utf8') == "1":
break
if os.getenv("GET_CPU_USAGE"):
dt = time.time() - start_t
# Get first sample
if dt > 30 and first_proc is None:
first_proc = messaging.recv_sock(proc_sock)
# Get last sample and exit
if dt > 90:
last_proc = messaging.recv_sock(proc_sock, wait=True)
cleanup_all_processes(None, None)
sys.exit(print_cpu_usage(first_proc, last_proc))
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()
# 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_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 (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"]))
# 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):
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)
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:
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 gps_planner_point_selection():
DECIMATION = 1
cloudlog.info("Starting gps_plannerd point selection")
rk = Ratekeeper(10.0, print_delay_threshold=np.inf)
context = zmq.Context()
live_location = messaging.sub_sock(context,
'liveLocation',
conflate=True,
addr=_REMOTE_ADDR)
car_state = messaging.sub_sock(context, 'carState', conflate=True)
gps_planner_points = messaging.pub_sock(context, 'gpsPlannerPoints')
ui_navigation_event = messaging.pub_sock(context, 'uiNavigationEvent')
# Load tracks and instructions from disk
basedir = os.environ['BASEDIR']
tracks = np.load(
os.path.join(basedir,
'selfdrive/controls/tracks/%s.npy' % LOOP)).item()
instructions = json.loads(
open(
os.path.join(
basedir, 'selfdrive/controls/tracks/instructions_%s.json' %
LOOP)).read())
# Put tracks into KD-trees
track_trees = {}
for name in tracks:
tracks[name] = tracks[name][::DECIMATION]
track_trees[name] = cKDTree(tracks[name][:, 0:3]) # xyz
cur_track = None
source_track = None
target_track = None
instruction = None
v_ego = 0.
state = INSTRUCTION_STATE.NONE
counter = 0
while True:
counter += 1
ll = messaging.recv_one(live_location)
ll = ll.liveLocation
cur_ecef = geodetic2ecef((ll.lat, ll.lon, ll.alt))
cs = messaging.recv_one_or_none(car_state)
if cs is not None:
v_ego = cs.carState.vEgo
cur_track, closest_track = update_current_track(
tracks, cur_track, cur_ecef, track_trees)
#print cur_track
instruction = update_instruction(instruction, instructions, cur_track,
source_track, state, cur_ecef, tracks)
source_track, target_track = get_tracks_from_instruction(
tracks, instruction, track_trees, cur_ecef)
state, cur_track = calc_instruction_state(state, cur_track,
closest_track, source_track,
target_track, instruction)
active_points = []
# Make list of points used by gpsPlannerPlan
if cur_track is not None:
active_points = get_spaced_points(tracks[cur_track['name']],
cur_track['idx'], cur_ecef,
v_ego)
cur_pos = log.ECEFPoint.new_message()
cur_pos.x, cur_pos.y, cur_pos.z = map(float, cur_ecef)
m = messaging.new_message('gpsPlannerPoints')
m.gpsPlannerPoints.curPos = cur_pos
m.gpsPlannerPoints.points = convert_ecef_to_capnp(active_points)
m.gpsPlannerPoints.valid = len(active_points) > 10
m.gpsPlannerPoints.trackName = "none" if cur_track is None else cur_track[
'name']
m.gpsPlannerPoints.speedLimit = 100. if cur_track is None else float(
cur_track['speed'])
m.gpsPlannerPoints.accelTarget = 0. if cur_track is None else float(
cur_track['accel'])
gps_planner_points.send(m.to_bytes())
m = messaging.new_message('uiNavigationEvent')
m.uiNavigationEvent.status = state
m.uiNavigationEvent.type = "none" if instruction is None else instruction[
'type']
m.uiNavigationEvent.distanceTo = 0. if instruction is None else float(
instruction['distance'])
endRoadPoint = log.ECEFPoint.new_message()
m.uiNavigationEvent.endRoadPoint = endRoadPoint
ui_navigation_event.send(m.to_bytes())
rk.keep_time()
def mapsd_thread():
global last_gps
gps_sock = messaging.sub_sock('gpsLocation', conflate=True)
gps_external_sock = messaging.sub_sock('gpsLocationExternal', conflate=True)
map_data_sock = messaging.pub_sock('liveMapData')
cur_way = None
curvature_valid = False
curvature = None
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
while True:
gps = messaging.recv_one(gps_sock)
gps_ext = messaging.recv_one_or_none(gps_external_sock)
if gps_ext is not None:
gps = gps_ext.gpsLocationExternal
else:
gps = gps.gpsLocation
last_gps = gps
fix_ok = gps.flags & 1
if not fix_ok or last_query_result is None or not cache_valid:
cur_way = None
curvature = None
curvature_valid = False
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
map_valid = False
else:
map_valid = True
lat = gps.latitude
lon = gps.longitude
heading = gps.bearing
speed = gps.speed
query_lock.acquire()
cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way)
if cur_way is not None:
pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
xs = pnts[:, 0]
ys = pnts[:, 1]
road_points = [float(x) for x in xs], [float(y) for y in ys]
if speed < 10:
curvature_valid = False
if curvature_valid and pnts.shape[0] <= 3:
curvature_valid = False
# The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found
if curvature_valid:
# Compute the curvature for each point
with np.errstate(divide='ignore'):
circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])]
circles = np.asarray(circles)
radii = np.nan_to_num(circles[:, 2])
radii[radii < 10] = np.inf
curvature = 1. / radii
# Index of closest point
closest = np.argmin(np.linalg.norm(pnts, axis=1))
dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close
# Compute distance along path
dists = list()
dists.append(0)
for p, p_prev in zip(pnts, pnts[1:, :]):
dists.append(dists[-1] + np.linalg.norm(p - p_prev))
dists = np.asarray(dists)
dists = dists - dists[closest] + dist_to_closest
dists = dists[1:-1]
close_idx = np.logical_and(dists > 0, dists < 500)
dists = dists[close_idx]
curvature = curvature[close_idx]
if len(curvature):
# TODO: Determine left or right turn
curvature = np.nan_to_num(curvature)
# Outlier rejection
new_curvature = np.percentile(curvature, 90, interpolation='lower')
k = 0.6
upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature
in_turn_indices = curvature > 0.8 * new_curvature
if np.any(in_turn_indices):
dist_to_turn = np.min(dists[in_turn_indices])
else:
dist_to_turn = 999
else:
upcoming_curvature = 0.
dist_to_turn = 999
query_lock.release()
dat = messaging.new_message()
dat.init('liveMapData')
if last_gps is not None:
dat.liveMapData.lastGps = last_gps
if cur_way is not None:
dat.liveMapData.wayId = cur_way.id
# Speed limit
max_speed = cur_way.max_speed()
if max_speed is not None:
dat.liveMapData.speedLimitValid = True
dat.liveMapData.speedLimit = max_speed
# TODO: use the function below to anticipate upcoming speed limits
#max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
#if max_speed_ahead is not None and max_speed_ahead_dist is not None:
# dat.liveMapData.speedLimitAheadValid = True
# dat.liveMapData.speedLimitAhead = float(max_speed_ahead)
# dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist)
advisory_max_speed = cur_way.advisory_max_speed()
if advisory_max_speed is not None:
dat.liveMapData.speedAdvisoryValid = True
dat.liveMapData.speedAdvisory = advisory_max_speed
# Curvature
dat.liveMapData.curvatureValid = curvature_valid
dat.liveMapData.curvature = float(upcoming_curvature)
dat.liveMapData.distToTurn = float(dist_to_turn)
if road_points is not None:
dat.liveMapData.roadX, dat.liveMapData.roadY = road_points
if curvature is not None:
dat.liveMapData.roadCurvatureX = [float(x) for x in dists]
dat.liveMapData.roadCurvature = [float(x) for x in curvature]
dat.liveMapData.mapValid = map_valid
map_data_sock.send(dat.to_bytes())
def update(self, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime,leftLaneVisible,rightLaneVisible):
if (not enabled) and (self.ALCA.laneChange_cancelled):
self.ALCA.laneChange_cancelled = False
self.ALCA.laneChange_cancelled_counter = 0
self.warningNeeded = 1
if self.warningCounter > 0:
self.warningCounter = self.warningCounter - 1
if self.warningCounter == 0:
self.warningNeeded = 1
if self.warningCounter == 0 or not enabled:
# when zero reset all warnings
self.DAS_222_accCameraBlind = 0 #we will see what we can use this for
self.DAS_219_lcTempUnavailableSpeed = 0
self.DAS_220_lcTempUnavailableRoad = 0
self.DAS_221_lcAborting = 0
self.DAS_211_accNoSeatBelt = 0
self.DAS_207_lkasUnavailable = 0 #use for manual not in drive?
self.DAS_208_rackDetected = 0 #use for low battery?
self.DAS_202_noisyEnvironment = 0 #use for planner error?
self.DAS_025_steeringOverride = 0 #use for manual steer?
self.DAS_206_apUnavailable = 0 #Ap disabled from CID
if CS.keepEonOff:
if CS.cstm_btns.get_button_status("dsp") != 9:
CS.cstm_btns.set_button_status("dsp", 9)
else:
if CS.cstm_btns.get_button_status("dsp") != 1:
CS.cstm_btns.set_button_status("dsp", 1)
# """ Controls thread """
if not CS.useTeslaMapData:
if self.speedlimit is None:
self.speedlimit = messaging.sub_sock('liveMapData',
conflate=True)
# *** no output if not enabled ***
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
# TODO: factor this out better
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
#if CS.CP.radarOffCan:
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required)
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
print("INVALID HUD", hud)
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)
# **** process the car messages ****
# *** compute control surfaces ***
# Prevent steering while stopped
MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s
vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY)
#upodate custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 100 == 0):
CS.cstm_btns.send_button_info()
#read speed limit params
if CS.hasTeslaIcIntegration:
self.set_speed_limit_active = True
self.speed_limit_offset = CS.userSpeedLimitOffsetKph
else:
self.set_speed_limit_active = (
self.params.get("SpeedLimitOffset")
is not None) and (self.params.get("LimitSetSpeed") == "1")
if self.set_speed_limit_active:
self.speed_limit_offset = float(
self.params.get("SpeedLimitOffset"))
if not self.isMetric:
self.speed_limit_offset = self.speed_limit_offset * CV.MPH_TO_MS
else:
self.speed_limit_offset = 0.
if CS.useTeslaGPS and (frame % 10 == 0):
if self.gpsLocationExternal is None:
self.gpsLocationExternal = messaging.pub_sock(
'gpsLocationExternal')
sol = gen_solution(CS)
sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
self.gpsLocationExternal.send(sol.to_bytes())
#get pitch/roll/yaw every 0.1 sec
if (frame % 10 == 0):
(self.accPitch, self.accRoll,
self.accYaw), (self.magPitch, self.magRoll,
self.magYaw), (self.gyroPitch, self.gyroRoll,
self.gyroYaw) = self.GYRO.update(
CS.v_ego, CS.a_ego,
CS.angle_steers)
CS.UE.uiGyroInfoEvent(self.accPitch, self.accRoll, self.accYaw,
self.magPitch, self.magRoll, self.magYaw,
self.gyroPitch, self.gyroRoll, self.gyroYaw)
# Update statuses for custom buttons every 0.1 sec.
if (frame % 10 == 0):
self.ALCA.update_status(
(CS.cstm_btns.get_button_status("alca") > 0)
and ((CS.enableALCA and not CS.hasTeslaIcIntegration) or
(CS.hasTeslaIcIntegration and CS.alcaEnabled)))
self.blinker.update_state(CS, frame)
# update PCC module info
pedal_can_sends = self.PCC.update_stat(CS, frame)
if self.PCC.pcc_available:
self.ACC.enable_adaptive_cruise = False
else:
# Update ACC module info.
self.ACC.update_stat(CS, True)
self.PCC.enable_pedal_cruise = False
# update CS.v_cruise_pcm based on module selected.
speed_uom_kph = 1.
if CS.imperial_speed_units:
speed_uom_kph = CV.KPH_TO_MPH
if self.ACC.enable_adaptive_cruise:
CS.v_cruise_pcm = self.ACC.acc_speed_kph * speed_uom_kph
elif self.PCC.enable_pedal_cruise:
CS.v_cruise_pcm = self.PCC.pedal_speed_kph * speed_uom_kph
else:
CS.v_cruise_pcm = max(0., CS.v_ego * CV.MS_TO_KPH) * speed_uom_kph
self.alca_enabled = self.ALCA.update(enabled, CS, actuators,
self.alcaStateData, frame,
self.blinker)
self.should_ldw = self._should_ldw(CS, frame)
apply_angle = -actuators.steerAngle # Tesla is reversed vs OP.
# Update HSO module info.
human_control = self.HSO.update_stat(self, CS, enabled, actuators,
frame)
human_lane_changing = CS.turn_signal_stalk_state > 0 and not self.alca_enabled
enable_steer_control = (enabled and not human_lane_changing
and not human_control and vehicle_moving)
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# Windup slower.
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
#BB disable limits to test 0.5.8
# apply_angle = clip(apply_angle , self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
# If human control, send the steering angle as read at steering wheel.
if human_control:
apply_angle = CS.angle_steers
# Send CAN commands.
can_sends = []
#if using radar, we need to send the VIN
if CS.useTeslaRadar and (frame % 100 == 0):
useRadar = 0
if CS.useTeslaRadar:
useRadar = 1
can_sends.append(
teslacan.create_radar_VIN_msg(self.radarVin_idx, CS.radarVIN,
1, 0x108, useRadar,
CS.radarPosition,
CS.radarEpasType))
self.radarVin_idx += 1
self.radarVin_idx = self.radarVin_idx % 3
#First we emulate DAS.
# DAS_longC_enabled (1),DAS_speed_override (1),DAS_apUnavailable (1), DAS_collision_warning (1), DAS_op_status (4)
# DAS_speed_kph(8),
# DAS_turn_signal_request (2),DAS_forward_collision_warning (2), DAS_hands_on_state (3),
# DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5),
# DAS_acc_speed_limit (8),
# DAS_speed_limit_units(8)
#send fake_das data as 0x553
# TODO: forward collision warning
if frame % 10 == 0:
speedlimitMsg = None
if self.speedlimit is not None:
speedlimitMsg = messaging.recv_one_or_none(self.speedlimit)
icLeadsMsg = self.icLeads.receive(non_blocking=True)
radarStateMsg = messaging.recv_one_or_none(self.radarState)
alcaStateMsg = self.alcaState.receive(non_blocking=True)
pathPlanMsg = messaging.recv_one_or_none(self.pathPlan)
icCarLRMsg = self.icCarLR.receive(non_blocking=True)
trafficeventsMsgs = None
if self.trafficevents is not None:
trafficeventsMsgs = messaging.recv_sock(self.trafficevents)
if CS.hasTeslaIcIntegration:
self.speed_limit_ms = CS.speed_limit_ms
if (speedlimitMsg is not None) and not CS.useTeslaMapData:
lmd = speedlimitMsg.liveMapData
self.speed_limit_ms = lmd.speedLimit if lmd.speedLimitValid else 0
if icLeadsMsg is not None:
self.icLeadsData = tesla.ICLeads.from_bytes(icLeadsMsg)
if radarStateMsg is not None:
#to show lead car on IC
if self.icLeadsData is not None:
can_messages = self.showLeadCarOnICCanMessage(
radarStateMsg=radarStateMsg)
can_sends.extend(can_messages)
if alcaStateMsg is not None:
self.alcaStateData = tesla.ALCAState.from_bytes(alcaStateMsg)
if pathPlanMsg is not None:
#to show curvature and lanes on IC
if self.alcaStateData is not None:
self.handlePathPlanSocketForCurvatureOnIC(
pathPlanMsg=pathPlanMsg,
alcaStateData=self.alcaStateData,
CS=CS)
if icCarLRMsg is not None:
can_messages = self.showLeftAndRightCarsOnICCanMessages(
icCarLRMsg=tesla.ICCarsLR.from_bytes(icCarLRMsg))
can_sends.extend(can_messages)
if trafficeventsMsgs is not None:
can_messages = self.handleTrafficEvents(
trafficEventsMsgs=trafficeventsMsgs)
can_sends.extend(can_messages)
op_status = 0x02
hands_on_state = 0x00
forward_collision_warning = 0 #1 if needed
if hud_alert == AH.FCW:
forward_collision_warning = hud_alert[1]
if forward_collision_warning > 1:
forward_collision_warning = 1
#cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold
cc_state = 1
alca_state = 0x00
speed_override = 0
collision_warning = 0x00
speed_control_enabled = 0
accel_min = -15
accel_max = 5
acc_speed_kph = 0
send_fake_warning = False
send_fake_msg = False
if enabled:
#self.opState 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning
alca_state = 0x01
if self.opState == 0:
op_status = 0x02
if self.opState == 1:
op_status = 0x03
if self.opState == 2:
op_status = 0x08
if self.opState == 3:
op_status = 0x01
if self.opState == 5:
op_status = 0x03
if self.blinker.override_direction > 0:
alca_state = 0x08 + self.blinker.override_direction
elif (self.lLine > 1) and (self.rLine > 1):
alca_state = 0x08
elif (self.lLine > 1):
alca_state = 0x06
elif (self.rLine > 1):
alca_state = 0x07
else:
alca_state = 0x01
#canceled by user
if self.ALCA.laneChange_cancelled and (
self.ALCA.laneChange_cancelled_counter > 0):
alca_state = 0x14
#min speed for ALCA
if (CS.CL_MIN_V > CS.v_ego):
alca_state = 0x05
#max angle for ALCA
if (abs(actuators.steerAngle) >= CS.CL_MAX_A):
alca_state = 0x15
if not enable_steer_control:
#op_status = 0x08
hands_on_state = 0x02
if hud_alert == AH.STEER:
if snd_chime == CM.MUTE:
hands_on_state = 0x03
else:
hands_on_state = 0x05
if self.PCC.pcc_available:
acc_speed_kph = self.PCC.pedal_speed_kph
if hud_alert == AH.FCW:
collision_warning = hud_alert[1]
if collision_warning > 1:
collision_warning = 1
#use disabling for alerts/errors to make them aware someting is goin going on
if (snd_chime == CM.DOUBLE) or (hud_alert == AH.FCW):
op_status = 0x08
if self.ACC.enable_adaptive_cruise:
acc_speed_kph = self.ACC.new_speed #pcm_speed * CV.MS_TO_KPH
if (self.PCC.pcc_available and self.PCC.enable_pedal_cruise) or (
self.ACC.enable_adaptive_cruise):
speed_control_enabled = 1
cc_state = 2
if not self.ACC.adaptive:
cc_state = 3
CS.speed_control_enabled = 1
else:
CS.speed_control_enabled = 0
if (CS.pcm_acc_status == 4):
#car CC enabled but not OP, display the HOLD message
cc_state = 3
else:
if (CS.pcm_acc_status == 4):
cc_state = 3
if enabled:
if frame % 2 == 0:
send_fake_msg = True
if frame % 25 == 0:
send_fake_warning = True
else:
if frame % 23 == 0:
send_fake_msg = True
if frame % 60 == 0:
send_fake_warning = True
if frame % 10 == 0:
can_sends.append(
teslacan.create_fake_DAS_obj_lane_msg(
self.leadDx, self.leadDy, self.leadClass, self.rLine,
self.lLine, self.curv0, self.curv1, self.curv2, self.curv3,
self.laneRange, self.laneWidth))
speed_override = 0
if (CS.pedal_interceptor_value > 10) and (cc_state > 1):
speed_override = 0 #force zero for now
if (not enable_steer_control) and op_status == 3:
#hands_on_state = 0x03
self.DAS_219_lcTempUnavailableSpeed = 1
self.warningCounter = 100
self.warningNeeded = 1
if enabled and self.ALCA.laneChange_cancelled and (
not CS.steer_override) and (
CS.turn_signal_stalk_state
== 0) and (self.ALCA.laneChange_cancelled_counter > 0):
self.DAS_221_lcAborting = 1
self.warningCounter = 300
self.warningNeeded = 1
if CS.hasTeslaIcIntegration:
highLowBeamStatus, highLowBeamReason, ahbIsEnabled = self.AHB.update(
CS, frame, self.ahbLead1)
if frame % 5 == 0:
self.cc_counter = (
self.cc_counter +
1) % 40 #use this to change status once a second
self.fleet_speed_state = 0x00 #fleet speed unavailable
if FleetSpeed.is_available(CS):
if self.ACC.fleet_speed.is_active(
frame) or self.PCC.fleet_speed.is_active(frame):
self.fleet_speed_state = 0x02 #fleet speed enabled
else:
self.fleet_speed_state = 0x01 #fleet speed available
can_sends.append(
teslacan.create_fake_DAS_msg2(highLowBeamStatus,
highLowBeamReason,
ahbIsEnabled,
self.fleet_speed_state))
if (self.cc_counter < 3) and (self.fleet_speed_state == 0x02):
CS.v_cruise_pcm = CS.v_cruise_pcm + 1
send_fake_msg = True
if (self.cc_counter == 3):
send_fake_msg = True
if send_fake_msg:
if enable_steer_control and op_status == 3:
op_status = 0x5
park_brake_request = 0 #experimental; disabled for now
if park_brake_request == 1:
print("Park Brake Request received")
adaptive_cruise = 1 if (
not self.PCC.pcc_available
and self.ACC.adaptive) or self.PCC.pcc_available else 0
can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_apUnavailable, collision_warning, op_status, \
acc_speed_kph, \
self.blinker.override_direction,forward_collision_warning, adaptive_cruise, hands_on_state, \
cc_state, 1 if self.PCC.pcc_available else 0, alca_state, \
CS.v_cruise_pcm,
CS.DAS_fusedSpeedLimit,
apply_angle,
1 if enable_steer_control else 0,
park_brake_request))
if send_fake_warning or (self.opState == 2) or (self.opState == 5) or (
self.stopSignWarning != self.stopSignWarning_last) or (
self.stopLightWarning != self.stopLightWarning_last) or (
self.warningNeeded == 1) or (frame % 100 == 0):
#if it's time to send OR we have a warning or emergency disable
can_sends.append(teslacan.create_fake_DAS_warning(self.DAS_211_accNoSeatBelt, CS.DAS_canErrors, \
self.DAS_202_noisyEnvironment, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation, \
self.stopSignWarning, self.stopLightWarning, \
self.DAS_222_accCameraBlind, self.DAS_219_lcTempUnavailableSpeed, self.DAS_220_lcTempUnavailableRoad, self.DAS_221_lcAborting, \
self.DAS_207_lkasUnavailable,self.DAS_208_rackDetected, self.DAS_025_steeringOverride,self.ldwStatus,CS.useWithoutHarness,CS.usesApillarHarness))
self.stopLightWarning_last = self.stopLightWarning
self.stopSignWarning_last = self.stopSignWarning
self.warningNeeded = 0
# end of DAS emulation """
if frame % 100 == 0: # and CS.hasTeslaIcIntegration:
#IF WE HAVE softPanda RUNNING, send a message every second to say we are still awake
can_sends.append(teslacan.create_fake_IC_msg())
# send enabled ethernet every 0.2 sec
if frame % 20 == 0:
can_sends.append(teslacan.create_enabled_eth_msg(1))
if (not self.PCC.pcc_available
) and frame % 5 == 0: # acc processed at 20Hz
cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \
self.speed_limit_ms * CV.MS_TO_KPH,
self.set_speed_limit_active, self.speed_limit_offset)
if cruise_btn:
cruise_msg = teslacan.create_cruise_adjust_msg(
spdCtrlLvr_stat=cruise_btn,
turnIndLvr_Stat=0,
real_steering_wheel_stalk=CS.steering_wheel_stalk)
# Send this CAN msg first because it is racing against the real stalk.
can_sends.insert(0, cruise_msg)
apply_accel = 0.
if self.PCC.pcc_available and frame % 5 == 0: # pedal processed at 20Hz
pedalcan = 2
if CS.useWithoutHarness:
pedalcan = 0
apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \
self.speed_limit_ms,
self.set_speed_limit_active, self.speed_limit_offset * CV.KPH_TO_MS, self.alca_enabled)
can_sends.append(
teslacan.create_pedal_command_msg(apply_accel,
int(accel_needed), accel_idx,
pedalcan))
self.last_angle = apply_angle
self.last_accel = apply_accel
return pedal_can_sends + can_sends
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
wifiIpAddress = "N/A"
wifi_ssid = "---"
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
HARDWARE.initialize_hardware()
thermal_config = HARDWARE.get_thermal_config()
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)
# sound trigger
sound_trigger = 1
opkrAutoShutdown = 0
shutdown_trigger = 1
is_openpilot_view_enabled = 0
env = dict(os.environ)
env['LD_LIBRARY_PATH'] = mediaplayer
getoff_alert = params.get_bool("OpkrEnableGetoffAlert")
hotspot_on_boot = params.get_bool("OpkrHotspotOnBoot")
hotspot_run = False
if int(params.get("OpkrAutoShutdown", encoding="utf8")) == 0:
opkrAutoShutdown = 0
elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 1:
opkrAutoShutdown = 5
elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 2:
opkrAutoShutdown = 30
elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 3:
opkrAutoShutdown = 60
elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 4:
opkrAutoShutdown = 180
elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 5:
opkrAutoShutdown = 300
elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 6:
opkrAutoShutdown = 600
elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 7:
opkrAutoShutdown = 1800
elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 8:
opkrAutoShutdown = 3600
elif int(params.get("OpkrAutoShutdown", encoding="utf8")) == 9:
opkrAutoShutdown = 10800
else:
opkrAutoShutdown = 18000
battery_charging_control = params.get_bool("OpkrBatteryChargingControl")
battery_charging_min = int(params.get("OpkrBatteryChargingMin", encoding="utf8"))
battery_charging_max = int(params.get("OpkrBatteryChargingMax", encoding="utf8"))
is_openpilot_dir = True
while 1:
ts = sec_since_boot()
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
shutdown_trigger = 1
else:
no_panda_cnt = 0
startup_conditions["ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan
sound_trigger == 1
#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 (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.clear_all(ParamKeyType.CLEAR_ON_PANDA_DISCONNECT)
pandaState_prev = pandaState
elif params.get_bool("IsOpenpilotViewEnabled") and not params.get_bool("IsDriverViewEnabled") and is_openpilot_view_enabled == 0:
is_openpilot_view_enabled = 1
startup_conditions["ignition"] = True
elif not params.get_bool("IsOpenpilotViewEnabled") and not params.get_bool("IsDriverViewEnabled") and is_openpilot_view_enabled == 1:
shutdown_trigger = 0
sound_trigger == 0
is_openpilot_view_enabled = 0
startup_conditions["ignition"] = False
# 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, wifi_ssid = 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
wifiIpAddress = HARDWARE.get_ip_address()
try:
ping_test = subprocess.check_output(["ping", "-c", "1", "-W", "1", "google.com"])
Params().put("LastAthenaPingTime", str(int(sec_since_boot() * 1e9))) if ping_test else False
except Exception:
Params().delete("LastAthenaPingTime")
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.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent()))
msg.deviceState.networkType = network_type
msg.deviceState.networkStrength = network_strength
msg.deviceState.wifiSSID = wifi_ssid
if network_info is not None:
msg.deviceState.networkInfo = network_info
msg.deviceState.wifiIpAddress = wifiIpAddress
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"] = True if ((now.year > 2020) or (now.year == 2020 and now.month >= 10)) else True # set True for battery less EON otherwise, set False.
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_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()
if shutdown_trigger == 1 and sound_trigger == 1 and msg.deviceState.batteryStatus == "Discharging" and started_seen and (sec_since_boot() - off_ts) > 1 and getoff_alert:
subprocess.Popen([mediaplayer + 'mediaplayer', '/data/openpilot/selfdrive/assets/sounds/eondetach.wav'], shell = False, stdin=None, stdout=None, stderr=None, env = env, close_fds=True)
sound_trigger = 0
# shutdown if the battery gets lower than 3%, it's discharging, we aren't running for
# more than a minute but we were running
if shutdown_trigger == 1 and msg.deviceState.batteryStatus == "Discharging" and \
started_seen and opkrAutoShutdown and (sec_since_boot() - off_ts) > opkrAutoShutdown and not os.path.isfile(pandaflash_ongoing):
os.system('LD_LIBRARY_PATH="" svc power shutdown')
if (count % int(1. / DT_TRML)) == 0:
if int(params.get("OpkrForceShutdown", encoding="utf8")) != 0 and not started_seen and msg.deviceState.batteryStatus == "Discharging":
shutdown_option = int(params.get("OpkrForceShutdown", encoding="utf8"))
if shutdown_option == 1:
opkrForceShutdown = 60
elif shutdown_option == 2:
opkrForceShutdown = 180
elif shutdown_option == 3:
opkrForceShutdown = 300
elif shutdown_option == 4:
opkrForceShutdown = 600
else:
opkrForceShutdown = 1800
if (sec_since_boot() - off_ts) > opkrForceShutdown and params.get_bool("OpkrForceShutdownTrigger"):
os.system('LD_LIBRARY_PATH="" svc power shutdown')
elif not params.get_bool("OpkrForceShutdownTrigger"):
off_ts = sec_since_boot()
# opkr
prebuiltlet = params.get_bool("PutPrebuiltOn")
if not os.path.isdir("/data/openpilot"):
if is_openpilot_dir:
os.system("cd /data/params/d; rm -f DongleId") # Delete DongleID if the Openpilot directory disappears, Seems you want to switch fork/branch.
is_openpilot_dir = False
elif not os.path.isfile(prebuiltfile) and prebuiltlet and is_openpilot_dir:
os.system("cd /data/openpilot; touch prebuilt")
elif os.path.isfile(prebuiltfile) and not prebuiltlet:
os.system("cd /data/openpilot; rm -f prebuilt")
# opkr
sshkeylet = params.get_bool("OpkrSSHLegacy")
if not os.path.isfile(sshkeyfile) and sshkeylet:
os.system("cp -f /data/openpilot/selfdrive/assets/addon/key/GithubSshKeys_legacy /data/params/d/GithubSshKeys; chmod 600 /data/params/d/GithubSshKeys; touch /data/public_key")
elif os.path.isfile(sshkeyfile) and not sshkeylet:
os.system("cp -f /data/openpilot/selfdrive/assets/addon/key/GithubSshKeys_new /data/params/d/GithubSshKeys; chmod 600 /data/params/d/GithubSshKeys; rm -f /data/public_key")
# opkr hotspot
if hotspot_on_boot and not hotspot_run and sec_since_boot() > 80:
os.system("service call wifi 37 i32 0 i32 1 &")
hotspot_run = True
# 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()
# atom
if usb_power and battery_charging_control:
power_monitor.charging_ctrl( msg, ts, battery_charging_max, battery_charging_min )
# 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 update(self, CS, frame, ahbLead1):
tms_now = _current_time_millis()
ahbInfoMsg = self.ahbInfo.receive(non_blocking=True)
frameInfoMsg = messaging.recv_one_or_none(self.frameInfo)
if ahbInfoMsg is not None:
self.ahbInfoData = tesla.AHBinfo.from_bytes(ahbInfoMsg)
if frameInfoMsg is not None:
self.frameInfoData = frameInfoMsg.frame
frameInfoGain = self.frameInfoData.globalGain
exposureTime = self.frameInfoData.androidCaptureResult.exposureTime
frameDuration = self.frameInfoData.androidCaptureResult.frameDuration
if frameInfoGain != self.frameInfoGain:
self.frameInfoGain = frameInfoGain
_debug("AHB Camera has new data [ frame - " +
str(self.frameInfoData.frameId) + "] = " +
str(frameInfoGain) + ", exposure = " + str(exposureTime) +
", frame duration = " + str(frameDuration))
#if AHB not enabled, then return OFF
if CS.ahbEnabled != 1:
_debug("AHB not enabled in CID")
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
#if stalk is not in the high beam position, return UNDECIDED
if (CS.ahbHighBeamStalkPosition != 1):
_debug("High Beam not on")
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA
self.ahbIsEnabled = False
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
else:
self.ahbIsEnabled = True
#if moving below 10mph take no decision, then return undecided
if CS.v_ego < 4.47:
_debug("moving too slow for decision")
highLowBeamStatus = self.prev_highLowBeamStatus
highLowBeamReason = self.prev_highLowBeamReason
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
if self.ahbInfoData is None:
_debug("No radar info")
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_SNA
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
# if gain less than max, we might see incoming car
if self.frameInfoGain < 510:
_debug("OP camera gain < 510")
self.time_last_car_detected = tms_now
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
#if lead car detected by radarD, i.e. OP has Lead, then reset timer and return OFF
if False and ahbLead1 is not None:
_debug("OP detected car")
self.time_last_car_detected = tms_now
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
#if lead car detected by radar, then reset timer and return OFF
if self.ahbInfoData.radarCarDetected:
_debug("Radar detected car")
self.time_last_car_detected = tms_now
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
#if lead car detected by vision, then reset timer and return OFF
if self.ahbInfoData.cameraCarDetected:
_debug("Vision detected car")
self.time_last_car_detected = tms_now
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_VISION_TARGET
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
#if still waiting for the delay after car detected, send off
if (tms_now - self.time_last_car_detected < 1000 * CS.ahbOffDuration):
_debug("Waiting for time delay since last car")
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_OFF
highLowBeamReason = AHBReason.HIGH_BEAM_OFF_REASON_MOVING_RADAR_TARGET
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
#we got here, high beam should be on
highLowBeamStatus = AHBDecision.DAS_HIGH_BEAM_UNDECIDED
highLowBeamReason = AHBReason.HIGH_BEAM_ON
_debug("All conditions met, turn lights ON")
return self.set_and_return(CS, frame, highLowBeamStatus,
highLowBeamReason)
def mpc_vwr_thread(addr="127.0.0.1"):
plt.ion()
fig = plt.figure(figsize=(15, 15))
ax = fig.add_subplot(1, 1, 1)
ax.set_xlim([-SCALE, SCALE])
ax.set_ylim([-SCALE, SCALE])
ax.grid(True)
line, = ax.plot([0.0], [0.0], ".b")
line2, = ax.plot([0.0], [0.0], 'r')
ax.set_aspect('equal', 'datalim')
plt.show()
live_location = messaging.sub_sock('liveLocation',
addr=addr,
conflate=True)
gps_planner_points = messaging.sub_sock('gpsPlannerPoints', conflate=True)
gps_planner_plan = messaging.sub_sock('gpsPlannerPlan', conflate=True)
last_points = messaging.recv_one(gps_planner_points)
last_plan = messaging.recv_one(gps_planner_plan)
while True:
p = messaging.recv_one_or_none(gps_planner_points)
pl = messaging.recv_one_or_none(gps_planner_plan)
ll = messaging.recv_one(live_location).liveLocation
if p is not None:
last_points = p
if pl is not None:
last_plan = pl
if not last_plan.gpsPlannerPlan.valid:
time.sleep(0.1)
line2.set_color('r')
continue
p0 = last_points.gpsPlannerPoints.points[0]
p0 = np.array([p0.x, p0.y, p0.z])
n = LocalCoord.from_geodetic(np.array([ll.lat, ll.lon, ll.alt]))
points = []
print(len(last_points.gpsPlannerPoints.points))
for p in last_points.gpsPlannerPoints.points:
ecef = np.array([p.x, p.y, p.z])
points.append(n.ecef2ned(ecef))
points = np.vstack(points)
line.set_xdata(points[:, 1])
line.set_ydata(points[:, 0])
y = np.matrix(np.arange(-100, 100.0, 0.5))
x = -np.matrix(np.polyval(last_plan.gpsPlannerPlan.poly, y))
xy = np.hstack([x.T, y.T])
cur_heading = np.radians(ll.heading - 90)
c, s = np.cos(cur_heading), np.sin(cur_heading)
R = np.array([[c, -s], [s, c]])
xy = xy.dot(R)
line2.set_xdata(xy[:, 1])
line2.set_ydata(-xy[:, 0])
line2.set_color('g')
ax.set_xlim([-SCALE, SCALE])
ax.set_ylim([-SCALE, SCALE])
fig.canvas.draw()
fig.canvas.flush_events()
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 update_pdl(
self,
enabled,
CS,
frame,
actuators,
pcm_speed,
pcm_override,
speed_limit_ms,
set_speed_limit_active,
speed_limit_offset,
alca_enabled,
):
idx = self.pedal_idx
self.prev_speed_limit_kph = self.speed_limit_kph
######################################################################################
# Determine pedal "zero"
#
# save position for cruising (zero acc, zero brake, no torque) when we are above 10 MPH
######################################################################################
if (
CS.torqueLevel < TORQUE_LEVEL_ACC
and CS.torqueLevel > TORQUE_LEVEL_DECEL
and CS.v_ego >= 10.0 * CV.MPH_TO_MS
and abs(CS.torqueLevel) < abs(self.lastTorqueForPedalForZeroTorque)
and self.prev_tesla_accel > 0.0
):
self.PedalForZeroTorque = self.prev_tesla_accel
self.lastTorqueForPedalForZeroTorque = CS.torqueLevel
# print ("Detected new Pedal For Zero Torque at %s" % (self.PedalForZeroTorque))
# print ("Torque level at detection %s" % (CS.torqueLevel))
# print ("Speed level at detection %s" % (CS.v_ego * CV.MS_TO_MPH))
if set_speed_limit_active and speed_limit_ms > 0:
self.speed_limit_kph = (speed_limit_ms + speed_limit_offset) * CV.MS_TO_KPH
if int(self.prev_speed_limit_kph) != int(self.speed_limit_kph):
self.pedal_speed_kph = self.speed_limit_kph
# reset MovingAverage for fleet speed when speed limit changes
self.fleet_speed.reset_averager()
else: # reset internal speed limit, so double pull doesn't set higher speed than current (e.g. after leaving the highway)
self.speed_limit_kph = 0.0
self.pedal_idx = (self.pedal_idx + 1) % 16
if not self.pcc_available or not enabled:
return 0.0, 0, idx
# Alternative speed decision logic that uses the lead car's distance
# and speed more directly.
# Bring in the lead car distance from the radarState feed
radSt = messaging.recv_one_or_none(self.radarState)
mapd = messaging.recv_one_or_none(self.live_map_data)
if radSt is not None:
self.lead_1 = radSt.radarState.leadOne
if _is_present(self.lead_1):
self.lead_last_seen_time_ms = _current_time_millis()
self.continuous_lead_sightings += 1
else:
self.continuous_lead_sightings = 0
v_ego = CS.v_ego
following = False
if self.lead_1:
following = (
self.lead_1.status
and self.lead_1.dRel < MAX_RADAR_DISTANCE
and self.lead_1.vLeadK > v_ego
and self.lead_1.aLeadK > 0.0
)
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, following)
]
accel_limits[1] *= _accel_limit_multiplier(CS, self.lead_1)
accel_limits[0] = _decel_limit(
accel_limits[0], CS.v_ego, self.lead_1, CS, self.pedal_speed_kph
)
jerk_limits = [
min(-0.1, accel_limits[0] / 2.0),
max(0.1, accel_limits[1] / 2.0),
] # TODO: make a separate lookup for jerk tuning
accel_limits = limit_accel_in_turns(v_ego, CS.angle_steers, accel_limits, CS.CP)
output_gb = 0
####################################################################
# this mode (Follow) uses the Follow logic created by JJ for ACC
#
# once the speed is detected we have to use our own PID to determine
# how much accel and break we have to do
####################################################################
# Broken in 0.7.9
#if PCCModes.is_selected(FollowMode(), CS.cstm_btns):
if False:
MPC_BRAKE_MULTIPLIER = 6.0
enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [
LongCtrlState.pid,
LongCtrlState.stopping,
]
# determine if pedal is pressed by human
self.prev_accelerator_pedal_pressed = self.accelerator_pedal_pressed
self.accelerator_pedal_pressed = CS.pedal_interceptor_value > 5
# reset PID if we just lifted foot of accelerator
if (
not self.accelerator_pedal_pressed
) and self.prev_accelerator_pedal_pressed:
self.reset(CS.v_ego)
if self.enable_pedal_cruise and not self.accelerator_pedal_pressed:
self.v_pid = self.calc_follow_speed_ms(CS, alca_enabled) or 0
if mapd is not None:
v_curve = max_v_in_mapped_curve_ms(
mapd.liveMapData, self.pedal_speed_kph
)
if v_curve:
self.v_pid = min(self.v_pid, v_curve)
# take fleet speed into consideration
self.v_pid = min(
self.v_pid,
self.fleet_speed.adjust(
CS, self.pedal_speed_kph * CV.KPH_TO_MS, frame
),
)
# cruise speed can't be negative even if user is distracted
self.v_pid = max(self.v_pid, 0.0)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start,
self.a_acc_start,
self.v_pid,
accel_limits[1],
accel_limits[0],
jerk_limits[1],
jerk_limits[0],
_DT_MPC,
)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.0)
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = self.v_pid
# Interpolation of trajectory
self.a_acc_sol = self.a_acc_start + (_DT / _DT_MPC) * (
self.a_acc - self.a_acc_start
)
self.v_acc_sol = (
self.v_acc_start + _DT * (self.a_acc_sol + self.a_acc_start) / 2.0
)
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
# we will try to feed forward the pedal position.... we might want to feed the last_output_gb....
# op feeds forward self.a_acc_sol
# it's all about testing now.
vTarget = clip(self.v_acc_sol, 0, self.v_cruise)
self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid)
feedforward = self.a_acc_sol
# feedforward = self.last_output_gb
t_go, t_brake = self.LoC.update(
self.enable_pedal_cruise,
# randomly disabling stuff during 0.7.9 merge to prevent runtime crashes
#CS.v_ego,
#CS.brake_pressed != 0,
#CS.standstill,
#False,
self.v_cruise,
vTarget,
self.vTargetFuture,
feedforward,
CS.CP,
)
output_gb = t_go - t_brake
# print ("Output GB Follow:", output_gb)
else:
self.reset(CS.v_ego)
# print ("PID reset")
output_gb = 0.0
starting = self.LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.a_ego, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.v_ego
reset_accel = CS.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.v_pid = reset_speed
self.last_speed_kph = None
##############################################################
# This mode uses the longitudinal MPC built in OP
#
# we use the values from actuators.gas and actuators.brake
##############################################################
#elif PCCModes.is_selected(OpMode(), CS.cstm_btns):
else:
output_gb = actuators.gas - actuators.brake
self.v_pid = pcm_speed
MPC_BRAKE_MULTIPLIER = 12.0
self.last_output_gb = output_gb
# accel and brake
apply_accel = clip(
output_gb, 0.0, 1
) # _accel_pedal_max(CS.v_ego, self.v_pid, self.lead_1, self.prev_tesla_accel, CS))
apply_brake = -clip(
output_gb * MPC_BRAKE_MULTIPLIER,
_brake_pedal_min(
CS.v_ego, self.v_pid, self.lead_1, CS, self.pedal_speed_kph
),
0.0,
)
# if speed is over 5mph, the "zero" is at PedalForZeroTorque; otherwise it is zero
pedal_zero = 0.0
if CS.v_ego >= 5.0 * CV.MPH_TO_MS:
pedal_zero = self.PedalForZeroTorque
tesla_brake = clip((1.0 - apply_brake) * pedal_zero, 0, pedal_zero)
tesla_accel = clip(
apply_accel * (MAX_PEDAL_VALUE - pedal_zero),
0,
MAX_PEDAL_VALUE - pedal_zero,
)
tesla_pedal = tesla_brake + tesla_accel
tesla_pedal = self.pedal_hysteresis(tesla_pedal, enabled)
tesla_pedal = clip(
tesla_pedal,
self.prev_tesla_pedal - PEDAL_MAX_DOWN,
self.prev_tesla_pedal + PEDAL_MAX_UP,
)
tesla_pedal = (
clip(tesla_pedal, 0.0, MAX_PEDAL_VALUE) if self.enable_pedal_cruise else 0.0
)
enable_pedal = 1.0 if self.enable_pedal_cruise else 0.0
self.torqueLevel_last = CS.torqueLevel
self.prev_tesla_pedal = tesla_pedal * enable_pedal
self.prev_tesla_accel = apply_accel * enable_pedal
self.prev_v_ego = CS.v_ego
return self.prev_tesla_pedal, enable_pedal, idx
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)
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
power_monitor = PowerMonitoring()
no_panda_cnt = 0
HARDWARE.initialize_hardware()
thermal_config = HARDWARE.get_thermal_config()
if params.get_bool("IsOnroad"):
cloudlog.event("onroad flag not cleared")
# dp
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_last = False
dp_auto_shutdown_in = 90
dp_last_modified_auto_shutdown_in = None
dp_auto_shutdown_in_last = 90
dp_fan_mode = 0
dp_fan_mode_last = None
modified = None
last_modified = None
last_modified_check = None
#dp
prebuilt_file = '/data/openpilot/prebuilt'
dp_prebuilt = params.get_bool("dp_prebuilt")
if not os.path.isfile(prebuilt_file) and dp_prebuilt:
os.system("touch /data/openpilot/prebuilt")
elif os.path.isfile(prebuilt_file) and not dp_prebuilt:
os.system("rm -fr /data/openpilot/prebuilt")
# 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)
# dp - light up = not started
if JETSON:
HARDWARE.led(True)
while 1:
# 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
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 not JETSON and 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.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(psutil.cpu_percent()))
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.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 or dp_no_batt:
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(dp_fan_mode, 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
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())
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()
# dp - show led when off and vice versa
if should_start != should_start_prev:
HARDWARE.led(False if should_start else True)
# Offroad power monitoring
if not dp_no_batt:
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()
# dp - auto shutdown
# reset off_ts if we change auto shutdown related params
if off_ts is not None:
if dp_auto_shutdown:
shutdown_sec = dp_auto_shutdown_in * 60
sec_now = sec_since_boot() - off_ts
if (shutdown_sec - 5) < sec_now:
msg.deviceState.chargingDisabled = True
if shutdown_sec < sec_now:
time.sleep(10)
HARDWARE.shutdown()
if dp_auto_shutdown_in_last != dp_auto_shutdown_in or dp_auto_shutdown_last != dp_auto_shutdown:
off_ts = sec_since_boot()
dp_auto_shutdown_last = dp_auto_shutdown
dp_auto_shutdown_in_last = dp_auto_shutdown_in
# 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
speed_txt = ax.text(-500, 900, '')
curv_txt = ax.text(-500, 775, '')
ax = fig.add_subplot(2, 1, 2)
ax.set_title('Curvature')
curvature_plt, = ax.plot([0.0], [0.0], "--xk")
ax.set_xlim([0, 500])
ax.set_ylim([0, 1e-2])
ax.set_xlabel('Distance along path [m]')
ax.set_ylabel('Curvature [1/m]')
ax.grid(True)
plt.show()
while True:
m = messaging.recv_one_or_none(live_map_sock)
p = messaging.recv_one_or_none(plan_sock)
if p is not None:
v = p.plan.vCurvature * CV.MS_TO_MPH
speed_txt.set_text('Desired curvature speed: %.2f mph' % v)
if m is not None:
print("Current way id: %d" % m.liveMapData.wayId)
curv_txt.set_text(
'Curvature valid: %s Dist: %03.0f m\nSpeedlimit valid: %s Speed: %.0f mph'
% (str(m.liveMapData.curvatureValid), m.liveMapData.distToTurn,
str(m.liveMapData.speedLimitValid),
m.liveMapData.speedLimit * CV.MS_TO_MPH))
points_plt.set_xdata(m.liveMapData.roadX)
points_plt.set_ydata(m.liveMapData.roadY)