def handle_agnos_update(wait_helper: WaitTimeHelper) -> None:
from system.hardware.tici.agnos import flash_agnos_update, get_target_slot_number
cur_version = HARDWARE.get_os_version()
updated_version = run([
"bash", "-c", r"unset AGNOS_VERSION && source launch_env.sh && \
echo -n $AGNOS_VERSION"
], OVERLAY_MERGED).strip()
cloudlog.info(f"AGNOS version check: {cur_version} vs {updated_version}")
if cur_version == updated_version:
return
# prevent an openpilot getting swapped in with a mismatched or partially downloaded agnos
set_consistent_flag(False)
cloudlog.info(
f"Beginning background installation for AGNOS {updated_version}")
set_offroad_alert("Offroad_NeosUpdate", True)
manifest_path = os.path.join(OVERLAY_MERGED,
"system/hardware/tici/agnos.json")
target_slot_number = get_target_slot_number()
flash_agnos_update(manifest_path, target_slot_number, cloudlog)
set_offroad_alert("Offroad_NeosUpdate", False)
def plannerd_thread(sm=None, pm=None):
config_realtime_process(5, Priority.CTRL_LOW)
cloudlog.info("plannerd is waiting for CarParams")
params = Params()
CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
cloudlog.info("plannerd got CarParams: %s", CP.carName)
use_lanelines = False
wide_camera = params.get_bool('WideCameraOnly')
cloudlog.event("e2e mode", on=use_lanelines)
longitudinal_planner = Planner(CP)
lateral_planner = LateralPlanner(use_lanelines=use_lanelines,
wide_camera=wide_camera)
if sm is None:
sm = messaging.SubMaster(
['carState', 'controlsState', 'radarState', 'modelV2'],
poll=['radarState', 'modelV2'],
ignore_avg_freq=['radarState'])
if pm is None:
pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan'])
while True:
sm.update()
if sm.updated['modelV2']:
lateral_planner.update(sm)
lateral_planner.publish(sm, pm)
longitudinal_planner.update(sm)
longitudinal_planner.publish(sm, pm)
def deleter_thread(exit_event):
while not exit_event.is_set():
out_of_bytes = get_available_bytes(default=MIN_BYTES + 1) < MIN_BYTES
out_of_percent = get_available_percent(default=MIN_PERCENT +
1) < MIN_PERCENT
if out_of_percent or out_of_bytes:
# remove the earliest directory we can
dirs = sorted(listdir_by_creation(ROOT),
key=lambda x: x in DELETE_LAST)
for delete_dir in dirs:
delete_path = os.path.join(ROOT, delete_dir)
if any(
name.endswith(".lock")
for name in os.listdir(delete_path)):
continue
try:
cloudlog.info(f"deleting {delete_path}")
if os.path.isfile(delete_path):
os.remove(delete_path)
else:
shutil.rmtree(delete_path)
break
except OSError:
cloudlog.exception(f"issue deleting {delete_path}")
exit_event.wait(.1)
else:
exit_event.wait(30)
def manager_thread() -> None:
cloudlog.bind(daemon="manager")
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
params = Params()
ignore: List[str] = []
if params.get("DongleId",
encoding='utf8') in (None, UNREGISTERED_DONGLE_ID):
ignore += ["manage_athenad", "uploader"]
if os.getenv("NOBOARD") is not None:
ignore.append("pandad")
ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0]
sm = messaging.SubMaster(['deviceState', 'carParams'],
poll=['deviceState'])
pm = messaging.PubMaster(['managerState'])
ensure_running(managed_processes.values(),
False,
params=params,
CP=sm['carParams'],
not_run=ignore)
while True:
sm.update()
started = sm['deviceState'].started
ensure_running(managed_processes.values(),
started,
params=params,
CP=sm['carParams'],
not_run=ignore)
running = ' '.join(
"%s%s\u001b[0m" %
("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
for p in managed_processes.values() if p.proc)
print(running)
cloudlog.debug(running)
# send managerState
msg = messaging.new_message('managerState')
msg.managerState.processes = [
p.get_process_state_msg() for p in managed_processes.values()
]
pm.send('managerState', msg)
# Exit main loop when uninstall/shutdown/reboot is needed
shutdown = False
for param in ("DoUninstall", "DoShutdown", "DoReboot"):
if params.get_bool(param):
shutdown = True
params.put("LastManagerExitReason", param)
cloudlog.warning(f"Shutting down manager - {param} set")
if shutdown:
break
def uploader_fn(exit_event):
try:
set_core_affinity([0, 1, 2, 3])
except Exception:
cloudlog.exception("failed to set core affinity")
clear_locks(ROOT)
params = Params()
dongle_id = params.get("DongleId", encoding='utf8')
if dongle_id is None:
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
if TICI and not Path("/data/media").is_mount():
cloudlog.warning("NVME not mounted")
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['uploaderState'])
uploader = Uploader(dongle_id, ROOT)
backoff = 0.1
while not exit_event.is_set():
sm.update(0)
offroad = params.get_bool("IsOffroad")
network_type = sm[
'deviceState'].networkType if not force_wifi else NetworkType.wifi
if network_type == NetworkType.none:
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
d = uploader.next_file_to_upload()
if d is None: # Nothing to upload
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
name, key, fn = d
# qlogs and bootlogs need to be compressed before uploading
if key.endswith(('qlog', 'rlog')) or (key.startswith('boot/')
and not key.endswith('.bz2')):
key += ".bz2"
success = uploader.upload(name, key, fn,
sm['deviceState'].networkType.raw,
sm['deviceState'].networkMetered)
if success:
backoff = 0.1
elif allow_sleep:
cloudlog.info("upload backoff %r", backoff)
time.sleep(backoff + random.uniform(0, backoff))
backoff = min(backoff * 2, 120)
pm.send("uploaderState", uploader.get_msg())
def manager_cleanup() -> None:
# send signals to kill all procs
for p in managed_processes.values():
p.stop(block=False)
# ensure all are killed
for p in managed_processes.values():
p.stop(block=True)
cloudlog.info("everything is dead")
def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types):
astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types)
cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}")
start_time = time.monotonic()
try:
astro_dog.get_orbit_data(t, only_predictions=True)
cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.1f}s")
return astro_dog.orbits, astro_dog.orbit_fetched_times, t
except (RuntimeError, ValueError, IOError) as e:
cloudlog.warning(f"No orbit data found or parsing failure: {e}")
return None, None, t
def __init__(self) -> None:
super().__init__()
cloudlog.info("Setting up TICI fan handler")
self.last_ignition = False
self.controller = PIDController(k_p=0,
k_i=4e-3,
k_f=1,
neg_limit=-80,
pos_limit=0,
rate=(1 / DT_TRML))
def graceful_shutdown(self, signum: int, frame) -> None:
# umount -f doesn't appear effective in avoiding "device busy" on NEOS,
# so don't actually die until the next convenient opportunity in main().
cloudlog.info(
"caught SIGINT/SIGTERM, dismounting overlay at next opportunity")
# forward the signal to all our child processes
child_procs = self.proc.children(recursive=True)
for p in child_procs:
p.send_signal(signum)
self.shutdown = True
self.ready_event.set()
def start(self) -> None:
# In case we only tried a non blocking stop we need to stop it before restarting
if self.shutting_down:
self.stop()
if self.proc is not None:
return
cloudlog.info(f"starting python {self.module}")
self.proc = Process(name=self.name, target=launcher, args=(self.module, self.name))
self.proc.start()
self.watchdog_seen = False
self.shutting_down = False
def finalize_update(wait_helper: WaitTimeHelper) -> None:
"""Take the current OverlayFS merged view and finalize a copy outside of
OverlayFS, ready to be swapped-in at BASEDIR. Copy using shutil.copytree"""
# Remove the update ready flag and any old updates
cloudlog.info("creating finalized version of the overlay")
set_consistent_flag(False)
# Copy the merged overlay view and set the update ready flag
if os.path.exists(FINALIZED):
shutil.rmtree(FINALIZED)
shutil.copytree(OVERLAY_MERGED, FINALIZED, symlinks=True)
run(["git", "reset", "--hard"], FINALIZED)
run(["git", "submodule", "foreach", "--recursive", "git", "reset"],
FINALIZED)
cloudlog.info("Starting git gc")
t = time.monotonic()
try:
run(["git", "gc"], FINALIZED)
cloudlog.event("Done git gc", duration=time.monotonic() - t)
except subprocess.CalledProcessError:
cloudlog.exception(f"Failed git gc, took {time.monotonic() - t:.3f} s")
if wait_helper.shutdown:
cloudlog.info("got interrupted finalizing overlay")
else:
set_consistent_flag(True)
cloudlog.info("done finalizing overlay")
def start(self) -> None:
# In case we only tried a non blocking stop we need to stop it before restarting
if self.shutting_down:
self.stop()
if self.proc is not None:
return
cwd = os.path.join(BASEDIR, self.cwd)
cloudlog.info(f"starting process {self.name}")
self.proc = Process(name=self.name, target=nativelauncher, args=(self.cmdline, cwd, self.name))
self.proc.start()
self.watchdog_seen = False
self.shutting_down = False
def signal(self, sig: int) -> None:
if self.proc is None:
return
# Don't signal if already exited
if self.proc.exitcode is not None and self.proc.pid is not None:
return
# Can't signal if we don't have a pid
if self.proc.pid is None:
return
cloudlog.info(f"sending signal {sig} to {self.name}")
os.kill(self.proc.pid, sig)
def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]):
# Check time and outputs are valid
valid = self.kf_valid(t)
if not all(valid):
if not valid[0]: # Filter not initialized
pass
elif not valid[1]:
cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
elif not valid[2]:
cloudlog.error("Gnss kalman filter state is nan")
if len(est_pos) > 0:
cloudlog.info(f"Reset kalman filter with {est_pos}")
self.init_gnss_localizer(est_pos)
else:
return
if len(measurements) > 0:
kf_add_observations(self.gnss_kf, t, measurements)
else:
# Ensure gnss filter is updated even with no new measurements
self.gnss_kf.predict(t)
def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types,
cache_dir):
astro_dog = AstroDog(valid_const=valid_const,
auto_update=auto_update,
valid_ephem_types=valid_ephem_types,
cache_dir=cache_dir)
cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}")
start_time = time.monotonic()
try:
astro_dog.get_orbit_data(t, only_predictions=True)
cloudlog.info(
f"Done parsing orbits. Took {time.monotonic() - start_time:.1f}s")
cloudlog.debug(
f"Downloaded orbits ({sum([len(v) for v in astro_dog.orbits])}): {list(astro_dog.orbits.keys())}"
+
f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in astro_dog.orbit_fetched_times._ranges]}"
)
return astro_dog.orbits, astro_dog.orbit_fetched_times, t
except (DownloadFailed, RuntimeError, ValueError, IOError) as e:
cloudlog.warning(f"No orbit data found or parsing failure: {e}")
return None, None, t
def radard_thread(sm=None, pm=None, can_sock=None):
config_realtime_process(5, Priority.CTRL_LOW)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(
f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface
# *** setup messaging
if can_sock is None:
can_sock = messaging.sub_sock('can')
if sm is None:
sm = messaging.SubMaster(
['modelV2', 'carState'], ignore_avg_freq=[
'modelV2', 'carState'
]) # Can't check average frequency, since radar determines timing
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
# TODO: always log leads once we can hide them conditionally
enable_lead = CP.openpilotLongitudinalControl or not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
if rr is None:
continue
sm.update(0)
dat = RD.update(sm, rr, enable_lead)
dat.radarState.cumLagMs = -rk.remaining * 1000.
pm.send('radarState', dat)
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message('liveTracks', len(tracks))
for cnt, ids in enumerate(sorted(tracks.keys())):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
pm.send('liveTracks', dat)
rk.monitor_time()
def main() -> NoReturn:
sentry.init(sentry.SentryProject.SELFDRIVE_NATIVE)
# Clear apport folder on start, otherwise duplicate crashes won't register
clear_apport_folder()
initial_tombstones = set(get_tombstones())
while True:
now_tombstones = set(get_tombstones())
for fn, _ in (now_tombstones - initial_tombstones):
try:
cloudlog.info(f"reporting new tombstone {fn}")
if fn.endswith(".crash"):
report_tombstone_apport(fn)
else:
report_tombstone_android(fn)
except Exception:
cloudlog.exception(f"Error reporting tombstone {fn}")
initial_tombstones = now_tombstones
time.sleep(5)
def init_overlay() -> None:
overlay_init_file = Path(os.path.join(BASEDIR, ".overlay_init"))
# Re-create the overlay if BASEDIR/.git has changed since we created the overlay
if overlay_init_file.is_file():
git_dir_path = os.path.join(BASEDIR, ".git")
new_files = run(
["find", git_dir_path, "-newer",
str(overlay_init_file)])
if not len(new_files.splitlines()):
# A valid overlay already exists
return
else:
cloudlog.info(".git directory changed, recreating overlay")
cloudlog.info("preparing new safe staging area")
params = Params()
params.put_bool("UpdateAvailable", False)
set_consistent_flag(False)
dismount_overlay()
run(["sudo", "rm", "-rf", STAGING_ROOT])
if os.path.isdir(STAGING_ROOT):
shutil.rmtree(STAGING_ROOT)
for dirname in [
STAGING_ROOT, OVERLAY_UPPER, OVERLAY_METADATA, OVERLAY_MERGED
]:
os.mkdir(dirname, 0o755)
if os.lstat(BASEDIR).st_dev != os.lstat(OVERLAY_MERGED).st_dev:
raise RuntimeError(
"base and overlay merge directories are on different filesystems; not valid for overlay FS!"
)
# Leave a timestamped canary in BASEDIR to check at startup. The device clock
# should be correct by the time we get here. If the init file disappears, or
# critical mtimes in BASEDIR are newer than .overlay_init, continue.sh can
# assume that BASEDIR has used for local development or otherwise modified,
# and skips the update activation attempt.
consistent_file = Path(os.path.join(BASEDIR, ".overlay_consistent"))
if consistent_file.is_file():
consistent_file.unlink()
overlay_init_file.touch()
os.sync()
overlay_opts = f"lowerdir={BASEDIR},upperdir={OVERLAY_UPPER},workdir={OVERLAY_METADATA}"
mount_cmd = [
"mount", "-t", "overlay", "-o", overlay_opts, "none", OVERLAY_MERGED
]
run(["sudo"] + mount_cmd)
run(["sudo", "chmod", "755", os.path.join(OVERLAY_METADATA, "work")])
git_diff = run(["git", "diff"], OVERLAY_MERGED, low_priority=True)
params.put("GitDiff", git_diff)
cloudlog.info(f"git diff output:\n{git_diff}")
def start(self) -> None:
params = Params()
pid = params.get(self.param_name, encoding='utf-8')
if pid is not None:
try:
os.kill(int(pid), 0)
with open(f'/proc/{pid}/cmdline') as f:
if self.module in f.read():
# daemon is running
return
except (OSError, FileNotFoundError):
# process is dead
pass
cloudlog.info(f"starting daemon {self.name}")
proc = subprocess.Popen(['python', '-m', self.module], # pylint: disable=subprocess-popen-preexec-fn
stdin=open('/dev/null'),
stdout=open('/dev/null', 'w'),
stderr=open('/dev/null', 'w'),
preexec_fn=os.setpgrp)
params.put(self.param_name, str(proc.pid))
def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types):
astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types)
cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}")
start_time = time.monotonic()
data = None
try:
astro_dog.get_orbit_data(t, only_predictions=True)
data = (astro_dog.orbits, astro_dog.orbit_fetched_times)
except RuntimeError as e:
cloudlog.info(f"No orbit data found. {e}")
cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.1f}s")
return data
def can_init():
global handle, context
handle = None
cloudlog.info("attempting can init")
context = usb1.USBContext()
#context.setDebug(9)
for device in context.getDeviceList(skip_on_error=True):
if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc:
handle = device.open()
handle.claimInterface(0)
handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'')
if handle is None:
cloudlog.warning("CAN NOT FOUND")
exit(-1)
cloudlog.info("got handle")
cloudlog.info("can init done")
def stop(self, retry: bool=True, block: bool=True) -> Optional[int]:
if self.proc is None:
return None
if self.proc.exitcode is None:
if not self.shutting_down:
cloudlog.info(f"killing {self.name}")
sig = signal.SIGKILL if self.sigkill else signal.SIGINT
self.signal(sig)
self.shutting_down = True
if not block:
return None
join_process(self.proc, 5)
# If process failed to die send SIGKILL or reboot
if self.proc.exitcode is None and retry:
if self.unkillable:
cloudlog.critical(f"unkillable process {self.name} failed to exit! rebooting in 15 if it doesn't die")
join_process(self.proc, 15)
if self.proc.exitcode is None:
cloudlog.critical(f"unkillable process {self.name} failed to die!")
os.system("date >> /data/unkillable_reboot")
os.sync()
HARDWARE.reboot()
raise RuntimeError
else:
cloudlog.info(f"killing {self.name} with SIGKILL")
self.signal(signal.SIGKILL)
self.proc.join()
ret = self.proc.exitcode
cloudlog.info(f"{self.name} is dead with {ret}")
if self.proc.exitcode is not None:
self.shutting_down = False
self.proc = None
return ret
def update(self, sm):
v_ego = sm['carState'].vEgo
v_cruise_kph = sm['controlsState'].vCruise
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise = v_cruise_kph * CV.KPH_TO_MS
long_control_state = sm['controlsState'].longControlState
force_slow_decel = sm['controlsState'].forceDecel
# Reset current state when not engaged, or user is controlling the speed
reset_state = long_control_state == LongCtrlState.off
# No change cost when user is controlling the speed, or when standstill
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
if reset_state:
self.v_desired_filter.x = v_ego
self.a_desired = 0.0
# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
accel_limits_turns = limit_accel_in_turns(
v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0],
accel_limits_turns[1])
# clip limits, cannot init MPC outside of bounds
accel_limits_turns[0] = min(accel_limits_turns[0],
self.a_desired + 0.05)
accel_limits_turns[1] = max(accel_limits_turns[1],
self.a_desired - 0.05)
self.mpc.set_weights(prev_accel_constraint)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC,
self.mpc.v_solution)
self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC,
self.mpc.a_solution)
self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N],
T_IDXS_MPC[:-1],
self.mpc.j_solution)
# TODO counter is only needed because radar is glitchy, remove once radar is gone
self.fcw = self.mpc.crash_cnt > 5
if self.fcw:
cloudlog.info("FCW triggered")
# Interpolate 0.05 seconds and save as starting point for next iteration
a_prev = self.a_desired
self.a_desired = float(
interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory))
self.v_desired_filter.x = self.v_desired_filter.x + DT_MDL * (
self.a_desired + a_prev) / 2.0
def main(sm=None, pm=None):
config_realtime_process([0, 1, 2, 3], 5)
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman'])
if pm is None:
pm = messaging.PubMaster(['liveParameters'])
params_reader = Params()
# wait for stats about the car to come in from controls
cloudlog.info("paramsd is waiting for CarParams")
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
cloudlog.info("paramsd got CarParams")
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
params = params_reader.get("LiveParameters")
# Check if car model matches
if params is not None:
params = json.loads(params)
if params.get('carFingerprint', None) != CP.carFingerprint:
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
# Check if starting values are sane
if params is not None:
try:
angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0
steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
params_sane = angle_offset_sane and steer_ratio_sane
if not params_sane:
cloudlog.info(f"Invalid starting values found {params}")
params = None
except Exception as e:
cloudlog.info(f"Error reading params {params}: {str(e)}")
params = None
# TODO: cache the params with the capnp struct
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': CP.steerRatio,
'stiffnessFactor': 1.0,
'angleOffsetAverageDeg': 0.0,
}
cloudlog.info("Parameter learner resetting to default values")
# When driving in wet conditions the stiffness can go down, and then be too low on the next drive
# Without a way to detect this we have to reset the stiffness every drive
params['stiffnessFactor'] = 1.0
learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg']))
angle_offset_average = params['angleOffsetAverageDeg']
angle_offset = angle_offset_average
while True:
sm.update()
if sm.all_checks():
for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]):
if sm.updated[which]:
t = sm.logMonoTime[which] * 1e-9
learner.handle_log(t, which, sm[which])
if sm.updated['liveLocationKalman']:
x = learner.kf.x
P = np.sqrt(learner.kf.P.diagonal())
if not all(map(math.isfinite, x)):
cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
x = learner.kf.x
angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET]), angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET] + x[States.ANGLE_OFFSET_FAST]), angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
msg = messaging.new_message('liveParameters')
liveParameters = msg.liveParameters
liveParameters.posenetValid = True
liveParameters.sensorValid = True
liveParameters.steerRatio = float(x[States.STEER_RATIO])
liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
liveParameters.roll = float(x[States.ROAD_ROLL])
liveParameters.angleOffsetAverageDeg = angle_offset_average
liveParameters.angleOffsetDeg = angle_offset
liveParameters.valid = all((
abs(liveParameters.angleOffsetAverageDeg) < 10.0,
abs(liveParameters.angleOffsetDeg) < 10.0,
0.2 <= liveParameters.stiffnessFactor <= 5.0,
min_sr <= liveParameters.steerRatio <= max_sr,
))
liveParameters.steerRatioStd = float(P[States.STEER_RATIO])
liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS])
liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET])
liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST])
msg.valid = sm.all_checks()
if sm.frame % 1200 == 0: # once a minute
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': liveParameters.steerRatio,
'stiffnessFactor': liveParameters.stiffnessFactor,
'angleOffsetAverageDeg': liveParameters.angleOffsetAverageDeg,
}
put_nonblocking("LiveParameters", json.dumps(params))
pm.send('liveParameters', msg)
def register(show_spinner=False) -> Optional[str]:
params = Params()
params.put("SubscriberInfo", HARDWARE.get_subscriber_info())
IMEI = params.get("IMEI", encoding='utf8')
HardwareSerial = params.get("HardwareSerial", encoding='utf8')
dongle_id: Optional[str] = params.get("DongleId", encoding='utf8')
needs_registration = None in (IMEI, HardwareSerial, dongle_id)
pubkey = Path(PERSIST + "/comma/id_rsa.pub")
if not pubkey.is_file():
dongle_id = UNREGISTERED_DONGLE_ID
cloudlog.warning(f"missing public key: {pubkey}")
elif needs_registration:
if show_spinner:
spinner = Spinner()
spinner.update("registering device")
# Create registration token, in the future, this key will make JWTs directly
with open(PERSIST +
"/comma/id_rsa.pub") as f1, open(PERSIST +
"/comma/id_rsa") as f2:
public_key = f1.read()
private_key = f2.read()
# Block until we get the imei
serial = HARDWARE.get_serial()
start_time = time.monotonic()
imei1: Optional[str] = None
imei2: Optional[str] = None
while imei1 is None and imei2 is None:
try:
imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1)
except Exception:
cloudlog.exception("Error getting imei, trying again...")
time.sleep(1)
if time.monotonic() - start_time > 60 and show_spinner:
spinner.update(
f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})"
)
params.put("IMEI", imei1)
params.put("HardwareSerial", serial)
backoff = 0
start_time = time.monotonic()
while True:
try:
register_token = jwt.encode(
{
'register': True,
'exp': datetime.utcnow() + timedelta(hours=1)
},
private_key,
algorithm='RS256')
cloudlog.info("getting pilotauth")
resp = api_get("v2/pilotauth/",
method='POST',
timeout=15,
imei=imei1,
imei2=imei2,
serial=serial,
public_key=public_key,
register_token=register_token)
if resp.status_code in (402, 403):
cloudlog.info(
f"Unable to register device, got {resp.status_code}")
dongle_id = UNREGISTERED_DONGLE_ID
else:
dongleauth = json.loads(resp.text)
dongle_id = dongleauth["dongle_id"]
break
except Exception:
cloudlog.exception("failed to authenticate")
backoff = min(backoff + 1, 15)
time.sleep(backoff)
if time.monotonic() - start_time > 60 and show_spinner:
spinner.update(
f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})"
)
if show_spinner:
spinner.close()
if dongle_id:
params.put("DongleId", dongle_id)
set_offroad_alert("Offroad_UnofficialHardware",
(dongle_id == UNREGISTERED_DONGLE_ID) and not PC)
return dongle_id
def flash_panda(panda_serial: str) -> Panda:
panda = Panda(panda_serial)
fw_signature = get_expected_signature(panda)
internal_panda = panda.is_internal() and not panda.bootstub
panda_version = "bootstub" if panda.bootstub else panda.get_version()
panda_signature = b"" if panda.bootstub else panda.get_signature()
cloudlog.warning(
f"Panda {panda_serial} connected, version: {panda_version}, signature {panda_signature.hex()[:16]}, expected {fw_signature.hex()[:16]}"
)
if panda.bootstub or panda_signature != fw_signature:
cloudlog.info("Panda firmware out of date, update required")
panda.flash()
cloudlog.info("Done flashing")
if panda.bootstub:
bootstub_version = panda.get_version()
cloudlog.info(
f"Flashed firmware not booting, flashing development bootloader. Bootstub version: {bootstub_version}"
)
if internal_panda:
HARDWARE.recover_internal_panda()
panda.recover(reset=(not internal_panda))
cloudlog.info("Done flashing bootloader")
if panda.bootstub:
cloudlog.info("Panda still not booting, exiting")
raise AssertionError
panda_signature = panda.get_signature()
if panda_signature != fw_signature:
cloudlog.info("Version mismatch after flashing, exiting")
raise AssertionError
return panda
def update_now(self, signum: int, frame) -> None:
cloudlog.info("caught SIGHUP, running update check immediately")
self.ready_event.set()
def main() -> NoReturn:
first_run = True
params = Params()
while True:
try:
params.delete("PandaSignatures")
# Flash all Pandas in DFU mode
for p in PandaDFU.list():
cloudlog.info(
f"Panda in DFU mode found, flashing recovery {p}")
PandaDFU(p).recover()
time.sleep(1)
panda_serials = Panda.list()
if len(panda_serials) == 0:
if first_run:
cloudlog.info("Resetting internal panda")
HARDWARE.reset_internal_panda()
time.sleep(2) # wait to come back up
continue
cloudlog.info(
f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}"
)
# Flash pandas
pandas: List[Panda] = []
for serial in panda_serials:
pandas.append(flash_panda(serial))
# check health for lost heartbeat
for panda in pandas:
health = panda.health()
if health["heartbeat_lost"]:
params.put_bool("PandaHeartbeatLost", True)
cloudlog.event("heartbeat lost",
deviceState=health,
serial=panda.get_usb_serial())
if first_run:
cloudlog.info(f"Resetting panda {panda.get_usb_serial()}")
panda.reset()
# sort pandas to have deterministic order
pandas.sort(key=cmp_to_key(panda_sort_cmp))
panda_serials = list(map(lambda p: p.get_usb_serial(),
pandas)) # type: ignore
# log panda fw versions
params.put("PandaSignatures",
b','.join(p.get_signature() for p in pandas))
# close all pandas
for p in pandas:
p.close()
except (usb1.USBErrorNoDevice, usb1.USBErrorPipe):
# a panda was disconnected while setting everything up. let's try again
cloudlog.exception("Panda USB exception while setting up")
continue
first_run = False
# run boardd with all connected serials as arguments
os.environ['MANAGER_DAEMON'] = 'boardd'
os.chdir(os.path.join(BASEDIR, "selfdrive/boardd"))
subprocess.run(["./boardd", *panda_serials], check=True)
def fetch_update(wait_helper: WaitTimeHelper) -> bool:
cloudlog.info("attempting git fetch inside staging overlay")
setup_git_options(OVERLAY_MERGED)
git_fetch_output = run(["git", "fetch"], OVERLAY_MERGED, low_priority=True)
cloudlog.info("git fetch success: %s", git_fetch_output)
cur_hash = run(["git", "rev-parse", "HEAD"], OVERLAY_MERGED).rstrip()
upstream_hash = run(["git", "rev-parse", "@{u}"], OVERLAY_MERGED).rstrip()
new_version: bool = cur_hash != upstream_hash
git_fetch_result = check_git_fetch_result(git_fetch_output)
new_branch = Params().get("SwitchToBranch", encoding='utf8')
if new_branch is not None:
new_version = True
cloudlog.info(f"comparing {cur_hash} to {upstream_hash}")
if new_version or git_fetch_result:
cloudlog.info("Running update")
if new_version:
cloudlog.info("git reset in progress")
cmds = [
["git", "reset", "--hard", "@{u}"],
["git", "clean", "-xdf"],
["git", "submodule", "init"],
["git", "submodule", "update"],
]
if new_branch is not None:
cloudlog.info(f"switching to branch {repr(new_branch)}")
cmds.insert(0, ["git", "checkout", "-f", new_branch])
r = [run(cmd, OVERLAY_MERGED, low_priority=True) for cmd in cmds]
cloudlog.info("git reset success: %s", '\n'.join(r))
if AGNOS:
handle_agnos_update(wait_helper)
# Create the finalized, ready-to-swap update
finalize_update(wait_helper)
cloudlog.info("openpilot update successful!")
else:
cloudlog.info("nothing new from git at this time")
return new_version
def dismount_overlay() -> None:
if os.path.ismount(OVERLAY_MERGED):
cloudlog.info("unmounting existing overlay")
run(["sudo", "umount", "-l", OVERLAY_MERGED])
