agnos 17.2, update comma
8
Jenkinsfile
vendored
@@ -167,7 +167,7 @@ node {
|
||||
env.GIT_COMMIT = checkout(scm).GIT_COMMIT
|
||||
|
||||
def excludeBranches = ['__nightly', 'devel', 'devel-staging', 'release3', 'release3-staging',
|
||||
'release-tici', 'release-tizi', 'release-tizi-staging', 'testing-closet*', 'hotfix-*']
|
||||
'release-tici', 'release-tizi', 'release-tizi-staging', 'release-mici-staging', 'testing-closet*', 'hotfix-*']
|
||||
def excludeRegex = excludeBranches.join('|').replaceAll('\\*', '.*')
|
||||
|
||||
if (env.BRANCH_NAME != 'master' && !env.BRANCH_NAME.contains('__jenkins_loop_')) {
|
||||
@@ -179,7 +179,7 @@ node {
|
||||
try {
|
||||
if (env.BRANCH_NAME == 'devel-staging') {
|
||||
deviceStage("build release-tizi-staging", "tizi-needs-can", [], [
|
||||
step("build release-tizi-staging", "RELEASE_BRANCH=release-tizi-staging $SOURCE_DIR/release/build_release.sh"),
|
||||
step("build release-tizi-staging", "RELEASE_BRANCH=release-tizi-staging $SOURCE_DIR/release/build_release.sh && git push -f origin release-tizi-staging:release-mici-staging"),
|
||||
])
|
||||
}
|
||||
|
||||
@@ -218,14 +218,14 @@ node {
|
||||
'camerad OX03C10': {
|
||||
deviceStage("OX03C10", "tizi-ox03c10", ["UNSAFE=1"], [
|
||||
step("build", "cd system/manager && ./build.py"),
|
||||
step("test pandad", "pytest selfdrive/pandad/tests/test_pandad.py", [diffPaths: ["panda", "selfdrive/pandad/"]]),
|
||||
step("test pandad", "pytest selfdrive/pandad/tests/test_pandad.py"),
|
||||
step("test camerad", "pytest system/camerad/test/test_camerad.py", [timeout: 90]),
|
||||
])
|
||||
},
|
||||
'camerad OS04C10': {
|
||||
deviceStage("OS04C10", "tici-os04c10", ["UNSAFE=1"], [
|
||||
step("build", "cd system/manager && ./build.py"),
|
||||
step("test pandad", "pytest selfdrive/pandad/tests/test_pandad.py", [diffPaths: ["panda", "selfdrive/pandad/"]]),
|
||||
step("test pandad", "pytest selfdrive/pandad/tests/test_pandad.py"),
|
||||
step("test camerad", "pytest system/camerad/test/test_camerad.py", [timeout: 90]),
|
||||
])
|
||||
},
|
||||
|
||||
91
SConstruct
@@ -4,9 +4,11 @@ import sys
|
||||
import sysconfig
|
||||
import platform
|
||||
import shlex
|
||||
import importlib
|
||||
import numpy as np
|
||||
|
||||
import SCons.Errors
|
||||
from SCons.Defaults import _stripixes
|
||||
|
||||
SCons.Warnings.warningAsException(True)
|
||||
|
||||
@@ -14,9 +16,6 @@ Decider('MD5-timestamp')
|
||||
|
||||
SetOption('num_jobs', max(1, int(os.cpu_count()/2)))
|
||||
|
||||
AddOption('--asan', action='store_true', help='turn on ASAN')
|
||||
AddOption('--ubsan', action='store_true', help='turn on UBSan')
|
||||
AddOption('--mutation', action='store_true', help='generate mutation-ready code')
|
||||
AddOption('--ccflags', action='store', type='string', default='', help='pass arbitrary flags over the command line')
|
||||
AddOption('--verbose', action='store_true', default=False, help='show full build commands')
|
||||
AddOption('--minimal',
|
||||
@@ -38,23 +37,46 @@ assert arch in [
|
||||
"Darwin", # macOS arm64 (x86 not supported)
|
||||
]
|
||||
|
||||
if arch != "larch64":
|
||||
import bzip2
|
||||
import capnproto
|
||||
import eigen
|
||||
import ffmpeg as ffmpeg_pkg
|
||||
import libjpeg
|
||||
import libyuv
|
||||
import ncurses
|
||||
import python3_dev
|
||||
import zeromq
|
||||
import zstd
|
||||
pkgs = [bzip2, capnproto, eigen, ffmpeg_pkg, libjpeg, libyuv, ncurses, zeromq, zstd]
|
||||
py_include = python3_dev.INCLUDE_DIR
|
||||
else:
|
||||
# TODO: remove when AGNOS has our new vendor pkgs
|
||||
pkgs = []
|
||||
py_include = sysconfig.get_paths()['include']
|
||||
pkg_names = ['bzip2', 'capnproto', 'eigen', 'ffmpeg', 'libjpeg', 'libyuv', 'ncurses', 'zeromq', 'zstd']
|
||||
pkgs = [importlib.import_module(name) for name in pkg_names]
|
||||
|
||||
|
||||
# ***** enforce a whitelist of system libraries *****
|
||||
# this prevents silently relying on a 3rd party package,
|
||||
# e.g. apt-installed libusb. all libraries should either
|
||||
# be distributed with all Linux distros and macOS, or
|
||||
# vendored in commaai/dependencies.
|
||||
allowed_system_libs = {
|
||||
"EGL", "GLESv2", "GL", "Qt5Charts", "Qt5Core", "Qt5Gui", "Qt5Widgets",
|
||||
"dl", "drm", "gbm", "m", "pthread",
|
||||
}
|
||||
|
||||
def _resolve_lib(env, name):
|
||||
for d in env.Flatten(env.get('LIBPATH', [])):
|
||||
p = Dir(str(d)).abspath
|
||||
for ext in ('.a', '.so', '.dylib'):
|
||||
f = File(os.path.join(p, f'lib{name}{ext}'))
|
||||
if f.exists() or f.has_builder():
|
||||
return name
|
||||
if name in allowed_system_libs:
|
||||
return name
|
||||
raise SCons.Errors.UserError(f"Unexpected non-vendored library '{name}'")
|
||||
|
||||
def _libflags(target, source, env, for_signature):
|
||||
libs = []
|
||||
lp = env.subst('$LIBLITERALPREFIX')
|
||||
for lib in env.Flatten(env.get('LIBS', [])):
|
||||
if isinstance(lib, str):
|
||||
if os.sep in lib or lib.startswith('#'):
|
||||
libs.append(File(lib))
|
||||
elif lib.startswith('-') or (lp and lib.startswith(lp)):
|
||||
libs.append(lib)
|
||||
else:
|
||||
libs.append(_resolve_lib(env, lib))
|
||||
else:
|
||||
libs.append(lib)
|
||||
return _stripixes(env['LIBLINKPREFIX'], libs, env['LIBLINKSUFFIX'],
|
||||
env['LIBPREFIXES'], env['LIBSUFFIXES'], env, env['LIBLITERALPREFIX'])
|
||||
|
||||
env = Environment(
|
||||
ENV={
|
||||
@@ -107,14 +129,14 @@ env = Environment(
|
||||
tools=["default", "cython", "compilation_db", "rednose_filter"],
|
||||
toolpath=["#site_scons/site_tools", "#rednose_repo/site_scons/site_tools"],
|
||||
)
|
||||
if arch != "larch64":
|
||||
env['_LIBFLAGS'] = _libflags
|
||||
|
||||
# Arch-specific flags and paths
|
||||
if arch == "larch64":
|
||||
env["CC"] = "clang"
|
||||
env["CXX"] = "clang++"
|
||||
env.Append(LIBPATH=[
|
||||
"/usr/local/lib",
|
||||
"/system/vendor/lib64",
|
||||
"/usr/lib/aarch64-linux-gnu",
|
||||
])
|
||||
arch_flags = ["-D__TICI__", "-mcpu=cortex-a57"]
|
||||
@@ -126,19 +148,6 @@ elif arch == "Darwin":
|
||||
])
|
||||
env.Append(CCFLAGS=["-DGL_SILENCE_DEPRECATION"])
|
||||
env.Append(CXXFLAGS=["-DGL_SILENCE_DEPRECATION"])
|
||||
else:
|
||||
env.Append(LIBPATH=[
|
||||
"/usr/lib",
|
||||
"/usr/local/lib",
|
||||
])
|
||||
|
||||
# Sanitizers and extra CCFLAGS from CLI
|
||||
if GetOption('asan'):
|
||||
env.Append(CCFLAGS=["-fsanitize=address", "-fno-omit-frame-pointer"])
|
||||
env.Append(LINKFLAGS=["-fsanitize=address"])
|
||||
elif GetOption('ubsan'):
|
||||
env.Append(CCFLAGS=["-fsanitize=undefined"])
|
||||
env.Append(LINKFLAGS=["-fsanitize=undefined"])
|
||||
|
||||
_extra_cc = shlex.split(GetOption('ccflags') or '')
|
||||
if _extra_cc:
|
||||
@@ -176,7 +185,7 @@ if os.environ.get('SCONS_PROGRESS'):
|
||||
|
||||
# ********** Cython build environment **********
|
||||
envCython = env.Clone()
|
||||
envCython["CPPPATH"] += [py_include, np.get_include()]
|
||||
envCython["CPPPATH"] += [sysconfig.get_paths()['include'], np.get_include()]
|
||||
envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-cpp", "-Wno-shadow", "-Wno-deprecated-declarations"]
|
||||
envCython["CCFLAGS"].remove("-Werror")
|
||||
|
||||
@@ -235,7 +244,15 @@ if arch == "larch64":
|
||||
# Build openpilot
|
||||
SConscript(['third_party/SConscript'])
|
||||
|
||||
SConscript(['selfdrive/SConscript'])
|
||||
# Build selfdrive
|
||||
SConscript([
|
||||
'selfdrive/pandad/SConscript',
|
||||
'selfdrive/controls/lib/lateral_mpc_lib/SConscript',
|
||||
'selfdrive/controls/lib/longitudinal_mpc_lib/SConscript',
|
||||
'selfdrive/locationd/SConscript',
|
||||
'selfdrive/modeld/SConscript',
|
||||
'selfdrive/ui/SConscript',
|
||||
])
|
||||
|
||||
if Dir('#tools/cabana/').exists() and arch != "larch64":
|
||||
SConscript(['tools/cabana/SConscript'])
|
||||
|
||||
@@ -1491,6 +1491,8 @@ struct LivePose {
|
||||
posenetOK @5 :Bool = false;
|
||||
sensorsOK @6 :Bool = false;
|
||||
|
||||
timestamp @8 :UInt64;
|
||||
|
||||
debugFilterState @7 :FilterState;
|
||||
|
||||
struct XYZMeasurement {
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
Import('env', 'envCython', 'arch')
|
||||
Import('env', 'envCython')
|
||||
|
||||
common_libs = [
|
||||
'params.cc',
|
||||
|
||||
@@ -31,7 +31,7 @@ class BounceFilter(FirstOrderFilter):
|
||||
scale = self.dt / (1.0 / 60.0) # tuned at 60 fps
|
||||
self.velocity.x += (x - self.x) * self.bounce * scale * self.dt
|
||||
self.velocity.update(0.0)
|
||||
if abs(self.velocity.x) < 1e-5:
|
||||
if abs(self.velocity.x) < 1e-3:
|
||||
self.velocity.x = 0.0
|
||||
self.x += self.velocity.x
|
||||
return self.x
|
||||
|
||||
@@ -2,6 +2,7 @@ import datetime
|
||||
from pathlib import Path
|
||||
|
||||
MIN_DATE = datetime.datetime(year=2025, month=2, day=21)
|
||||
MAX_DATE = datetime.datetime(year=2035, month=1, day=1)
|
||||
|
||||
def min_date():
|
||||
# on systemd systems, the default time is the systemd build time
|
||||
@@ -12,4 +13,4 @@ def min_date():
|
||||
return MIN_DATE
|
||||
|
||||
def system_time_valid():
|
||||
return datetime.datetime.now() > min_date()
|
||||
return min_date() < datetime.datetime.now() < MAX_DATE
|
||||
|
||||
@@ -1 +1 @@
|
||||
#define COMMA_VERSION "0.10.4"
|
||||
#define COMMA_VERSION "0.11.1"
|
||||
|
||||
@@ -10,7 +10,6 @@ from openpilot.system.hardware import TICI, HARDWARE
|
||||
# TODO: pytest-cpp doesn't support FAIL, and we need to create test translations in sessionstart
|
||||
# pending https://github.com/pytest-dev/pytest-cpp/pull/147
|
||||
collect_ignore = [
|
||||
"selfdrive/ui/tests/test_translations",
|
||||
"selfdrive/test/process_replay/test_processes.py",
|
||||
"selfdrive/test/process_replay/test_regen.py",
|
||||
]
|
||||
|
||||
@@ -16,7 +16,7 @@ export VECLIB_MAXIMUM_THREADS=1
|
||||
export QCOM_PRIORITY=12
|
||||
|
||||
if [ -z "$AGNOS_VERSION" ]; then
|
||||
export AGNOS_VERSION="16"
|
||||
export AGNOS_VERSION="17.2"
|
||||
fi
|
||||
|
||||
export STAGING_ROOT="/data/safe_staging"
|
||||
|
||||
@@ -26,18 +26,18 @@ dependencies = [
|
||||
"numpy >=2.0",
|
||||
|
||||
# vendored native dependencies
|
||||
"bzip2 @ git+https://github.com/commaai/dependencies.git@releases#subdirectory=bzip2",
|
||||
"capnproto @ git+https://github.com/commaai/dependencies.git@releases#subdirectory=capnproto",
|
||||
"eigen @ git+https://github.com/commaai/dependencies.git@releases#subdirectory=eigen",
|
||||
"ffmpeg @ git+https://github.com/commaai/dependencies.git@releases#subdirectory=ffmpeg",
|
||||
"libjpeg @ git+https://github.com/commaai/dependencies.git@releases#subdirectory=libjpeg",
|
||||
"libyuv @ git+https://github.com/commaai/dependencies.git@releases#subdirectory=libyuv",
|
||||
"python3-dev @ git+https://github.com/commaai/dependencies.git@releases#subdirectory=python3-dev",
|
||||
"zstd @ git+https://github.com/commaai/dependencies.git@releases#subdirectory=zstd",
|
||||
"ncurses @ git+https://github.com/commaai/dependencies.git@releases#subdirectory=ncurses",
|
||||
"zeromq @ git+https://github.com/commaai/dependencies.git@releases#subdirectory=zeromq",
|
||||
"git-lfs @ git+https://github.com/commaai/dependencies.git@releases#subdirectory=git-lfs",
|
||||
"gcc-arm-none-eabi @ git+https://github.com/commaai/dependencies.git@releases#subdirectory=gcc-arm-none-eabi",
|
||||
"bzip2 @ git+https://github.com/commaai/dependencies.git@release-bzip2#subdirectory=bzip2",
|
||||
"capnproto @ git+https://github.com/commaai/dependencies.git@release-capnproto#subdirectory=capnproto",
|
||||
"eigen @ git+https://github.com/commaai/dependencies.git@release-eigen#subdirectory=eigen",
|
||||
"ffmpeg @ git+https://github.com/commaai/dependencies.git@release-ffmpeg#subdirectory=ffmpeg",
|
||||
"libjpeg @ git+https://github.com/commaai/dependencies.git@release-libjpeg#subdirectory=libjpeg",
|
||||
"libyuv @ git+https://github.com/commaai/dependencies.git@release-libyuv#subdirectory=libyuv",
|
||||
"zstd @ git+https://github.com/commaai/dependencies.git@release-zstd#subdirectory=zstd",
|
||||
"ncurses @ git+https://github.com/commaai/dependencies.git@release-ncurses#subdirectory=ncurses",
|
||||
"zeromq @ git+https://github.com/commaai/dependencies.git@release-zeromq#subdirectory=zeromq",
|
||||
"libusb @ git+https://github.com/commaai/dependencies.git@release-libusb#subdirectory=libusb",
|
||||
"git-lfs @ git+https://github.com/commaai/dependencies.git@release-git-lfs#subdirectory=git-lfs",
|
||||
"gcc-arm-none-eabi @ git+https://github.com/commaai/dependencies.git@release-gcc-arm-none-eabi#subdirectory=gcc-arm-none-eabi",
|
||||
|
||||
# body / webrtcd
|
||||
"av",
|
||||
@@ -205,6 +205,7 @@ lint.flake8-implicit-str-concat.allow-multiline = false
|
||||
"pyray.is_mouse_button_pressed".msg = "This can miss events. Use Widget._handle_mouse_press"
|
||||
"pyray.is_mouse_button_released".msg = "This can miss events. Use Widget._handle_mouse_release"
|
||||
"pyray.draw_text".msg = "Use a function (such as rl.draw_font_ex) that takes font as an argument"
|
||||
"pyray.draw_texture".msg = "Use rl.draw_texture_ex for float position support"
|
||||
|
||||
[tool.ruff.format]
|
||||
quote-style = "preserve"
|
||||
@@ -248,3 +249,6 @@ unsupported-operator = "ignore"
|
||||
# Ignore not-subscriptable - false positives from dynamic types
|
||||
not-subscriptable = "ignore"
|
||||
# not-iterable errors are now fixed
|
||||
|
||||
[tool.uv]
|
||||
python-preference = "only-managed"
|
||||
|
||||
|
Before Width: | Height: | Size: 18 KiB |
|
Before Width: | Height: | Size: 3.2 KiB |
|
Before Width: | Height: | Size: 1.6 KiB |
BIN
selfdrive/assets/icons_mici/setup/factory_reset.png
Normal file
|
After Width: | Height: | Size: 11 KiB |
|
Before Width: | Height: | Size: 7.7 KiB |
|
Before Width: | Height: | Size: 18 KiB |
|
Before Width: | Height: | Size: 6.8 KiB |
|
Before Width: | Height: | Size: 13 KiB |
|
Before Width: | Height: | Size: 8.9 KiB |
|
Before Width: | Height: | Size: 21 KiB |
BIN
selfdrive/assets/icons_mici/setup/reset_failed.png
Normal file
|
After Width: | Height: | Size: 12 KiB |
|
Before Width: | Height: | Size: 1.1 KiB |
|
Before Width: | Height: | Size: 8.6 KiB |
|
Before Width: | Height: | Size: 22 KiB |
|
Before Width: | Height: | Size: 5.6 KiB |
|
Before Width: | Height: | Size: 6.5 KiB |
|
Before Width: | Height: | Size: 13 KiB |
|
Before Width: | Height: | Size: 8.4 KiB |
|
Before Width: | Height: | Size: 5.2 KiB |
|
Before Width: | Height: | Size: 17 KiB |
|
Before Width: | Height: | Size: 6.9 KiB |
|
Before Width: | Height: | Size: 4.0 KiB |
|
Before Width: | Height: | Size: 15 KiB |
@@ -94,7 +94,7 @@ class PointBuckets:
|
||||
def add_point(self, x: float, y: float) -> None:
|
||||
raise NotImplementedError
|
||||
|
||||
def get_points(self, num_points: int = None) -> Any:
|
||||
def get_points(self, num_points: int | None = None) -> Any:
|
||||
points = np.vstack([x.arr for x in self.buckets.values()])
|
||||
if num_points is None:
|
||||
return points
|
||||
@@ -172,7 +172,7 @@ class PoseCalibrator:
|
||||
ned_from_calib_euler = self._ned_from_calib(pose.orientation)
|
||||
angular_velocity_calib = self._transform_calib_from_device(pose.angular_velocity)
|
||||
acceleration_calib = self._transform_calib_from_device(pose.acceleration)
|
||||
velocity_calib = self._transform_calib_from_device(pose.angular_velocity)
|
||||
velocity_calib = self._transform_calib_from_device(pose.velocity)
|
||||
|
||||
return Pose(ned_from_calib_euler, velocity_calib, acceleration_calib, angular_velocity_calib)
|
||||
|
||||
|
||||
@@ -28,11 +28,26 @@ MIN_LAG = 0.15
|
||||
MAX_LAG_STD = 0.1
|
||||
MAX_LAT_ACCEL = 2.0
|
||||
MAX_LAT_ACCEL_DIFF = 0.6
|
||||
MIN_LAT_ACCEL_RANGE = 0.5
|
||||
MIN_CONFIDENCE = 0.7
|
||||
CORR_BORDER_OFFSET = 5
|
||||
LAG_CANDIDATE_CORR_THRESHOLD = 0.9
|
||||
SMOOTH_K = 5
|
||||
SMOOTH_SIGMA = 1.0
|
||||
|
||||
|
||||
def masked_symmetric_moving_average(x: np.ndarray, mask: np.ndarray, k: int, sigma: float) -> np.ndarray:
|
||||
assert k >= 1 and k % 2 == 1, "k must be positive and odd"
|
||||
pad = k // 2
|
||||
i = np.arange(k) - pad
|
||||
w = np.exp(-0.5 * (i / sigma) ** 2)
|
||||
w /= w.sum()
|
||||
xp = np.pad(x * mask, pad, mode="edge")
|
||||
mp = np.pad(mask, pad, mode="edge")
|
||||
num = np.convolve(xp, w, mode="valid")
|
||||
den = np.convolve(mp, w, mode="valid")
|
||||
return np.divide(num, den, out=np.full_like(num, np.nan, dtype=np.float64), where=den != 0)
|
||||
|
||||
def masked_normalized_cross_correlation(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, n: int):
|
||||
"""
|
||||
References:
|
||||
@@ -294,11 +309,14 @@ class LateralLagEstimator:
|
||||
|
||||
times, desired, actual, okay = self.points.get()
|
||||
# check if there are any new valid data points since the last update
|
||||
is_valid = self.points_valid()
|
||||
is_valid = self.points_valid() and (actual.max() - actual.min() >= MIN_LAT_ACCEL_RANGE)
|
||||
if self.last_estimate_t != 0 and times[0] <= self.last_estimate_t:
|
||||
new_values_start_idx = next(-i for i, t in enumerate(reversed(times)) if t <= self.last_estimate_t)
|
||||
is_valid = is_valid and not (new_values_start_idx == 0 or not np.any(okay[new_values_start_idx:]))
|
||||
|
||||
desired = masked_symmetric_moving_average(desired, okay, SMOOTH_K, SMOOTH_SIGMA)
|
||||
actual = masked_symmetric_moving_average(actual, okay, SMOOTH_K, SMOOTH_SIGMA)
|
||||
|
||||
delay, corr, confidence = self.actuator_delay(desired, actual, okay, self.dt, MIN_LAG, MAX_LAG)
|
||||
if corr < self.min_ncc or confidence < self.min_confidence or not is_valid:
|
||||
return
|
||||
@@ -310,16 +328,16 @@ class LateralLagEstimator:
|
||||
def actuator_delay(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray,
|
||||
dt: float, min_lag: float, max_lag: float) -> tuple[float, float, float]:
|
||||
assert len(expected_sig) == len(actual_sig)
|
||||
min_lag_samples, max_lag_samples = int(round(min_lag / dt)), int(round(max_lag / dt))
|
||||
padded_size = fft_next_good_size(len(expected_sig) + max_lag_samples)
|
||||
min_lag_samples, max_lag_samples, one_sec_samples = int(round(min_lag / dt)), int(round(max_lag / dt)), int(round(1.0 / dt))
|
||||
padded_size = fft_next_good_size(len(expected_sig) + max(max_lag_samples, one_sec_samples))
|
||||
|
||||
ncc = masked_normalized_cross_correlation(expected_sig, actual_sig, mask, padded_size)
|
||||
|
||||
# only consider lags from min_lag to max_lag
|
||||
roi = np.s_[len(expected_sig) - 1 + min_lag_samples: len(expected_sig) - 1 + max_lag_samples]
|
||||
extended_roi = np.s_[roi.start - CORR_BORDER_OFFSET: roi.stop + CORR_BORDER_OFFSET]
|
||||
roi_ncc = ncc[roi]
|
||||
extended_roi_ncc = ncc[extended_roi]
|
||||
# only consider lags from ranges:
|
||||
roi = np.s_[len(expected_sig) - 1 + min_lag_samples: len(expected_sig) - 1 + max_lag_samples] # min_lag - max_lag range
|
||||
threshold_roi = np.s_[len(expected_sig) - 1: len(expected_sig) - 1 + one_sec_samples] # 0 - 1 second range
|
||||
confidence_roi = np.s_[threshold_roi.start - CORR_BORDER_OFFSET: threshold_roi.stop + CORR_BORDER_OFFSET] # threshold range +/- border
|
||||
roi_ncc, confidence_roi_ncc, threshold_roi_ncc = ncc[roi], ncc[confidence_roi], ncc[threshold_roi]
|
||||
|
||||
max_corr_index = np.argmax(roi_ncc)
|
||||
corr = roi_ncc[max_corr_index]
|
||||
@@ -327,8 +345,8 @@ class LateralLagEstimator:
|
||||
|
||||
# to estimate lag confidence, gather all high-correlation candidates and see how spread they are
|
||||
# if e.g. 0.8 and 0.4 are both viable, this is an ambiguous case
|
||||
ncc_thresh = (roi_ncc.max() - roi_ncc.min()) * LAG_CANDIDATE_CORR_THRESHOLD + roi_ncc.min()
|
||||
good_lag_candidate_mask = extended_roi_ncc >= ncc_thresh
|
||||
ncc_thresh = (threshold_roi_ncc.max() - threshold_roi_ncc.min()) * LAG_CANDIDATE_CORR_THRESHOLD + threshold_roi_ncc.min()
|
||||
good_lag_candidate_mask = confidence_roi_ncc >= ncc_thresh
|
||||
good_lag_candidate_edges = np.diff(good_lag_candidate_mask.astype(int), prepend=0, append=0)
|
||||
starts, ends = np.where(good_lag_candidate_edges == 1)[0], np.where(good_lag_candidate_edges == -1)[0] - 1
|
||||
run_idx = np.searchsorted(starts, max_corr_index + CORR_BORDER_OFFSET, side='right') - 1
|
||||
|
||||
@@ -239,6 +239,7 @@ class LocationEstimator:
|
||||
livePose.inputsOK = inputs_valid
|
||||
livePose.posenetOK = not std_spike or self.car_speed <= 5.0
|
||||
livePose.sensorsOK = sensors_valid
|
||||
livePose.timestamp = int(np.nan_to_num(self.kf.t) * 1e9)
|
||||
|
||||
return msg
|
||||
|
||||
|
||||
@@ -71,6 +71,7 @@ class VehicleParamsLearner:
|
||||
|
||||
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
|
||||
if which == 'livePose':
|
||||
t = msg.timestamp * 1e-9
|
||||
device_pose = Pose.from_live_pose(msg)
|
||||
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
|
||||
|
||||
@@ -133,8 +134,8 @@ class VehicleParamsLearner:
|
||||
|
||||
if not self.active:
|
||||
# Reset time when stopped so uncertainty doesn't grow
|
||||
self.kf.filter.set_filter_time(t) # type: ignore
|
||||
self.kf.filter.reset_rewind() # type: ignore
|
||||
self.kf.filter.set_filter_time(t)
|
||||
self.kf.filter.reset_rewind()
|
||||
|
||||
def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
|
||||
x = self.kf.x
|
||||
@@ -291,43 +292,13 @@ def main():
|
||||
params_memory = Params("/dev/shm/params")
|
||||
params_memory.remove("LastGPSPosition")
|
||||
|
||||
last_fail_print_t = 0.0
|
||||
last_lp_invalid_print_t = 0.0
|
||||
|
||||
show_debug = False
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
ok_alive = sm.all_alive()
|
||||
ok_freq = sm.all_freq_ok()
|
||||
ok_valid = sm.all_valid()
|
||||
ok_all = ok_alive and ok_freq and ok_valid
|
||||
|
||||
if ok_all:
|
||||
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])
|
||||
else:
|
||||
now = time.monotonic()
|
||||
if show_debug and now - last_fail_print_t > 1.0:
|
||||
last_fail_print_t = now
|
||||
|
||||
print(f"\n[liveParameters all_checks FAIL] frame={sm.frame} "
|
||||
f"alive={ok_alive} freq={ok_freq} valid={ok_valid}")
|
||||
|
||||
for s in sm.services:
|
||||
recv_age_ms = (now - sm.recv_time[s]) * 1000.0 if sm.recv_time[s] > 0 else -1.0
|
||||
print(
|
||||
f" {s:18s} "
|
||||
f"seen={sm.seen[s]} "
|
||||
f"updated={sm.updated[s]} "
|
||||
f"alive={sm.alive[s]} "
|
||||
f"freq_ok={sm.freq_ok[s]} "
|
||||
f"valid={sm.valid[s]} "
|
||||
f"recv_age_ms={recv_age_ms:7.1f} "
|
||||
f"logMonoTime={sm.logMonoTime[s]}"
|
||||
)
|
||||
|
||||
if sm.updated[gps_location_service]:
|
||||
gps = sm[gps_location_service]
|
||||
@@ -342,31 +313,7 @@ def main():
|
||||
}))
|
||||
|
||||
if sm.updated['livePose']:
|
||||
lp_valid = sm.valid["livePose"] and sm.valid["liveCalibration"]
|
||||
|
||||
now = time.monotonic()
|
||||
if show_debug and (not lp_valid) and (now - last_lp_invalid_print_t > 1.0):
|
||||
last_lp_invalid_print_t = now
|
||||
|
||||
print(f"\n[liveParameters lp_valid FAIL] frame={sm.frame} "
|
||||
f"livePose_valid={sm.valid['livePose']} "
|
||||
f"liveCalibration_valid={sm.valid['liveCalibration']} "
|
||||
f"all_checks={ok_all}")
|
||||
|
||||
for s in ['livePose', 'liveCalibration', 'carState', gps_location_service]:
|
||||
recv_age_ms = (now - sm.recv_time[s]) * 1000.0 if sm.recv_time[s] > 0 else -1.0
|
||||
print(
|
||||
f" {s:18s} "
|
||||
f"seen={sm.seen[s]} "
|
||||
f"updated={sm.updated[s]} "
|
||||
f"alive={sm.alive[s]} "
|
||||
f"freq_ok={sm.freq_ok[s]} "
|
||||
f"valid={sm.valid[s]} "
|
||||
f"recv_age_ms={recv_age_ms:7.1f} "
|
||||
f"logMonoTime={sm.logMonoTime[s]}"
|
||||
)
|
||||
|
||||
msg = learner.get_msg(lp_valid, debug=DEBUG)
|
||||
msg = learner.get_msg(sm.all_checks(), debug=DEBUG)
|
||||
|
||||
msg_dat = msg.to_bytes()
|
||||
if sm.frame % 1200 == 0: # once a minute
|
||||
@@ -374,5 +321,6 @@ def main():
|
||||
|
||||
pm.send('liveParameters', msg_dat)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
1
selfdrive/locationd/test/.gitignore
vendored
@@ -1 +0,0 @@
|
||||
out/
|
||||
@@ -1,112 +0,0 @@
|
||||
import random
|
||||
|
||||
import numpy as np
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, \
|
||||
MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT, MAX_ALLOWED_PITCH_SPREAD, MAX_ALLOWED_YAW_SPREAD
|
||||
|
||||
|
||||
def process_messages(c, cam_odo_calib, cycles,
|
||||
cam_odo_speed=MIN_SPEED_FILTER + 1,
|
||||
carstate_speed=MIN_SPEED_FILTER + 1,
|
||||
cam_odo_yr=0.0,
|
||||
cam_odo_speed_std=1e-3,
|
||||
cam_odo_height_std=1e-3):
|
||||
old_rpy_weight_prev = 0.0
|
||||
for _ in range(cycles):
|
||||
assert (old_rpy_weight_prev - c.old_rpy_weight < 1/SMOOTH_CYCLES + 1e-3)
|
||||
old_rpy_weight_prev = c.old_rpy_weight
|
||||
c.handle_v_ego(carstate_speed)
|
||||
c.handle_cam_odom([cam_odo_speed,
|
||||
np.sin(cam_odo_calib[2]) * cam_odo_speed,
|
||||
-np.sin(cam_odo_calib[1]) * cam_odo_speed],
|
||||
[0.0, 0.0, cam_odo_yr],
|
||||
[0.0, 0.0, 0.0],
|
||||
[cam_odo_speed_std, cam_odo_speed_std, cam_odo_speed_std],
|
||||
[0.0, 0.0, HEIGHT_INIT.item()],
|
||||
[cam_odo_height_std, cam_odo_height_std, cam_odo_height_std])
|
||||
|
||||
class TestCalibrationd:
|
||||
|
||||
def test_read_saved_params(self):
|
||||
msg = messaging.new_message('liveCalibration')
|
||||
msg.liveCalibration.validBlocks = random.randint(1, 10)
|
||||
msg.liveCalibration.rpyCalib = [random.random() for _ in range(3)]
|
||||
msg.liveCalibration.height = [random.random() for _ in range(1)]
|
||||
Params().put("CalibrationParams", msg.to_bytes())
|
||||
c = Calibrator(param_put=True)
|
||||
|
||||
np.testing.assert_allclose(msg.liveCalibration.rpyCalib, c.rpy)
|
||||
np.testing.assert_allclose(msg.liveCalibration.height, c.height)
|
||||
assert msg.liveCalibration.validBlocks == c.valid_blocks
|
||||
|
||||
|
||||
def test_calibration_basics(self):
|
||||
c = Calibrator(param_put=False)
|
||||
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED)
|
||||
assert c.valid_blocks == INPUTS_WANTED
|
||||
np.testing.assert_allclose(c.rpy, np.zeros(3))
|
||||
np.testing.assert_allclose(c.height, HEIGHT_INIT)
|
||||
c.reset()
|
||||
|
||||
|
||||
def test_calibration_low_speed_reject(self):
|
||||
c = Calibrator(param_put=False)
|
||||
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed=MIN_SPEED_FILTER - 1)
|
||||
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, carstate_speed=MIN_SPEED_FILTER - 1)
|
||||
assert c.valid_blocks == 0
|
||||
np.testing.assert_allclose(c.rpy, np.zeros(3))
|
||||
np.testing.assert_allclose(c.height, HEIGHT_INIT)
|
||||
|
||||
|
||||
def test_calibration_yaw_rate_reject(self):
|
||||
c = Calibrator(param_put=False)
|
||||
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_yr=MAX_YAW_RATE_FILTER)
|
||||
assert c.valid_blocks == 0
|
||||
np.testing.assert_allclose(c.rpy, np.zeros(3))
|
||||
np.testing.assert_allclose(c.height, HEIGHT_INIT)
|
||||
|
||||
|
||||
def test_calibration_speed_std_reject(self):
|
||||
c = Calibrator(param_put=False)
|
||||
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed_std=1e3)
|
||||
assert c.valid_blocks == INPUTS_NEEDED
|
||||
np.testing.assert_allclose(c.rpy, np.zeros(3))
|
||||
|
||||
|
||||
def test_calibration_speed_std_height_reject(self):
|
||||
c = Calibrator(param_put=False)
|
||||
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_height_std=1e3)
|
||||
assert c.valid_blocks == INPUTS_NEEDED
|
||||
np.testing.assert_allclose(c.rpy, np.zeros(3))
|
||||
|
||||
|
||||
def test_calibration_auto_reset(self):
|
||||
c = Calibrator(param_put=False)
|
||||
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
|
||||
assert c.valid_blocks == INPUTS_NEEDED
|
||||
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0], atol=1e-3)
|
||||
process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*0.9, MAX_ALLOWED_YAW_SPREAD*0.9], BLOCK_SIZE + 10)
|
||||
assert c.valid_blocks == INPUTS_NEEDED + 1
|
||||
assert c.cal_status == log.LiveCalibrationData.Status.calibrated
|
||||
|
||||
c = Calibrator(param_put=False)
|
||||
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
|
||||
assert c.valid_blocks == INPUTS_NEEDED
|
||||
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
|
||||
process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], BLOCK_SIZE + 10)
|
||||
assert c.valid_blocks == 1
|
||||
assert c.cal_status == log.LiveCalibrationData.Status.recalibrating
|
||||
np.testing.assert_allclose(c.rpy, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], atol=1e-2)
|
||||
|
||||
c = Calibrator(param_put=False)
|
||||
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
|
||||
assert c.valid_blocks == INPUTS_NEEDED
|
||||
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
|
||||
process_messages(c, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], BLOCK_SIZE + 10)
|
||||
assert c.valid_blocks == 1
|
||||
assert c.cal_status == log.LiveCalibrationData.Status.recalibrating
|
||||
np.testing.assert_allclose(c.rpy, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], atol=1e-2)
|
||||
@@ -1,135 +0,0 @@
|
||||
import random
|
||||
import numpy as np
|
||||
import time
|
||||
import pytest
|
||||
|
||||
from cereal import messaging, log, car
|
||||
from openpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag, masked_normalized_cross_correlation, \
|
||||
BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC
|
||||
from openpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams
|
||||
from openpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
from openpilot.system.hardware import PC
|
||||
|
||||
MAX_ERR_FRAMES = 1
|
||||
DT = 0.05
|
||||
|
||||
|
||||
def process_messages(estimator, lag_frames, n_frames, vego=20.0, rejection_threshold=0.0):
|
||||
for i in range(n_frames):
|
||||
t = i * estimator.dt
|
||||
desired_la = np.cos(10 * t) * 0.1
|
||||
actual_la = np.cos(10 * (t - lag_frames * estimator.dt)) * 0.1
|
||||
|
||||
# if sample is masked out, set it to desired value (no lag)
|
||||
rejected = random.uniform(0, 1) < rejection_threshold
|
||||
if rejected:
|
||||
actual_la = desired_la
|
||||
|
||||
desired_cuvature = float(desired_la / (vego ** 2))
|
||||
actual_yr = float(actual_la / vego)
|
||||
msgs = [
|
||||
(t, "carControl", car.CarControl(latActive=not rejected)),
|
||||
(t, "carState", car.CarState(vEgo=vego, steeringPressed=False)),
|
||||
(t, "controlsState", log.ControlsState(desiredCurvature=desired_cuvature)),
|
||||
(t, "livePose", log.LivePose(angularVelocityDevice=log.LivePose.XYZMeasurement(z=actual_yr, valid=True),
|
||||
posenetOK=True, inputsOK=True)),
|
||||
(t, "liveCalibration", log.LiveCalibrationData(rpyCalib=[0, 0, 0], calStatus=log.LiveCalibrationData.Status.calibrated)),
|
||||
]
|
||||
for t, w, m in msgs:
|
||||
estimator.handle_log(t, w, m)
|
||||
estimator.update_points()
|
||||
estimator.update_estimate()
|
||||
|
||||
|
||||
class TestLagd:
|
||||
def test_read_saved_params(self):
|
||||
params = Params()
|
||||
|
||||
lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
|
||||
CP = next(m for m in lr if m.which() == "carParams").carParams
|
||||
|
||||
msg = messaging.new_message('liveDelay')
|
||||
msg.liveDelay.lateralDelayEstimate = random.random()
|
||||
msg.liveDelay.validBlocks = random.randint(1, 10)
|
||||
params.put("LiveDelay", msg.to_bytes())
|
||||
params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
|
||||
|
||||
saved_lag_params = retrieve_initial_lag(params, CP)
|
||||
assert saved_lag_params is not None
|
||||
|
||||
lag, valid_blocks = saved_lag_params
|
||||
assert lag == msg.liveDelay.lateralDelayEstimate
|
||||
assert valid_blocks == msg.liveDelay.validBlocks
|
||||
|
||||
def test_ncc(self):
|
||||
lag_frames = random.randint(1, 19)
|
||||
|
||||
desired_sig = np.sin(np.arange(0.0, 10.0, 0.1))
|
||||
actual_sig = np.sin(np.arange(0.0, 10.0, 0.1) - lag_frames * 0.1)
|
||||
mask = np.ones(len(desired_sig), dtype=bool)
|
||||
|
||||
corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20]
|
||||
assert np.argmax(corr) == lag_frames
|
||||
|
||||
# add some noise
|
||||
desired_sig += np.random.normal(0, 0.05, len(desired_sig))
|
||||
actual_sig += np.random.normal(0, 0.05, len(actual_sig))
|
||||
corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20]
|
||||
assert np.argmax(corr) in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1)
|
||||
|
||||
# mask out 40% of the values, and make them noise
|
||||
mask = np.random.choice([True, False], size=len(desired_sig), p=[0.6, 0.4])
|
||||
desired_sig[~mask] = np.random.normal(0, 1, size=np.sum(~mask))
|
||||
actual_sig[~mask] = np.random.normal(0, 1, size=np.sum(~mask))
|
||||
corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20]
|
||||
assert np.argmax(corr) in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1)
|
||||
|
||||
def test_empty_estimator(self):
|
||||
mocked_CP = car.CarParams(steerActuatorDelay=0.8)
|
||||
estimator = LateralLagEstimator(mocked_CP, DT)
|
||||
msg = estimator.get_msg(True)
|
||||
assert msg.liveDelay.status == 'unestimated'
|
||||
assert np.allclose(msg.liveDelay.lateralDelay, estimator.initial_lag)
|
||||
assert np.allclose(msg.liveDelay.lateralDelayEstimate, estimator.initial_lag)
|
||||
assert msg.liveDelay.validBlocks == 0
|
||||
assert msg.liveDelay.calPerc == 0
|
||||
|
||||
def test_estimator_basics(self, subtests):
|
||||
for lag_frames in range(3, 10):
|
||||
with subtests.test(msg=f"lag_frames={lag_frames}"):
|
||||
mocked_CP = car.CarParams(steerActuatorDelay=0.8)
|
||||
estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0)
|
||||
process_messages(estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE)
|
||||
msg = estimator.get_msg(True)
|
||||
assert msg.liveDelay.status == 'estimated'
|
||||
assert np.allclose(msg.liveDelay.lateralDelay, lag_frames * DT, atol=0.01)
|
||||
assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
|
||||
assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
|
||||
assert msg.liveDelay.validBlocks == BLOCK_NUM_NEEDED
|
||||
assert msg.liveDelay.calPerc == 100
|
||||
|
||||
def test_estimator_masking(self):
|
||||
mocked_CP, lag_frames = car.CarParams(steerActuatorDelay=0.8), random.randint(3, 19)
|
||||
estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, min_valid_block_count=1)
|
||||
process_messages(estimator, lag_frames, (int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_SIZE) * 2, rejection_threshold=0.4)
|
||||
msg = estimator.get_msg(True)
|
||||
assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
|
||||
assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
|
||||
assert msg.liveDelay.calPerc == 100
|
||||
|
||||
@pytest.mark.skipif(PC, reason="only on device")
|
||||
def test_estimator_performance(self):
|
||||
mocked_CP = car.CarParams(steerActuatorDelay=0.8)
|
||||
estimator = LateralLagEstimator(mocked_CP, DT)
|
||||
|
||||
ds = []
|
||||
for _ in range(1000):
|
||||
st = time.perf_counter()
|
||||
estimator.update_points()
|
||||
estimator.update_estimate()
|
||||
d = time.perf_counter() - st
|
||||
ds.append(d)
|
||||
|
||||
assert np.mean(ds) < DT
|
||||
@@ -1,190 +0,0 @@
|
||||
import numpy as np
|
||||
from collections import defaultdict
|
||||
from enum import Enum
|
||||
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
from openpilot.selfdrive.test.process_replay.migration import migrate_all
|
||||
from openpilot.selfdrive.test.process_replay.process_replay import replay_process_with_name
|
||||
|
||||
# TODO find a new segment to test
|
||||
TEST_ROUTE = "4019fff6e54cf1c7|00000123--4bc0d95ef6/5"
|
||||
GPS_MESSAGES = ['gpsLocationExternal', 'gpsLocation']
|
||||
SELECT_COMPARE_FIELDS = {
|
||||
'yaw_rate': ['angularVelocityDevice', 'z'],
|
||||
'roll': ['orientationNED', 'x'],
|
||||
'inputs_flag': ['inputsOK'],
|
||||
'sensors_flag': ['sensorsOK'],
|
||||
}
|
||||
JUNK_IDX = 100
|
||||
CONSISTENT_SPIKES_COUNT = 10
|
||||
|
||||
|
||||
class Scenario(Enum):
|
||||
BASE = 'base'
|
||||
GYRO_OFF = 'gyro_off'
|
||||
GYRO_SPIKE_MIDWAY = 'gyro_spike_midway'
|
||||
GYRO_CONSISTENT_SPIKES = 'gyro_consistent_spikes'
|
||||
ACCEL_OFF = 'accel_off'
|
||||
ACCEL_SPIKE_MIDWAY = 'accel_spike_midway'
|
||||
ACCEL_CONSISTENT_SPIKES = 'accel_consistent_spikes'
|
||||
SENSOR_TIMING_SPIKE_MIDWAY = 'timing_spikes'
|
||||
SENSOR_TIMING_CONSISTENT_SPIKES = 'timing_consistent_spikes'
|
||||
|
||||
|
||||
def get_select_fields_data(logs):
|
||||
def get_nested_keys(msg, keys):
|
||||
val = None
|
||||
for key in keys:
|
||||
val = getattr(msg if val is None else val, key) if isinstance(key, str) else val[key]
|
||||
return val
|
||||
lp = [x.livePose for x in logs if x.which() == 'livePose']
|
||||
data = defaultdict(list)
|
||||
for msg in lp:
|
||||
for key, fields in SELECT_COMPARE_FIELDS.items():
|
||||
data[key].append(get_nested_keys(msg, fields))
|
||||
for key in data:
|
||||
data[key] = np.array(data[key][JUNK_IDX:], dtype=float)
|
||||
return data
|
||||
|
||||
|
||||
def modify_logs_midway(logs, which, count, fn):
|
||||
non_which = [x for x in logs if x.which() != which]
|
||||
which = [x for x in logs if x.which() == which]
|
||||
temps = which[len(which) // 2:len(which) // 2 + count]
|
||||
for i, temp in enumerate(temps):
|
||||
temp = temp.as_builder()
|
||||
fn(temp)
|
||||
which[len(which) // 2 + i] = temp.as_reader()
|
||||
return sorted(non_which + which, key=lambda x: x.logMonoTime)
|
||||
|
||||
|
||||
def run_scenarios(scenario, logs):
|
||||
if scenario == Scenario.BASE:
|
||||
pass
|
||||
|
||||
elif scenario == Scenario.GYRO_OFF:
|
||||
logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime)
|
||||
|
||||
elif scenario == Scenario.GYRO_SPIKE_MIDWAY or scenario == Scenario.GYRO_CONSISTENT_SPIKES:
|
||||
def gyro_spike(msg):
|
||||
msg.gyroscope.gyroUncalibrated.v[0] += 3.0
|
||||
count = 1 if scenario == Scenario.GYRO_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
|
||||
logs = modify_logs_midway(logs, 'gyroscope', count, gyro_spike)
|
||||
|
||||
elif scenario == Scenario.ACCEL_OFF:
|
||||
logs = sorted([x for x in logs if x.which() != 'accelerometer'], key=lambda x: x.logMonoTime)
|
||||
|
||||
elif scenario == Scenario.ACCEL_SPIKE_MIDWAY or scenario == Scenario.ACCEL_CONSISTENT_SPIKES:
|
||||
def acc_spike(msg):
|
||||
msg.accelerometer.acceleration.v[0] += 100.0
|
||||
count = 1 if scenario == Scenario.ACCEL_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
|
||||
logs = modify_logs_midway(logs, 'accelerometer', count, acc_spike)
|
||||
|
||||
elif scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY or scenario == Scenario.SENSOR_TIMING_CONSISTENT_SPIKES:
|
||||
def timing_spike(msg):
|
||||
msg.accelerometer.timestamp -= int(0.150 * 1e9)
|
||||
count = 1 if scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
|
||||
logs = modify_logs_midway(logs, 'accelerometer', count, timing_spike)
|
||||
|
||||
replayed_logs = replay_process_with_name(name='locationd', lr=logs)
|
||||
return get_select_fields_data(logs), get_select_fields_data(replayed_logs)
|
||||
|
||||
|
||||
class TestLocationdScenarios:
|
||||
"""
|
||||
Test locationd with different scenarios. In all these scenarios, we expect the following:
|
||||
- locationd kalman filter should never go unstable (we care mostly about yaw_rate, roll, gpsOK, inputsOK, sensorsOK)
|
||||
- faulty values should be ignored, with appropriate flags set
|
||||
"""
|
||||
|
||||
@classmethod
|
||||
def setup_class(cls):
|
||||
cls.logs = migrate_all(LogReader(TEST_ROUTE))
|
||||
|
||||
def test_base(self):
|
||||
"""
|
||||
Test: unchanged log
|
||||
Expected Result:
|
||||
- yaw_rate: unchanged
|
||||
- roll: unchanged
|
||||
"""
|
||||
orig_data, replayed_data = run_scenarios(Scenario.BASE, self.logs)
|
||||
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
|
||||
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
|
||||
|
||||
def test_gyro_off(self):
|
||||
"""
|
||||
Test: no gyroscope message for the entire segment
|
||||
Expected Result:
|
||||
- yaw_rate: 0
|
||||
- roll: 0
|
||||
- sensorsOK: False
|
||||
"""
|
||||
_, replayed_data = run_scenarios(Scenario.GYRO_OFF, self.logs)
|
||||
assert np.allclose(replayed_data['yaw_rate'], 0.0)
|
||||
assert np.allclose(replayed_data['roll'], 0.0)
|
||||
assert np.all(replayed_data['sensors_flag'] == 0.0)
|
||||
|
||||
def test_gyro_spike(self):
|
||||
"""
|
||||
Test: a gyroscope spike in the middle of the segment
|
||||
Expected Result:
|
||||
- yaw_rate: unchanged
|
||||
- roll: unchanged
|
||||
- inputsOK: False for some time after the spike, True for the rest
|
||||
"""
|
||||
orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs)
|
||||
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
|
||||
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
|
||||
assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag'])
|
||||
assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag'])
|
||||
|
||||
def test_consistent_gyro_spikes(self):
|
||||
"""
|
||||
Test: consistent timing spikes for N gyroscope messages in the middle of the segment
|
||||
Expected Result: inputsOK becomes False after N of bad measurements
|
||||
"""
|
||||
orig_data, replayed_data = run_scenarios(Scenario.GYRO_CONSISTENT_SPIKES, self.logs)
|
||||
assert np.diff(replayed_data['inputs_flag'])[501] == -1.0
|
||||
assert np.diff(replayed_data['inputs_flag'])[708] == 1.0
|
||||
|
||||
def test_accel_off(self):
|
||||
"""
|
||||
Test: no accelerometer message for the entire segment
|
||||
Expected Result:
|
||||
- yaw_rate: 0
|
||||
- roll: 0
|
||||
- sensorsOK: False
|
||||
"""
|
||||
_, replayed_data = run_scenarios(Scenario.ACCEL_OFF, self.logs)
|
||||
assert np.allclose(replayed_data['yaw_rate'], 0.0)
|
||||
assert np.allclose(replayed_data['roll'], 0.0)
|
||||
assert np.all(replayed_data['sensors_flag'] == 0.0)
|
||||
|
||||
def test_accel_spike(self):
|
||||
"""
|
||||
ToDo:
|
||||
Test: an accelerometer spike in the middle of the segment
|
||||
Expected Result: Right now, the kalman filter is not robust to small spikes like it is to gyroscope spikes.
|
||||
"""
|
||||
orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs)
|
||||
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
|
||||
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
|
||||
|
||||
def test_single_timing_spike(self):
|
||||
"""
|
||||
Test: timing of 150ms off for the single accelerometer message in the middle of the segment
|
||||
Expected Result: the message is ignored, and inputsOK is False for that time
|
||||
"""
|
||||
orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_SPIKE_MIDWAY, self.logs)
|
||||
assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag'])
|
||||
assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag'])
|
||||
|
||||
def test_consistent_timing_spikes(self):
|
||||
"""
|
||||
Test: consistent timing spikes for N accelerometer messages in the middle of the segment
|
||||
Expected Result: inputsOK becomes False after N of bad measurements
|
||||
"""
|
||||
orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_CONSISTENT_SPIKES, self.logs)
|
||||
assert np.diff(replayed_data['inputs_flag'])[501] == -1.0
|
||||
assert np.diff(replayed_data['inputs_flag'])[707] == 1.0
|
||||
@@ -1,67 +0,0 @@
|
||||
import random
|
||||
import numpy as np
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.selfdrive.locationd.paramsd import retrieve_initial_vehicle_params, migrate_cached_vehicle_params_if_needed
|
||||
from openpilot.selfdrive.locationd.models.car_kf import CarKalman
|
||||
from openpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE
|
||||
from openpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
|
||||
|
||||
def get_random_live_parameters(CP):
|
||||
msg = messaging.new_message("liveParameters")
|
||||
msg.liveParameters.steerRatio = (random.random() + 0.5) * CP.steerRatio
|
||||
msg.liveParameters.stiffnessFactor = random.random()
|
||||
msg.liveParameters.angleOffsetAverageDeg = random.random()
|
||||
msg.liveParameters.debugFilterState.std = [random.random() for _ in range(CarKalman.P_initial.shape[0])]
|
||||
return msg
|
||||
|
||||
|
||||
class TestParamsd:
|
||||
def test_read_saved_params(self):
|
||||
params = Params()
|
||||
|
||||
lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
|
||||
CP = next(m for m in lr if m.which() == "carParams").carParams
|
||||
|
||||
msg = get_random_live_parameters(CP)
|
||||
params.put("LiveParametersV2", msg.to_bytes())
|
||||
params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
|
||||
|
||||
migrate_cached_vehicle_params_if_needed(params) # this is not tested here but should not mess anything up or throw an error
|
||||
sr, sf, offset, p_init = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
|
||||
np.testing.assert_allclose(sr, msg.liveParameters.steerRatio)
|
||||
np.testing.assert_allclose(sf, msg.liveParameters.stiffnessFactor)
|
||||
np.testing.assert_allclose(offset, msg.liveParameters.angleOffsetAverageDeg)
|
||||
np.testing.assert_equal(p_init.shape, CarKalman.P_initial.shape)
|
||||
np.testing.assert_allclose(np.diagonal(p_init), msg.liveParameters.debugFilterState.std)
|
||||
|
||||
# TODO Remove this test after the support for old format is removed
|
||||
def test_read_saved_params_old_format(self):
|
||||
params = Params()
|
||||
|
||||
lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
|
||||
CP = next(m for m in lr if m.which() == "carParams").carParams
|
||||
|
||||
msg = get_random_live_parameters(CP)
|
||||
params.put("LiveParameters", msg.liveParameters.to_dict())
|
||||
params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
|
||||
params.remove("LiveParametersV2")
|
||||
|
||||
migrate_cached_vehicle_params_if_needed(params)
|
||||
sr, sf, offset, _ = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
|
||||
np.testing.assert_allclose(sr, msg.liveParameters.steerRatio)
|
||||
np.testing.assert_allclose(sf, msg.liveParameters.stiffnessFactor)
|
||||
np.testing.assert_allclose(offset, msg.liveParameters.angleOffsetAverageDeg)
|
||||
assert params.get("LiveParametersV2") is not None
|
||||
|
||||
def test_read_saved_params_corrupted_old_format(self):
|
||||
params = Params()
|
||||
params.put("LiveParameters", {})
|
||||
params.remove("LiveParametersV2")
|
||||
|
||||
migrate_cached_vehicle_params_if_needed(params)
|
||||
assert params.get("LiveParameters") is None
|
||||
assert params.get("LiveParametersV2") is None
|
||||
@@ -1,25 +0,0 @@
|
||||
from cereal import car
|
||||
from openpilot.selfdrive.locationd.torqued import TorqueEstimator
|
||||
|
||||
|
||||
def test_cal_percent():
|
||||
est = TorqueEstimator(car.CarParams())
|
||||
msg = est.get_msg()
|
||||
assert msg.liveTorqueParameters.calPerc == 0
|
||||
|
||||
for (low, high), min_pts in zip(est.filtered_points.buckets.keys(),
|
||||
est.filtered_points.buckets_min_points.values(), strict=True):
|
||||
for _ in range(int(min_pts)):
|
||||
est.filtered_points.add_point((low + high) / 2.0, 0.0)
|
||||
|
||||
# enough bucket points, but not enough total points
|
||||
msg = est.get_msg()
|
||||
assert msg.liveTorqueParameters.calPerc == (len(est.filtered_points) / est.min_points_total * 100 + 100) / 2
|
||||
|
||||
# add enough points to bucket with most capacity
|
||||
key = list(est.filtered_points.buckets)[0]
|
||||
for _ in range(est.min_points_total - len(est.filtered_points)):
|
||||
est.filtered_points.add_point((key[0] + key[1]) / 2.0, 0.0)
|
||||
|
||||
msg = est.get_msg()
|
||||
assert msg.liveTorqueParameters.calPerc == 100
|
||||
@@ -1,7 +1,6 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import numpy as np
|
||||
import time
|
||||
from collections import deque, defaultdict
|
||||
|
||||
import cereal.messaging as messaging
|
||||
@@ -181,7 +180,9 @@ class TorqueEstimator(ParameterEstimator):
|
||||
self.lag = msg.lateralDelay
|
||||
# calculate lateral accel from past steering torque
|
||||
elif which == "livePose":
|
||||
if len(self.raw_points['steer_torque']) == self.hist_len:
|
||||
is_valid = msg.angularVelocityDevice.valid and msg.orientationNED.valid and msg.inputsOK and msg.sensorsOK and msg.posenetOK
|
||||
if len(self.raw_points['steer_torque']) == self.hist_len and is_valid:
|
||||
t = msg.timestamp * 1e-9
|
||||
device_pose = Pose.from_live_pose(msg)
|
||||
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
|
||||
angular_velocity_calibrated = calibrated_pose.angular_velocity
|
||||
@@ -252,54 +253,24 @@ def main(demo=False):
|
||||
params = Params()
|
||||
estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams))
|
||||
|
||||
last_fail_print_t = 0.0
|
||||
|
||||
show_debug = False
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
ok_alive = sm.all_alive()
|
||||
ok_freq = sm.all_freq_ok()
|
||||
ok_valid = sm.all_valid()
|
||||
ok_all = ok_alive and ok_freq and ok_valid
|
||||
|
||||
if ok_all:
|
||||
if sm.all_checks():
|
||||
for which in sm.updated.keys():
|
||||
if sm.updated[which]:
|
||||
t = sm.logMonoTime[which] * 1e-9
|
||||
estimator.handle_log(t, which, sm[which])
|
||||
elif show_debug:
|
||||
now = time.monotonic()
|
||||
|
||||
# 너무 많이 찍히지 않게 1초에 1번만
|
||||
if now - last_fail_print_t > 1.0:
|
||||
last_fail_print_t = now
|
||||
|
||||
print(f"\n[liveTorque all_checks FAIL] frame={sm.frame} "
|
||||
f"alive={ok_alive} freq={ok_freq} valid={ok_valid}")
|
||||
|
||||
for s in sm.services:
|
||||
recv_age_ms = (now - sm.recv_time[s]) * 1000.0 if sm.recv_time[s] > 0 else -1.0
|
||||
print(
|
||||
f" {s:16s} "
|
||||
f"seen={sm.seen[s]} "
|
||||
f"updated={sm.updated[s]} "
|
||||
f"alive={sm.alive[s]} "
|
||||
f"freq_ok={sm.freq_ok[s]} "
|
||||
f"valid={sm.valid[s]} "
|
||||
f"recv_age_ms={recv_age_ms:7.1f} "
|
||||
f"logMonoTime={sm.logMonoTime[s]}"
|
||||
)
|
||||
|
||||
# 4Hz driven by livePose
|
||||
if sm.frame % 5 == 0:
|
||||
pm.send('liveTorqueParameters', estimator.get_msg(valid=ok_all))
|
||||
pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks(), with_points=DEBUG))
|
||||
|
||||
# Cache points every 60 seconds while onroad
|
||||
if sm.frame % 240 == 0:
|
||||
msg = estimator.get_msg(valid=ok_all, with_points=True)
|
||||
msg = estimator.get_msg(valid=sm.all_checks(), with_points=True)
|
||||
params.put_nonblocking("LiveTorqueParameters", msg.to_bytes())
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import argparse
|
||||
|
||||
|
||||
@@ -45,13 +45,17 @@ def tg_compile(flags, model_name):
|
||||
pkl = fn + "_tinygrad.pkl"
|
||||
onnx_path = fn + ".onnx"
|
||||
chunk_targets = get_chunk_paths(pkl, estimate_pickle_max_size(os.path.getsize(onnx_path)))
|
||||
compile_node = lenv.Command(
|
||||
pkl,
|
||||
[onnx_path] + tinygrad_files + [chunker_file],
|
||||
f'{pythonpath_string} {flags} {image_flag} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {pkl}',
|
||||
)
|
||||
def do_chunk(target, source, env):
|
||||
chunk_file(pkl, chunk_targets)
|
||||
return lenv.Command(
|
||||
chunk_targets,
|
||||
[onnx_path] + tinygrad_files + [chunker_file],
|
||||
[f'{pythonpath_string} {flags} {image_flag} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {pkl}',
|
||||
do_chunk]
|
||||
compile_node,
|
||||
do_chunk,
|
||||
)
|
||||
|
||||
# Compile small models
|
||||
|
||||
@@ -1,4 +1,3 @@
|
||||
import re
|
||||
from pathlib import Path
|
||||
Import('env', 'arch', 'common')
|
||||
|
||||
@@ -19,39 +18,38 @@ env.Command(
|
||||
|
||||
if GetOption('extras') and arch == "larch64":
|
||||
# build installers
|
||||
if arch != "Darwin":
|
||||
raylib_env = env.Clone()
|
||||
raylib_env['LIBPATH'] += [f'#third_party/raylib/{arch}/']
|
||||
raylib_env['LINKFLAGS'].append('-Wl,-strip-debug')
|
||||
raylib_env = env.Clone()
|
||||
raylib_env['LIBPATH'] += [f'#third_party/raylib/{arch}/']
|
||||
raylib_env['LINKFLAGS'].append('-Wl,-strip-debug')
|
||||
|
||||
raylib_libs = common + ["raylib"]
|
||||
if arch == "larch64":
|
||||
raylib_libs += ["GLESv2", "EGL", "gbm", "drm"]
|
||||
else:
|
||||
raylib_libs += ["GL"]
|
||||
raylib_libs = common + ["raylib"]
|
||||
if arch == "larch64":
|
||||
raylib_libs += ["GLESv2", "EGL", "gbm", "drm"]
|
||||
else:
|
||||
raylib_libs += ["GL"]
|
||||
|
||||
release = "release3"
|
||||
installers = [
|
||||
("openpilot", release),
|
||||
("openpilot_test", f"{release}-staging"),
|
||||
("openpilot_nightly", "nightly"),
|
||||
("openpilot_internal", "nightly-dev"),
|
||||
]
|
||||
release = "release3"
|
||||
installers = [
|
||||
("openpilot", release),
|
||||
("openpilot_test", f"{release}-staging"),
|
||||
("openpilot_nightly", "nightly"),
|
||||
("openpilot_internal", "nightly-dev"),
|
||||
]
|
||||
|
||||
cont = raylib_env.Command("installer/continue_openpilot.o", "installer/continue_openpilot.sh",
|
||||
cont = raylib_env.Command("installer/continue_openpilot.o", "installer/continue_openpilot.sh",
|
||||
"ld -r -b binary -o $TARGET $SOURCE")
|
||||
inter = raylib_env.Command("installer/inter_ttf.o", "installer/inter-ascii.ttf",
|
||||
"ld -r -b binary -o $TARGET $SOURCE")
|
||||
inter_bold = raylib_env.Command("installer/inter_bold.o", "../assets/fonts/Inter-Bold.ttf",
|
||||
"ld -r -b binary -o $TARGET $SOURCE")
|
||||
inter = raylib_env.Command("installer/inter_ttf.o", "installer/inter-ascii.ttf",
|
||||
"ld -r -b binary -o $TARGET $SOURCE")
|
||||
inter_bold = raylib_env.Command("installer/inter_bold.o", "../assets/fonts/Inter-Bold.ttf",
|
||||
"ld -r -b binary -o $TARGET $SOURCE")
|
||||
inter_light = raylib_env.Command("installer/inter_light.o", "../assets/fonts/Inter-Light.ttf",
|
||||
"ld -r -b binary -o $TARGET $SOURCE")
|
||||
for name, branch in installers:
|
||||
d = {'BRANCH': f"'\"{branch}\"'"}
|
||||
if "internal" in name:
|
||||
d['INTERNAL'] = "1"
|
||||
inter_light = raylib_env.Command("installer/inter_light.o", "../assets/fonts/Inter-Light.ttf",
|
||||
"ld -r -b binary -o $TARGET $SOURCE")
|
||||
for name, branch in installers:
|
||||
d = {'BRANCH': f"'\"{branch}\"'"}
|
||||
if "internal" in name:
|
||||
d['INTERNAL'] = "1"
|
||||
|
||||
obj = raylib_env.Object(f"installer/installers/installer_{name}.o", ["installer/installer.cc"], CPPDEFINES=d)
|
||||
f = raylib_env.Program(f"installer/installers/installer_{name}", [obj, cont, inter, inter_bold, inter_light], LIBS=raylib_libs)
|
||||
# keep installers small
|
||||
assert f[0].get_size() < 2500*1e3, f[0].get_size()
|
||||
obj = raylib_env.Object(f"installer/installers/installer_{name}.o", ["installer/installer.cc"], CPPDEFINES=d)
|
||||
f = raylib_env.Program(f"installer/installers/installer_{name}", [obj, cont, inter, inter_bold, inter_light], LIBS=raylib_libs)
|
||||
# keep installers small
|
||||
assert f[0].get_size() < 2500*1e3, f[0].get_size()
|
||||
|
||||
@@ -81,14 +81,16 @@ void run(const char* cmd) {
|
||||
}
|
||||
|
||||
void finishInstall() {
|
||||
if (tici_device) {
|
||||
BeginDrawing();
|
||||
ClearBackground(BLACK);
|
||||
BeginDrawing();
|
||||
ClearBackground(BLACK);
|
||||
if (tici_device) {
|
||||
const char *m = "Finishing install...";
|
||||
int text_width = MeasureText(m, FONT_SIZE);
|
||||
DrawTextEx(font_display, m, (Vector2){(float)(GetScreenWidth() - text_width)/2 + FONT_SIZE, (float)(GetScreenHeight() - FONT_SIZE)/2}, FONT_SIZE, 0, WHITE);
|
||||
EndDrawing();
|
||||
}
|
||||
} else {
|
||||
DrawTextEx(font_display, "finishing setup", (Vector2){12, 0}, 77, 0, (Color){255, 255, 255, (unsigned char)(255 * 0.9)});
|
||||
}
|
||||
EndDrawing();
|
||||
util::sleep_for(60 * 1000);
|
||||
}
|
||||
|
||||
|
||||
@@ -62,6 +62,7 @@ class HomeLayout(Widget):
|
||||
self._setup_callbacks()
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._exp_mode_button.show_event()
|
||||
self.last_refresh = time.monotonic()
|
||||
self._refresh()
|
||||
|
||||
@@ -91,7 +91,7 @@ class TrainingGuide(Widget):
|
||||
def _render(self, _):
|
||||
# Safeguard against fast tapping
|
||||
step = min(self._step, len(self._textures) - 1)
|
||||
rl.draw_texture(self._textures[step], 0, 0, rl.WHITE)
|
||||
rl.draw_texture_ex(self._textures[step], rl.Vector2(0, 0), 0.0, 1.0, rl.WHITE)
|
||||
|
||||
# progress bar
|
||||
if 0 < step < len(STEP_RECTS) - 1:
|
||||
|
||||
@@ -100,6 +100,7 @@ class DeveloperLayout(Widget):
|
||||
self._scroller.render(rect)
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._scroller.show_event()
|
||||
self._update_toggles()
|
||||
|
||||
|
||||
@@ -72,6 +72,7 @@ class DeviceLayout(Widget):
|
||||
self._power_off_btn.action_item.right_button.set_visible(ui_state.is_offroad())
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._scroller.show_event()
|
||||
|
||||
def _render(self, rect):
|
||||
|
||||
@@ -80,6 +80,7 @@ class SoftwareLayout(Widget):
|
||||
], line_separator=True, spacing=0)
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._scroller.show_event()
|
||||
|
||||
def _render(self, rect):
|
||||
|
||||
@@ -148,6 +148,7 @@ class TogglesLayout(Widget):
|
||||
ui_state.personality = personality
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._scroller.show_event()
|
||||
self._update_toggles()
|
||||
|
||||
|
||||
@@ -161,14 +161,14 @@ class Sidebar(Widget):
|
||||
# Settings button
|
||||
settings_down = mouse_down and rl.check_collision_point_rec(mouse_pos, SETTINGS_BTN)
|
||||
tint = Colors.BUTTON_PRESSED if settings_down else Colors.BUTTON_NORMAL
|
||||
rl.draw_texture(self._settings_img, int(SETTINGS_BTN.x), int(SETTINGS_BTN.y), tint)
|
||||
rl.draw_texture_ex(self._settings_img, rl.Vector2(SETTINGS_BTN.x, SETTINGS_BTN.y), 0.0, 1.0, tint)
|
||||
|
||||
# Home/Flag button
|
||||
flag_pressed = mouse_down and rl.check_collision_point_rec(mouse_pos, HOME_BTN)
|
||||
button_img = self._flag_img if ui_state.started else self._home_img
|
||||
|
||||
tint = Colors.BUTTON_PRESSED if (ui_state.started and flag_pressed) else Colors.BUTTON_NORMAL
|
||||
rl.draw_texture(button_img, int(HOME_BTN.x), int(HOME_BTN.y), tint)
|
||||
rl.draw_texture_ex(button_img, rl.Vector2(HOME_BTN.x, HOME_BTN.y), 0.0, 1.0, tint)
|
||||
|
||||
# Microphone button
|
||||
if self._recording_audio:
|
||||
@@ -178,8 +178,8 @@ class Sidebar(Widget):
|
||||
bg_color = rl.Color(Colors.DANGER.r, Colors.DANGER.g, Colors.DANGER.b, int(255 * 0.65)) if mic_pressed else Colors.DANGER
|
||||
|
||||
rl.draw_rectangle_rounded(self._mic_indicator_rect, 1, 10, bg_color)
|
||||
rl.draw_texture(self._mic_img, int(self._mic_indicator_rect.x + (self._mic_indicator_rect.width - self._mic_img.width) / 2),
|
||||
int(self._mic_indicator_rect.y + (self._mic_indicator_rect.height - self._mic_img.height) / 2), Colors.WHITE)
|
||||
rl.draw_texture_ex(self._mic_img, rl.Vector2(self._mic_indicator_rect.x + (self._mic_indicator_rect.width - self._mic_img.width) / 2,
|
||||
self._mic_indicator_rect.y + (self._mic_indicator_rect.height - self._mic_img.height) / 2), 0.0, 1.0, Colors.WHITE)
|
||||
|
||||
def _draw_network_indicator(self, rect: rl.Rectangle):
|
||||
# Signal strength dots
|
||||
|
||||
@@ -7,7 +7,7 @@ from collections.abc import Callable
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
from openpilot.system.ui.widgets.layouts import HBoxLayout
|
||||
from openpilot.system.ui.widgets.icon_widget import IconWidget
|
||||
from openpilot.system.ui.widgets.label import MiciLabel, UnifiedLabel
|
||||
from openpilot.system.ui.widgets.label import UnifiedLabel
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.version import RELEASE_BRANCHES
|
||||
@@ -77,7 +77,7 @@ class NetworkIcon(Widget):
|
||||
# Offset by difference in height between slashless and slash icons to make center align match
|
||||
draw_y -= (self._wifi_slash_txt.height - self._wifi_none_txt.height) / 2
|
||||
|
||||
rl.draw_texture(draw_net_txt, int(draw_x), int(draw_y), rl.Color(255, 255, 255, int(255 * 0.9)))
|
||||
rl.draw_texture_ex(draw_net_txt, rl.Vector2(draw_x, draw_y), 0.0, 1.0, rl.Color(255, 255, 255, int(255 * 0.9)))
|
||||
|
||||
|
||||
class MiciHomeLayout(Widget):
|
||||
@@ -103,14 +103,15 @@ class MiciHomeLayout(Widget):
|
||||
self._mic_icon,
|
||||
], spacing=18)
|
||||
|
||||
self._openpilot_label = MiciLabel("carrotpilot", font_size=80, color=rl.Color(255, 255, 255, int(255 * 0.9)), font_weight=FontWeight.DISPLAY)
|
||||
self._version_label = MiciLabel("", font_size=36, font_weight=FontWeight.ROMAN)
|
||||
self._large_version_label = MiciLabel("", font_size=64, color=rl.GRAY, font_weight=FontWeight.ROMAN)
|
||||
self._date_label = MiciLabel("", font_size=36, color=rl.GRAY, font_weight=FontWeight.ROMAN)
|
||||
self._openpilot_label = UnifiedLabel("openpilot", font_size=96, font_weight=FontWeight.DISPLAY, max_width=480, wrap_text=False)
|
||||
self._version_label = UnifiedLabel("", font_size=36, font_weight=FontWeight.ROMAN, max_width=480, wrap_text=False)
|
||||
self._large_version_label = UnifiedLabel("", font_size=64, text_color=rl.GRAY, font_weight=FontWeight.ROMAN, max_width=480, wrap_text=False)
|
||||
self._date_label = UnifiedLabel("", font_size=36, text_color=rl.GRAY, font_weight=FontWeight.ROMAN, max_width=480, wrap_text=False)
|
||||
self._branch_label = UnifiedLabel("", font_size=36, text_color=rl.GRAY, font_weight=FontWeight.ROMAN, scroll=True)
|
||||
self._version_commit_label = MiciLabel("", font_size=36, color=rl.GRAY, font_weight=FontWeight.ROMAN)
|
||||
self._version_commit_label = UnifiedLabel("", font_size=36, text_color=rl.GRAY, font_weight=FontWeight.ROMAN, max_width=480, wrap_text=False)
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._version_text = self._get_version_text()
|
||||
self._update_params()
|
||||
|
||||
@@ -182,12 +183,12 @@ class MiciHomeLayout(Widget):
|
||||
self._version_label.render()
|
||||
|
||||
self._date_label.set_text(" " + self._version_text[3])
|
||||
self._date_label.set_position(version_pos.x + self._version_label.rect.width + 10, version_pos.y)
|
||||
self._date_label.set_position(version_pos.x + self._version_label.text_width + 10, version_pos.y)
|
||||
self._date_label.render()
|
||||
|
||||
self._branch_label.set_max_width(gui_app.width - self._version_label.rect.width - self._date_label.rect.width - 32)
|
||||
self._branch_label.set_max_width(gui_app.width - self._version_label.text_width - self._date_label.text_width - 32)
|
||||
self._branch_label.set_text(" " + ("release" if release_branch else self._version_text[1]))
|
||||
self._branch_label.set_position(version_pos.x + self._version_label.rect.width + self._date_label.rect.width + 20, version_pos.y)
|
||||
self._branch_label.set_position(version_pos.x + self._version_label.text_width + self._date_label.text_width + 20, version_pos.y)
|
||||
self._branch_label.render()
|
||||
|
||||
if not release_branch:
|
||||
|
||||
@@ -144,7 +144,7 @@ class AlertItem(Widget):
|
||||
bg_texture = self._bg_small_pressed if self.is_pressed else self._bg_small
|
||||
|
||||
# Draw background
|
||||
rl.draw_texture(bg_texture, int(self._rect.x), int(self._rect.y), rl.WHITE)
|
||||
rl.draw_texture_ex(bg_texture, rl.Vector2(self._rect.x, self._rect.y), 0.0, 1.0, rl.WHITE)
|
||||
|
||||
# Calculate text area (left side, avoiding icon on right)
|
||||
title_width = self.ALERT_WIDTH - (self.ALERT_PADDING * 2) - self.ICON_SIZE - self.ICON_MARGIN
|
||||
@@ -183,7 +183,7 @@ class AlertItem(Widget):
|
||||
icon_texture = self._icon_orange
|
||||
icon_x = self._rect.x + self.ALERT_WIDTH - self.ALERT_PADDING - self.ICON_SIZE
|
||||
icon_y = self._rect.y + self.ALERT_PADDING
|
||||
rl.draw_texture(icon_texture, int(icon_x), int(icon_y), rl.WHITE)
|
||||
rl.draw_texture_ex(icon_texture, rl.Vector2(icon_x, icon_y), 0.0, 1.0, rl.WHITE)
|
||||
|
||||
|
||||
class MiciOffroadAlerts(Scroller):
|
||||
|
||||
@@ -14,8 +14,7 @@ from openpilot.system.ui.widgets.label import gui_label
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.version import terms_version, training_version
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, device
|
||||
from openpilot.selfdrive.ui.mici.widgets.button import BigCircleButton
|
||||
from openpilot.selfdrive.ui.mici.widgets.dialog import BigConfirmationDialogV2
|
||||
from openpilot.selfdrive.ui.mici.widgets.dialog import BigConfirmationCircleButton
|
||||
from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
|
||||
from openpilot.selfdrive.ui.mici.onroad.driver_camera_dialog import BaseDriverCameraDialog
|
||||
|
||||
@@ -152,8 +151,10 @@ class TrainingGuideDMTutorial(NavWidget):
|
||||
def _render(self, _):
|
||||
self._dialog.render(self._rect)
|
||||
|
||||
rl.draw_rectangle_gradient_v(int(self._rect.x), int(self._rect.y + self._rect.height - 80),
|
||||
int(self._rect.width), 80, rl.BLANK, rl.BLACK)
|
||||
gradient_y = int(self._rect.y + self._rect.height - 80)
|
||||
gradient_h = int(self._rect.y) + int(self._rect.height) - gradient_y
|
||||
rl.draw_rectangle_gradient_v(int(self._rect.x), gradient_y,
|
||||
int(self._rect.width), gradient_h, rl.BLANK, rl.BLACK)
|
||||
|
||||
# draw white ring around dm icon to indicate progress
|
||||
ring_thickness = 8
|
||||
@@ -215,26 +216,19 @@ class TrainingGuideRecordFront(NavScroller):
|
||||
def __init__(self, continue_callback: Callable[[], None]):
|
||||
super().__init__()
|
||||
|
||||
def show_accept_dialog():
|
||||
def on_accept():
|
||||
ui_state.params.put_bool_nonblocking("RecordFront", True)
|
||||
continue_callback()
|
||||
def on_accept():
|
||||
ui_state.params.put_bool_nonblocking("RecordFront", True)
|
||||
continue_callback()
|
||||
|
||||
gui_app.push_widget(BigConfirmationDialogV2("allow data uploading", "icons_mici/setup/driver_monitoring/dm_check.png", exit_on_confirm=False,
|
||||
confirm_callback=on_accept))
|
||||
def on_decline():
|
||||
ui_state.params.put_bool_nonblocking("RecordFront", False)
|
||||
continue_callback()
|
||||
|
||||
def show_decline_dialog():
|
||||
def on_decline():
|
||||
ui_state.params.put_bool_nonblocking("RecordFront", False)
|
||||
continue_callback()
|
||||
self._accept_button = BigConfirmationCircleButton("allow data uploading", gui_app.texture("icons_mici/setup/driver_monitoring/dm_check.png", 64, 64),
|
||||
on_accept, exit_on_confirm=False)
|
||||
|
||||
gui_app.push_widget(BigConfirmationDialogV2("no, don't upload", "icons_mici/setup/cancel.png", exit_on_confirm=False, confirm_callback=on_decline))
|
||||
|
||||
self._accept_button = BigCircleButton("icons_mici/setup/driver_monitoring/dm_check.png")
|
||||
self._accept_button.set_click_callback(show_accept_dialog)
|
||||
|
||||
self._decline_button = BigCircleButton("icons_mici/setup/cancel.png")
|
||||
self._decline_button.set_click_callback(show_decline_dialog)
|
||||
self._decline_button = BigConfirmationCircleButton("no, don't upload", gui_app.texture("icons_mici/setup/cancel.png", 64, 64), on_decline,
|
||||
exit_on_confirm=False)
|
||||
|
||||
self._scroller.add_widgets([
|
||||
GreyBigButton("driver camera data", "do you want to share video data for training?",
|
||||
@@ -274,12 +268,9 @@ class TrainingGuide(NavWidget):
|
||||
TrainingGuideRecordFront(continue_callback=completed_callback),
|
||||
]
|
||||
|
||||
self._child(self._steps[0])
|
||||
self._steps[0].set_enabled(lambda: self.enabled and not self.is_dismissing) # for nav stack
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._steps[0].show_event()
|
||||
|
||||
def _render(self, _):
|
||||
self._steps[0].render(self._rect)
|
||||
|
||||
@@ -312,7 +303,7 @@ class QRCodeWidget(Widget):
|
||||
def _render(self, _):
|
||||
if self._qr_texture:
|
||||
scale = self._size / self._qr_texture.height
|
||||
rl.draw_texture_ex(self._qr_texture, rl.Vector2(self._rect.x, self._rect.y), 0.0, scale, rl.WHITE)
|
||||
rl.draw_texture_ex(self._qr_texture, rl.Vector2(round(self._rect.x), round(self._rect.y)), 0.0, scale, rl.WHITE)
|
||||
|
||||
def __del__(self):
|
||||
if self._qr_texture and self._qr_texture.id != 0:
|
||||
@@ -323,27 +314,20 @@ class TermsPage(Scroller):
|
||||
def __init__(self, on_accept, on_decline):
|
||||
super().__init__()
|
||||
|
||||
def show_accept_dialog():
|
||||
gui_app.push_widget(BigConfirmationDialogV2("accept\nterms", "icons_mici/setup/driver_monitoring/dm_check.png",
|
||||
confirm_callback=on_accept))
|
||||
self._accept_button = BigConfirmationCircleButton("accept\nterms", gui_app.texture("icons_mici/setup/driver_monitoring/dm_check.png", 64, 64), on_accept)
|
||||
self._decline_button = BigConfirmationCircleButton("decline &\nuninstall", gui_app.texture("icons_mici/setup/cancel.png", 64, 64), on_decline,
|
||||
red=True, exit_on_confirm=False)
|
||||
|
||||
def show_decline_dialog():
|
||||
gui_app.push_widget(BigConfirmationDialogV2("decline &\nuninstall", "icons_mici/setup/cancel.png",
|
||||
red=True, exit_on_confirm=False, confirm_callback=on_decline))
|
||||
|
||||
self._accept_button = BigCircleButton("icons_mici/setup/driver_monitoring/dm_check.png")
|
||||
self._accept_button.set_click_callback(show_accept_dialog)
|
||||
|
||||
self._decline_button = BigCircleButton("icons_mici/setup/cancel.png", red=True)
|
||||
self._decline_button.set_click_callback(show_decline_dialog)
|
||||
self._terms_header = GreyBigButton("terms and\nconditions", "scroll to continue",
|
||||
gui_app.texture("icons_mici/setup/green_info.png", 64, 64))
|
||||
self._must_accept_card = GreyBigButton("", "You must accept the Terms & Conditions to use openpilot.")
|
||||
|
||||
self._scroller.add_widgets([
|
||||
GreyBigButton("terms and\nconditions", "scroll to continue",
|
||||
gui_app.texture("icons_mici/setup/green_info.png", 64, 64)),
|
||||
self._terms_header,
|
||||
GreyBigButton("swipe for QR code", "or go to https://comma.ai/terms",
|
||||
gui_app.texture("icons_mici/setup/small_slider/slider_arrow.png", 64, 56, flip_x=True)),
|
||||
QRCodeWidget("https://comma.ai/terms"),
|
||||
GreyBigButton("", "You must accept the Terms & Conditions to use openpilot."),
|
||||
self._must_accept_card,
|
||||
self._accept_button,
|
||||
self._decline_button,
|
||||
])
|
||||
|
||||
@@ -5,32 +5,37 @@ from openpilot.selfdrive.ui.mici.widgets.dialog import BigDialog, BigInputDialog
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.selfdrive.ui.layouts.settings.common import restart_needed_callback
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.selfdrive.ui.widgets.ssh_key import SshKeyAction
|
||||
from openpilot.selfdrive.ui.widgets.ssh_key import SshKeyFetcher
|
||||
|
||||
|
||||
class DeveloperLayoutMici(NavScroller):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self._ssh_fetcher = SshKeyFetcher(ui_state.params)
|
||||
|
||||
def github_username_callback(username: str):
|
||||
if username:
|
||||
ssh_keys = SshKeyAction()
|
||||
ssh_keys._fetch_ssh_key(username)
|
||||
if not ssh_keys._error_message:
|
||||
self._ssh_keys_btn.set_value(username)
|
||||
else:
|
||||
dlg = BigDialog("", ssh_keys._error_message)
|
||||
gui_app.push_widget(dlg)
|
||||
self._ssh_keys_btn.set_value("Loading...")
|
||||
self._ssh_keys_btn.set_enabled(False)
|
||||
|
||||
def on_response(error):
|
||||
self._ssh_keys_btn.set_enabled(True)
|
||||
if error is None:
|
||||
self._ssh_keys_btn.set_value(username)
|
||||
else:
|
||||
self._ssh_keys_btn.set_value("Not set")
|
||||
gui_app.push_widget(BigDialog("", error))
|
||||
|
||||
self._ssh_fetcher.fetch(username, on_response)
|
||||
else:
|
||||
ui_state.params.remove("GithubUsername")
|
||||
ui_state.params.remove("GithubSshKeys")
|
||||
self._ssh_fetcher.clear()
|
||||
self._ssh_keys_btn.set_value("Not set")
|
||||
|
||||
def ssh_keys_callback():
|
||||
github_username = ui_state.params.get("GithubUsername") or ""
|
||||
dlg = BigInputDialog("enter GitHub username...", github_username, minimum_length=0, confirm_callback=github_username_callback)
|
||||
if not system_time_valid():
|
||||
dlg = BigDialog("Please connect to Wi-Fi to fetch your key", "")
|
||||
dlg = BigDialog("", "Please connect to Wi-Fi to fetch your key.")
|
||||
gui_app.push_widget(dlg)
|
||||
return
|
||||
gui_app.push_widget(dlg)
|
||||
@@ -42,8 +47,8 @@ class DeveloperLayoutMici(NavScroller):
|
||||
|
||||
# adb, ssh, ssh keys, debug mode, joystick debug mode, longitudinal maneuver mode, ip address
|
||||
# ******** Main Scroller ********
|
||||
self._adb_toggle = BigCircleParamControl("icons_mici/adb_short.png", "AdbEnabled", icon_size=(82, 82), icon_offset=(0, 12))
|
||||
self._ssh_toggle = BigCircleParamControl("icons_mici/ssh_short.png", "SshEnabled", icon_size=(82, 82), icon_offset=(0, 12))
|
||||
self._adb_toggle = BigCircleParamControl(gui_app.texture("icons_mici/adb_short.png", 82, 82), "AdbEnabled", icon_offset=(0, 12))
|
||||
self._ssh_toggle = BigCircleParamControl(gui_app.texture("icons_mici/ssh_short.png", 82, 82), "SshEnabled", icon_offset=(0, 12))
|
||||
self._joystick_toggle = BigToggle("joystick debug mode",
|
||||
initial_state=ui_state.params.get_bool("JoystickDebugMode"),
|
||||
toggle_callback=self._on_joystick_debug_mode)
|
||||
@@ -99,6 +104,10 @@ class DeveloperLayoutMici(NavScroller):
|
||||
|
||||
ui_state.add_offroad_transition_callback(self._update_toggles)
|
||||
|
||||
def _update_state(self):
|
||||
super()._update_state()
|
||||
self._ssh_fetcher.update()
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._update_toggles()
|
||||
|
||||
@@ -9,16 +9,15 @@ from openpilot.common.params import Params
|
||||
from openpilot.common.time_helpers import system_time_valid
|
||||
from openpilot.system.ui.widgets.scroller import NavRawScrollPanel, NavScroller
|
||||
from openpilot.selfdrive.ui.mici.widgets.button import BigButton, BigCircleButton
|
||||
from openpilot.selfdrive.ui.mici.widgets.dialog import BigDialog, BigConfirmationDialogV2
|
||||
from openpilot.selfdrive.ui.mici.widgets.dialog import BigDialog, BigConfirmationDialog
|
||||
from openpilot.selfdrive.ui.mici.widgets.pairing_dialog import PairingDialog
|
||||
from openpilot.selfdrive.ui.mici.onroad.driver_camera_dialog import DriverCameraDialog
|
||||
from openpilot.selfdrive.ui.mici.layouts.onboarding import TrainingGuide, TermsPage
|
||||
from openpilot.system.ui.mici_setup import BigPillButton
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
from openpilot.selfdrive.ui.ui_state import device, ui_state
|
||||
from openpilot.system.ui.widgets.label import MiciLabel
|
||||
from openpilot.system.ui.widgets.label import UnifiedLabel
|
||||
from openpilot.system.ui.widgets.html_render import HtmlModal, HtmlRenderer
|
||||
from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID
|
||||
|
||||
@@ -27,13 +26,11 @@ class ReviewTermsPage(TermsPage, NavScroller):
|
||||
"""TermsPage with NavWidget swipe-to-dismiss for reviewing in device settings."""
|
||||
def __init__(self):
|
||||
super().__init__(on_accept=self.dismiss, on_decline=self.dismiss)
|
||||
self._terms_header.set_visible(False)
|
||||
self._must_accept_card.set_visible(False)
|
||||
self._accept_button.set_visible(False)
|
||||
self._decline_button.set_visible(False)
|
||||
|
||||
close_button = BigPillButton("close")
|
||||
close_button.set_click_callback(self.dismiss)
|
||||
self._scroller.add_widget(close_button)
|
||||
|
||||
|
||||
class ReviewTrainingGuide(TrainingGuide):
|
||||
def show_event(self):
|
||||
@@ -67,34 +64,31 @@ class MiciFccModal(NavRawScrollPanel):
|
||||
rl.draw_texture_ex(self._fcc_logo, fcc_pos, 0.0, 1.0, rl.WHITE)
|
||||
|
||||
|
||||
def _engaged_confirmation_callback(callback: Callable, action_text: str):
|
||||
def _engaged_confirmation_click(callback: Callable, action_text: str, icon: rl.Texture, exit_on_confirm: bool = True, red: bool = False):
|
||||
if not ui_state.engaged:
|
||||
def confirm_callback():
|
||||
# Check engaged again in case it changed while the dialog was open
|
||||
# TODO: if true, we stay on the dialog if not exit_on_confirm until normal onroad timeout
|
||||
if not ui_state.engaged:
|
||||
callback()
|
||||
|
||||
red = False
|
||||
if action_text == "power off":
|
||||
icon = "icons_mici/settings/device/power.png"
|
||||
red = True
|
||||
elif action_text == "reboot":
|
||||
icon = "icons_mici/settings/device/reboot.png"
|
||||
elif action_text == "reset":
|
||||
icon = "icons_mici/settings/device/lkas.png"
|
||||
elif action_text == "uninstall":
|
||||
icon = "icons_mici/settings/device/uninstall.png"
|
||||
else:
|
||||
# TODO: check
|
||||
icon = "icons_mici/settings/comma_icon.png"
|
||||
|
||||
dlg: BigConfirmationDialogV2 | BigDialog = BigConfirmationDialogV2(f"slide to\n{action_text.lower()}", icon, red=red,
|
||||
exit_on_confirm=action_text == "reset",
|
||||
confirm_callback=confirm_callback)
|
||||
gui_app.push_widget(dlg)
|
||||
gui_app.push_widget(BigConfirmationDialog(f"slide to\n{action_text.lower()}", icon, confirm_callback, exit_on_confirm=exit_on_confirm, red=red))
|
||||
else:
|
||||
dlg = BigDialog(f"Disengage to {action_text}", "")
|
||||
gui_app.push_widget(dlg)
|
||||
gui_app.push_widget(BigDialog("", f"Disengage to {action_text}"))
|
||||
|
||||
|
||||
class EngagedConfirmationCircleButton(BigCircleButton):
|
||||
def __init__(self, title: str, icon: rl.Texture, callback: Callable[[], None], exit_on_confirm: bool = True,
|
||||
red: bool = False, icon_offset: tuple[int, int] = (0, 0)):
|
||||
super().__init__(icon, red, icon_offset)
|
||||
self.set_click_callback(lambda: _engaged_confirmation_click(callback, title, icon, exit_on_confirm=exit_on_confirm, red=red))
|
||||
|
||||
|
||||
class EngagedConfirmationButton(BigButton):
|
||||
def __init__(self, text: str, action_text: str, icon: rl.Texture, callback: Callable[[], None],
|
||||
exit_on_confirm: bool = True, red: bool = False):
|
||||
super().__init__(text, "", icon)
|
||||
self.set_click_callback(lambda: _engaged_confirmation_click(callback, action_text, icon, exit_on_confirm=exit_on_confirm, red=red))
|
||||
|
||||
|
||||
class DeviceInfoLayoutMici(Widget):
|
||||
@@ -104,14 +98,15 @@ class DeviceInfoLayoutMici(Widget):
|
||||
self.set_rect(rl.Rectangle(0, 0, 360, 180))
|
||||
|
||||
params = Params()
|
||||
header_color = rl.Color(255, 255, 255, int(255 * 0.9))
|
||||
subheader_color = rl.Color(255, 255, 255, int(255 * 0.9 * 0.65))
|
||||
max_width = int(self._rect.width - 20)
|
||||
self._dongle_id_label = MiciLabel("device ID", 48, width=max_width, color=header_color, font_weight=FontWeight.DISPLAY)
|
||||
self._dongle_id_text_label = MiciLabel(params.get("DongleId") or 'N/A', 32, width=max_width, color=subheader_color, font_weight=FontWeight.ROMAN)
|
||||
self._dongle_id_label = UnifiedLabel("device ID", 48, max_width=max_width, font_weight=FontWeight.DISPLAY, wrap_text=False)
|
||||
self._dongle_id_text_label = UnifiedLabel(params.get("DongleId") or 'N/A', 32, max_width=max_width, text_color=subheader_color,
|
||||
font_weight=FontWeight.ROMAN, wrap_text=False)
|
||||
|
||||
self._serial_number_label = MiciLabel("serial", 48, color=header_color, font_weight=FontWeight.DISPLAY)
|
||||
self._serial_number_text_label = MiciLabel(params.get("HardwareSerial") or 'N/A', 32, width=max_width, color=subheader_color, font_weight=FontWeight.ROMAN)
|
||||
self._serial_number_label = UnifiedLabel("serial", 48, max_width=max_width, font_weight=FontWeight.DISPLAY, wrap_text=False)
|
||||
self._serial_number_text_label = UnifiedLabel(params.get("HardwareSerial") or 'N/A', 32, max_width=max_width, text_color=subheader_color,
|
||||
font_weight=FontWeight.ROMAN, wrap_text=False)
|
||||
|
||||
def _render(self, _):
|
||||
self._dongle_id_label.set_position(self._rect.x + 20, self._rect.y - 10)
|
||||
@@ -135,7 +130,7 @@ class UpdaterState(IntEnum):
|
||||
|
||||
class PairBigButton(BigButton):
|
||||
def __init__(self):
|
||||
super().__init__("pair", "connect.comma.ai", "icons_mici/settings/comma_icon.png", icon_size=(33, 60))
|
||||
super().__init__("pair", "connect.comma.ai", gui_app.texture("icons_mici/settings/comma_icon.png", 33, 60))
|
||||
|
||||
def _get_label_font_size(self):
|
||||
return 64
|
||||
@@ -161,9 +156,9 @@ class PairBigButton(BigButton):
|
||||
return
|
||||
dlg: BigDialog | PairingDialog
|
||||
if not system_time_valid():
|
||||
dlg = BigDialog(tr("Please connect to Wi-Fi to complete initial pairing"), "")
|
||||
dlg = BigDialog("", tr("Please connect to Wi-Fi to complete initial pairing."))
|
||||
elif UNREGISTERED_DONGLE_ID == (ui_state.params.get("DongleId") or UNREGISTERED_DONGLE_ID):
|
||||
dlg = BigDialog(tr("Device must be registered with the comma.ai backend to pair"), "")
|
||||
dlg = BigDialog("", tr("Device must be registered with the comma.ai backend to pair."))
|
||||
else:
|
||||
dlg = PairingDialog()
|
||||
gui_app.push_widget(dlg)
|
||||
@@ -193,7 +188,7 @@ class UpdateOpenpilotBigButton(BigButton):
|
||||
super()._handle_mouse_release(mouse_pos)
|
||||
|
||||
if not system_time_valid():
|
||||
dlg = BigDialog(tr("Please connect to Wi-Fi to update"), "")
|
||||
dlg = BigDialog("", tr("Please connect to Wi-Fi to update."))
|
||||
gui_app.push_widget(dlg)
|
||||
return
|
||||
|
||||
@@ -314,33 +309,33 @@ class DeviceLayoutMici(NavScroller):
|
||||
def uninstall_openpilot_callback():
|
||||
ui_state.params.put_bool("DoUninstall", True)
|
||||
|
||||
reset_calibration_btn = BigButton("reset calibration", "", "icons_mici/settings/device/lkas.png", icon_size=(114, 60))
|
||||
reset_calibration_btn.set_click_callback(lambda: _engaged_confirmation_callback(reset_calibration_callback, "reset"))
|
||||
reset_calibration_btn = EngagedConfirmationButton("reset calibration", "reset", gui_app.texture("icons_mici/settings/device/lkas.png", 122, 64),
|
||||
reset_calibration_callback)
|
||||
|
||||
uninstall_openpilot_btn = BigButton("uninstall openpilot", "", "icons_mici/settings/device/uninstall.png")
|
||||
uninstall_openpilot_btn.set_click_callback(lambda: _engaged_confirmation_callback(uninstall_openpilot_callback, "uninstall"))
|
||||
uninstall_openpilot_btn = EngagedConfirmationButton("uninstall openpilot", "uninstall",
|
||||
gui_app.texture("icons_mici/settings/device/uninstall.png", 64, 64),
|
||||
uninstall_openpilot_callback, exit_on_confirm=False)
|
||||
|
||||
reboot_btn = BigCircleButton("icons_mici/settings/device/reboot.png", red=False, icon_size=(64, 70))
|
||||
reboot_btn.set_click_callback(lambda: _engaged_confirmation_callback(reboot_callback, "reboot"))
|
||||
reboot_btn = EngagedConfirmationCircleButton("reboot", gui_app.texture("icons_mici/settings/device/reboot.png", 64, 70),
|
||||
reboot_callback, exit_on_confirm=False)
|
||||
|
||||
self._power_off_btn = BigCircleButton("icons_mici/settings/device/power.png", red=True, icon_size=(64, 66))
|
||||
self._power_off_btn.set_click_callback(lambda: _engaged_confirmation_callback(power_off_callback, "power off"))
|
||||
self._power_off_btn = EngagedConfirmationCircleButton("power off", gui_app.texture("icons_mici/settings/device/power.png", 64, 66),
|
||||
power_off_callback, exit_on_confirm=False, red=True)
|
||||
self._power_off_btn.set_visible(lambda: not ui_state.ignition)
|
||||
|
||||
regulatory_btn = BigButton("regulatory info", "", "icons_mici/settings/device/info.png")
|
||||
regulatory_btn = BigButton("regulatory info", "", gui_app.texture("icons_mici/settings/device/info.png", 64, 64))
|
||||
regulatory_btn.set_click_callback(self._on_regulatory)
|
||||
|
||||
driver_cam_btn = BigButton("driver\ncamera preview", "", "icons_mici/settings/device/cameras.png")
|
||||
driver_cam_btn = BigButton("driver\ncamera preview", "", gui_app.texture("icons_mici/settings/device/cameras.png", 64, 64))
|
||||
driver_cam_btn.set_click_callback(lambda: gui_app.push_widget(DriverCameraDialog()))
|
||||
driver_cam_btn.set_enabled(lambda: ui_state.is_offroad())
|
||||
|
||||
review_training_guide_btn = BigButton("review\ntraining guide", "", "icons_mici/settings/device/info.png")
|
||||
review_training_guide_btn = BigButton("review\ntraining guide", "", gui_app.texture("icons_mici/settings/device/info.png", 64, 64))
|
||||
review_training_guide_btn.set_click_callback(lambda: gui_app.push_widget(ReviewTrainingGuide(completed_callback=lambda: gui_app.pop_widgets_to(self))))
|
||||
review_training_guide_btn.set_enabled(lambda: ui_state.is_offroad())
|
||||
|
||||
terms_btn = BigButton("terms &\nconditions", "", "icons_mici/settings/device/info.png")
|
||||
terms_btn = BigButton("terms &\nconditions", "", gui_app.texture("icons_mici/settings/device/info.png", 64, 64))
|
||||
terms_btn.set_click_callback(lambda: gui_app.push_widget(ReviewTermsPage()))
|
||||
terms_btn.set_enabled(lambda: ui_state.is_offroad())
|
||||
|
||||
self._scroller.add_widgets([
|
||||
DeviceInfoLayoutMici(),
|
||||
|
||||
@@ -81,12 +81,12 @@ class FirehoseLayoutBase(Widget):
|
||||
def _render(self, rect: rl.Rectangle):
|
||||
# compute total content height for scrolling
|
||||
content_height = self._measure_content_height(rect)
|
||||
scroll_offset = round(self._scroll_panel.update(rect, content_height))
|
||||
scroll_offset = self._scroll_panel.update(rect, content_height)
|
||||
|
||||
# start drawing with offset
|
||||
x = int(rect.x + 40)
|
||||
y = int(rect.y + 40 + scroll_offset)
|
||||
w = int(rect.width - 80)
|
||||
x = rect.x + 40
|
||||
y = rect.y + 40 + scroll_offset
|
||||
w = rect.width - 80
|
||||
|
||||
# Title
|
||||
title_text = tr(TITLE)
|
||||
@@ -100,7 +100,7 @@ class FirehoseLayoutBase(Widget):
|
||||
y += 20
|
||||
|
||||
# Separator
|
||||
rl.draw_rectangle(x, y, w, 2, self.GRAY)
|
||||
rl.draw_rectangle_rec(rl.Rectangle(x, y, w, 2), self.GRAY)
|
||||
y += 20
|
||||
|
||||
# Status
|
||||
@@ -116,7 +116,7 @@ class FirehoseLayoutBase(Widget):
|
||||
y += 20
|
||||
|
||||
# Separator
|
||||
rl.draw_rectangle(x, y, w, 2, self.GRAY)
|
||||
rl.draw_rectangle_rec(rl.Rectangle(x, y, w, 2), self.GRAY)
|
||||
y += 20
|
||||
|
||||
# Instructions intro
|
||||
|
||||
@@ -3,9 +3,8 @@ import numpy as np
|
||||
import pyray as rl
|
||||
from collections.abc import Callable
|
||||
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.ui.mici.widgets.dialog import BigInputDialog, BigConfirmationDialogV2
|
||||
from openpilot.selfdrive.ui.mici.widgets.dialog import BigInputDialog, BigConfirmationDialog
|
||||
from openpilot.selfdrive.ui.mici.widgets.button import BigButton, LABEL_COLOR
|
||||
from openpilot.system.ui.lib.application import gui_app, MousePos, FontWeight
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
@@ -14,39 +13,26 @@ from openpilot.system.ui.lib.wifi_manager import WifiManager, Network, SecurityT
|
||||
|
||||
|
||||
class LoadingAnimation(Widget):
|
||||
HIDE_TIME = 4
|
||||
RADIUS = 8
|
||||
SPACING = 24 # center-to-center: diameter (16) + gap (8)
|
||||
Y_MAG = 11.2
|
||||
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self._opacity_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
|
||||
self._opacity_target = 1.0
|
||||
self._hide_time = 0.0
|
||||
|
||||
def show_event(self):
|
||||
self._opacity_target = 1.0
|
||||
self._hide_time = rl.get_time()
|
||||
w = self.SPACING * 2 + self.RADIUS * 2
|
||||
h = self.RADIUS * 2 + int(self.Y_MAG)
|
||||
self.set_rect(rl.Rectangle(0, 0, w, h))
|
||||
|
||||
def _render(self, _):
|
||||
if rl.get_time() - self._hide_time > self.HIDE_TIME:
|
||||
self._opacity_target = 0.0
|
||||
|
||||
self._opacity_filter.update(self._opacity_target)
|
||||
|
||||
if self._opacity_filter.x < 0.01:
|
||||
return
|
||||
|
||||
cx = int(self._rect.x + self._rect.width / 2)
|
||||
cy = int(self._rect.y + self._rect.height / 2)
|
||||
|
||||
y_mag = 7
|
||||
anim_scale = 4
|
||||
spacing = 14
|
||||
# Balls rest at bottom center; bounce upward
|
||||
base_x = int(self._rect.x + self._rect.width / 2)
|
||||
base_y = int(self._rect.y + self._rect.height - self.RADIUS)
|
||||
|
||||
for i in range(3):
|
||||
x = cx - spacing + i * spacing
|
||||
y = int(cy + min(math.sin((rl.get_time() - i * 0.2) * anim_scale) * y_mag, 0))
|
||||
alpha = int(np.interp(cy - y, [0, y_mag], [255 * 0.45, 255 * 0.9]) * self._opacity_filter.x)
|
||||
rl.draw_circle(x, y, 5, rl.Color(255, 255, 255, alpha))
|
||||
x = base_x + (i - 1) * self.SPACING
|
||||
y = int(base_y + min(math.sin((rl.get_time() - i * 0.2) * 4) * self.Y_MAG, 0))
|
||||
alpha = int(np.interp(base_y - y, [0, self.Y_MAG], [255 * 0.45, 255 * 0.9]))
|
||||
rl.draw_circle(x, y, self.RADIUS, rl.Color(255, 255, 255, alpha))
|
||||
|
||||
|
||||
class WifiIcon(Widget):
|
||||
@@ -124,6 +110,10 @@ class WifiButton(BigButton):
|
||||
if self._is_connected or self._is_connecting:
|
||||
self._wrong_password = False
|
||||
|
||||
@property
|
||||
def network_forgetting(self) -> bool:
|
||||
return self._network_forgetting
|
||||
|
||||
def _forget_network(self):
|
||||
if self._network_forgetting:
|
||||
return
|
||||
@@ -175,7 +165,7 @@ class WifiButton(BigButton):
|
||||
|
||||
if self._is_connected and not self._network_forgetting:
|
||||
check_y = int(label_y - sub_label_height + (sub_label_height - self._check_txt.height) / 2)
|
||||
rl.draw_texture(self._check_txt, int(sub_label_x), check_y, rl.Color(255, 255, 255, int(255 * 0.9 * 0.65)))
|
||||
rl.draw_texture_ex(self._check_txt, rl.Vector2(sub_label_x, check_y), 0.0, 1.0, rl.Color(255, 255, 255, int(255 * 0.9 * 0.65)))
|
||||
sub_label_x += self._check_txt.width + 14
|
||||
|
||||
sub_label_rect = rl.Rectangle(sub_label_x, label_y - sub_label_height, sub_label_w, sub_label_height)
|
||||
@@ -256,8 +246,7 @@ class ForgetButton(Widget):
|
||||
|
||||
def _handle_mouse_release(self, mouse_pos: MousePos):
|
||||
super()._handle_mouse_release(mouse_pos)
|
||||
dlg = BigConfirmationDialogV2("slide to forget", "icons_mici/settings/network/new/trash.png", red=True,
|
||||
confirm_callback=self._forget_network)
|
||||
dlg = BigConfirmationDialog("slide to forget", gui_app.texture("icons_mici/settings/network/new/trash.png", 54, 64), self._forget_network, red=True)
|
||||
gui_app.push_widget(dlg)
|
||||
|
||||
def _render(self, _):
|
||||
@@ -270,11 +259,26 @@ class ForgetButton(Widget):
|
||||
rl.draw_texture_ex(self._trash_txt, (trash_x, trash_y), 0, 1.0, rl.WHITE)
|
||||
|
||||
|
||||
class ScanningButton(BigButton):
|
||||
def __init__(self):
|
||||
super().__init__("", "searching for networks")
|
||||
self.set_enabled(False)
|
||||
self._loading_animation = LoadingAnimation()
|
||||
|
||||
def _draw_content(self, btn_y: float):
|
||||
super()._draw_content(btn_y)
|
||||
anim = self._loading_animation
|
||||
x = self._rect.x + self._rect.width - anim.rect.width - 40
|
||||
y = btn_y + self._rect.height - anim.rect.height - 30
|
||||
anim.set_position(x, y)
|
||||
anim.render()
|
||||
|
||||
|
||||
class WifiUIMici(NavScroller):
|
||||
def __init__(self, wifi_manager: WifiManager):
|
||||
super().__init__()
|
||||
|
||||
self._loading_animation = LoadingAnimation()
|
||||
self._scanning_btn = ScanningButton()
|
||||
|
||||
self._wifi_manager = wifi_manager
|
||||
self._networks: dict[str, Network] = {}
|
||||
@@ -285,20 +289,23 @@ class WifiUIMici(NavScroller):
|
||||
networks_updated=self._on_network_updated,
|
||||
)
|
||||
|
||||
@property
|
||||
def any_network_forgetting(self) -> bool:
|
||||
# TODO: deactivate before forget and add DISCONNECTING state
|
||||
return any(btn.network_forgetting for btn in self._scroller.items if isinstance(btn, WifiButton))
|
||||
|
||||
def show_event(self):
|
||||
# Clear scroller items and update from latest scan results
|
||||
# Re-sort scroller items and update from latest scan results
|
||||
super().show_event()
|
||||
self._loading_animation.show_event()
|
||||
self._wifi_manager.set_active(True)
|
||||
self._scroller.items.clear()
|
||||
# trigger button update on latest sorted networks
|
||||
self._on_network_updated(self._wifi_manager.networks)
|
||||
self._networks = {n.ssid: n for n in self._wifi_manager.networks}
|
||||
self._update_buttons(re_sort=True)
|
||||
|
||||
def _on_network_updated(self, networks: list[Network]):
|
||||
self._networks = {network.ssid: network for network in networks}
|
||||
self._update_buttons()
|
||||
|
||||
def _update_buttons(self):
|
||||
def _update_buttons(self, re_sort: bool = False):
|
||||
# Update existing buttons, add new ones to the end
|
||||
existing = {btn.network.ssid: btn for btn in self._scroller.items if isinstance(btn, WifiButton)}
|
||||
|
||||
@@ -310,10 +317,22 @@ class WifiUIMici(NavScroller):
|
||||
btn.set_click_callback(lambda ssid=network.ssid: self._connect_to_network(ssid))
|
||||
self._scroller.add_widget(btn)
|
||||
|
||||
# Mark networks no longer in scan results (display handled by _update_state)
|
||||
for btn in self._scroller.items:
|
||||
if isinstance(btn, WifiButton) and btn.network.ssid not in self._networks:
|
||||
btn.set_network_missing(True)
|
||||
if re_sort:
|
||||
# Remove stale buttons and sort to match scan order, preserving eager state
|
||||
btn_map = {btn.network.ssid: btn for btn in self._scroller.items if isinstance(btn, WifiButton)}
|
||||
self._scroller.items[:] = [btn_map[ssid] for ssid in self._networks if ssid in btn_map]
|
||||
else:
|
||||
# Mark networks no longer in scan results (display handled by _update_state)
|
||||
for btn in self._scroller.items:
|
||||
if isinstance(btn, WifiButton) and btn.network.ssid not in self._networks:
|
||||
btn.set_network_missing(True)
|
||||
|
||||
# Keep scanning button at the end
|
||||
items = self._scroller.items
|
||||
if self._scanning_btn in items:
|
||||
items.append(items.pop(items.index(self._scanning_btn)))
|
||||
else:
|
||||
self._scroller.add_widget(self._scanning_btn)
|
||||
|
||||
def _connect_with_password(self, ssid: str, password: str):
|
||||
self._wifi_manager.connect_to_network(ssid, password)
|
||||
@@ -370,17 +389,3 @@ class WifiUIMici(NavScroller):
|
||||
super()._update_state()
|
||||
|
||||
self._move_network_to_front(self._wifi_manager.wifi_state.ssid)
|
||||
|
||||
# Show loading animation near end
|
||||
max_scroll = max(self._scroller.content_size - self._scroller.rect.width, 1)
|
||||
progress = -self._scroller.scroll_panel.get_offset() / max_scroll
|
||||
if progress > 0.8 or len(self._scroller.items) <= 1:
|
||||
self._loading_animation.show_event()
|
||||
|
||||
def _render(self, _):
|
||||
super()._render(self._rect)
|
||||
|
||||
anim_w = 90
|
||||
anim_x = self._rect.x + self._rect.width - anim_w
|
||||
anim_y = self._rect.y + self._rect.height - 25 + 2
|
||||
self._loading_animation.render(rl.Rectangle(anim_x, anim_y, anim_w, 20))
|
||||
|
||||
@@ -20,23 +20,23 @@ class SettingsLayout(NavScroller):
|
||||
self._params = Params()
|
||||
|
||||
toggles_panel = TogglesLayoutMici()
|
||||
toggles_btn = SettingsBigButton("toggles", "", "icons_mici/settings.png")
|
||||
toggles_btn = SettingsBigButton("toggles", "", gui_app.texture("icons_mici/settings.png", 64, 64))
|
||||
toggles_btn.set_click_callback(lambda: gui_app.push_widget(toggles_panel))
|
||||
|
||||
network_panel = NetworkLayoutMici()
|
||||
network_btn = SettingsBigButton("network", "", "icons_mici/settings/network/wifi_strength_full.png", icon_size=(76, 56))
|
||||
network_btn = SettingsBigButton("network", "", gui_app.texture("icons_mici/settings/network/wifi_strength_full.png", 76, 56))
|
||||
network_btn.set_click_callback(lambda: gui_app.push_widget(network_panel))
|
||||
|
||||
device_panel = DeviceLayoutMici()
|
||||
device_btn = SettingsBigButton("device", "", "icons_mici/settings/device_icon.png", icon_size=(74, 60))
|
||||
device_btn = SettingsBigButton("device", "", gui_app.texture("icons_mici/settings/device_icon.png", 72, 58))
|
||||
device_btn.set_click_callback(lambda: gui_app.push_widget(device_panel))
|
||||
|
||||
developer_panel = DeveloperLayoutMici()
|
||||
developer_btn = SettingsBigButton("developer", "", "icons_mici/settings/developer_icon.png", icon_size=(64, 60))
|
||||
developer_btn = SettingsBigButton("developer", "", gui_app.texture("icons_mici/settings/developer_icon.png", 64, 60))
|
||||
developer_btn.set_click_callback(lambda: gui_app.push_widget(developer_panel))
|
||||
|
||||
firehose_panel = FirehoseLayout()
|
||||
firehose_btn = SettingsBigButton("firehose", "", "icons_mici/settings/firehose.png", icon_size=(52, 62))
|
||||
firehose_btn = SettingsBigButton("firehose", "", gui_app.texture("icons_mici/settings/firehose.png", 52, 62))
|
||||
firehose_btn.set_click_callback(lambda: gui_app.push_widget(firehose_panel))
|
||||
|
||||
self._scroller.add_widgets([
|
||||
|
||||
@@ -129,7 +129,7 @@ class BookmarkIcon(Widget):
|
||||
if self._offset_filter.x > 0:
|
||||
icon_x = self.rect.x + self.rect.width - round(self._offset_filter.x)
|
||||
icon_y = self.rect.y + (self.rect.height - self._icon.height) / 2 # Vertically centered
|
||||
rl.draw_texture(self._icon, int(icon_x), int(icon_y), rl.WHITE)
|
||||
rl.draw_texture_ex(self._icon, rl.Vector2(icon_x, icon_y), 0.0, 1.0, rl.WHITE)
|
||||
|
||||
|
||||
class AugmentedRoadView(CameraView):
|
||||
|
||||
@@ -61,7 +61,7 @@ class DriverStateRenderer(Widget):
|
||||
self._dm_cone = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_cone.png", cone_and_person_size, cone_and_person_size)
|
||||
center_size = round(36 / self.BASE_SIZE * self._rect.width)
|
||||
self._dm_center = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_center.png", center_size, center_size)
|
||||
self._dm_background = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_background.png", self._rect.width, self._rect.height)
|
||||
self._dm_background = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_background.png", int(self._rect.width), int(self._rect.height))
|
||||
|
||||
def set_should_draw(self, should_draw: bool):
|
||||
self._should_draw = should_draw
|
||||
@@ -88,15 +88,14 @@ class DriverStateRenderer(Widget):
|
||||
if DEBUG:
|
||||
rl.draw_rectangle_lines_ex(self._rect, 1, rl.RED)
|
||||
|
||||
rl.draw_texture(self._dm_background,
|
||||
int(self._rect.x),
|
||||
int(self._rect.y),
|
||||
rl.Color(255, 255, 255, int(255 * self._fade_filter.x)))
|
||||
rl.draw_texture_ex(self._dm_background,
|
||||
rl.Vector2(self._rect.x, self._rect.y), 0.0, 1.0,
|
||||
rl.Color(255, 255, 255, int(255 * self._fade_filter.x)))
|
||||
|
||||
rl.draw_texture(self._dm_person,
|
||||
int(self._rect.x + (self._rect.width - self._dm_person.width) / 2),
|
||||
int(self._rect.y + (self._rect.height - self._dm_person.height) / 2),
|
||||
rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x)))
|
||||
rl.draw_texture_ex(self._dm_person,
|
||||
rl.Vector2(self._rect.x + (self._rect.width - self._dm_person.width) / 2,
|
||||
self._rect.y + (self._rect.height - self._dm_person.height) / 2), 0.0, 1.0,
|
||||
rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x)))
|
||||
|
||||
if self.effective_active:
|
||||
source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height)
|
||||
|
||||
@@ -316,7 +316,7 @@ class HudRenderer(Widget):
|
||||
EXCLAMATION_POINT_SPACING = 10
|
||||
exclamation_pos_x = pos_x - self._txt_exclamation_point.width / 2 + wheel_txt.width / 2 + EXCLAMATION_POINT_SPACING
|
||||
exclamation_pos_y = pos_y - self._txt_exclamation_point.height / 2
|
||||
rl.draw_texture(self._txt_exclamation_point, int(exclamation_pos_x), int(exclamation_pos_y), rl.WHITE)
|
||||
rl.draw_texture_ex(self._txt_exclamation_point, rl.Vector2(exclamation_pos_x, exclamation_pos_y), 0.0, 1.0, rl.WHITE)
|
||||
|
||||
|
||||
def _get_cpu_temp_text(self) -> str:
|
||||
|
||||
@@ -28,7 +28,7 @@ class ScrollState(Enum):
|
||||
|
||||
|
||||
class BigCircleButton(Widget):
|
||||
def __init__(self, icon: str, red: bool = False, icon_size: tuple[int, int] = (64, 53), icon_offset: tuple[int, int] = (0, 0)):
|
||||
def __init__(self, icon: rl.Texture, red: bool = False, icon_offset: tuple[int, int] = (0, 0)):
|
||||
super().__init__()
|
||||
self._red = red
|
||||
self._icon_offset = icon_offset
|
||||
@@ -39,7 +39,7 @@ class BigCircleButton(Widget):
|
||||
self._click_delay = 0.075
|
||||
|
||||
# Icons
|
||||
self._txt_icon = gui_app.texture(icon, *icon_size)
|
||||
self._txt_icon = icon
|
||||
self._txt_btn_disabled_bg = gui_app.texture("icons_mici/buttons/button_circle_disabled.png", 180, 180)
|
||||
|
||||
self._txt_btn_bg = gui_app.texture("icons_mici/buttons/button_circle.png", 180, 180)
|
||||
@@ -71,8 +71,8 @@ class BigCircleButton(Widget):
|
||||
|
||||
|
||||
class BigCircleToggle(BigCircleButton):
|
||||
def __init__(self, icon: str, toggle_callback: Callable | None = None, icon_size: tuple[int, int] = (64, 53), icon_offset: tuple[int, int] = (0, 0)):
|
||||
super().__init__(icon, False, icon_size=icon_size, icon_offset=icon_offset)
|
||||
def __init__(self, icon: rl.Texture, toggle_callback: Callable | None = None, icon_offset: tuple[int, int] = (0, 0)):
|
||||
super().__init__(icon, False, icon_offset=icon_offset)
|
||||
self._toggle_callback = toggle_callback
|
||||
|
||||
# State
|
||||
@@ -107,15 +107,13 @@ class BigButton(Widget):
|
||||
|
||||
"""A lightweight stand-in for the Qt BigButton, drawn & updated each frame."""
|
||||
|
||||
def __init__(self, text: str, value: str = "", icon: Union[str, rl.Texture] = "", icon_size: tuple[int, int] = (64, 64),
|
||||
scroll: bool = False):
|
||||
def __init__(self, text: str, value: str = "", icon: Union[rl.Texture, None] = None, scroll: bool = False):
|
||||
super().__init__()
|
||||
self.set_rect(rl.Rectangle(0, 0, 402, 180))
|
||||
self.text = text
|
||||
self.value = value
|
||||
self._icon_size = icon_size
|
||||
self._txt_icon = icon
|
||||
self._scroll = scroll
|
||||
self.set_icon(icon)
|
||||
|
||||
self._scale_filter = BounceFilter(1.0, 0.1, 1 / gui_app.target_fps)
|
||||
self._click_delay = 0.075
|
||||
@@ -133,8 +131,8 @@ class BigButton(Widget):
|
||||
|
||||
self._load_images()
|
||||
|
||||
def set_icon(self, icon: Union[str, rl.Texture]):
|
||||
self._txt_icon = gui_app.texture(icon, *self._icon_size) if isinstance(icon, str) and len(icon) else icon
|
||||
def set_icon(self, icon: Union[rl.Texture, None]):
|
||||
self._txt_icon = icon
|
||||
|
||||
def set_rotate_icon(self, rotate: bool):
|
||||
if rotate and self._rotate_icon_t is not None:
|
||||
@@ -151,7 +149,7 @@ class BigButton(Widget):
|
||||
|
||||
def _width_hint(self) -> int:
|
||||
# Single line if scrolling, so hide behind icon if exists
|
||||
icon_size = self._icon_size[0] if self._txt_icon and self._scroll and self.value else 0
|
||||
icon_size = self._txt_icon.width if self._txt_icon and self._scroll and self.value else 0
|
||||
return int(self._rect.width - self.LABEL_HORIZONTAL_PADDING * 2 - icon_size)
|
||||
|
||||
def _get_label_font_size(self):
|
||||
@@ -194,7 +192,9 @@ class BigButton(Widget):
|
||||
SHAKE_DURATION = 0.5
|
||||
SHAKE_AMPLITUDE = 24.0
|
||||
SHAKE_FREQUENCY = 32.0
|
||||
t = rl.get_time() - (self._shake_start or 0.0)
|
||||
if self._shake_start is None:
|
||||
return 0.0
|
||||
t = rl.get_time() - self._shake_start
|
||||
if t > SHAKE_DURATION:
|
||||
return 0.0
|
||||
decay = 1.0 - t / SHAKE_DURATION
|
||||
@@ -335,6 +335,43 @@ class BigMultiToggle(BigToggle):
|
||||
y += 35
|
||||
|
||||
|
||||
class GreyBigButton(BigButton):
|
||||
"""Users should manage newlines with this class themselves"""
|
||||
|
||||
LABEL_HORIZONTAL_PADDING = 30
|
||||
|
||||
def __init__(self, *args, **kwargs):
|
||||
super().__init__(*args, **kwargs)
|
||||
self.set_touch_valid_callback(lambda: False)
|
||||
|
||||
self._rect.width = 476
|
||||
|
||||
self._label.set_font_size(36)
|
||||
self._label.set_font_weight(FontWeight.BOLD)
|
||||
self._label.set_line_height(1.0)
|
||||
|
||||
self._sub_label.set_font_size(36)
|
||||
self._sub_label.set_text_color(rl.Color(255, 255, 255, int(255 * 0.9)))
|
||||
self._sub_label.set_font_weight(FontWeight.DISPLAY_REGULAR)
|
||||
self._sub_label.set_alignment_vertical(rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE if not self._label.text else
|
||||
rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM)
|
||||
self._sub_label.set_line_height(0.95)
|
||||
|
||||
@property
|
||||
def LABEL_VERTICAL_PADDING(self):
|
||||
return BigButton.LABEL_VERTICAL_PADDING if self._label.text else 18
|
||||
|
||||
def _width_hint(self) -> int:
|
||||
return int(self._rect.width - self.LABEL_HORIZONTAL_PADDING * 2)
|
||||
|
||||
def _get_label_font_size(self):
|
||||
return 36
|
||||
|
||||
def _render(self, _):
|
||||
rl.draw_rectangle_rounded(self._rect, 0.4, 10, rl.Color(255, 255, 255, int(255 * 0.15)))
|
||||
self._draw_content(self._rect.y)
|
||||
|
||||
|
||||
class BigMultiParamToggle(BigMultiToggle):
|
||||
def __init__(self, text: str, param: str, options: list[str], toggle_callback: Callable | None = None,
|
||||
select_callback: Callable | None = None):
|
||||
@@ -370,9 +407,9 @@ class BigParamControl(BigToggle):
|
||||
|
||||
# TODO: param control base class
|
||||
class BigCircleParamControl(BigCircleToggle):
|
||||
def __init__(self, icon: str, param: str, toggle_callback: Callable | None = None, icon_size: tuple[int, int] = (64, 53),
|
||||
def __init__(self, icon: rl.Texture, param: str, toggle_callback: Callable | None = None,
|
||||
icon_offset: tuple[int, int] = (0, 0)):
|
||||
super().__init__(icon, toggle_callback, icon_size=icon_size, icon_offset=icon_offset)
|
||||
super().__init__(icon, toggle_callback, icon_offset=icon_offset)
|
||||
self._param = param
|
||||
self.params = Params()
|
||||
self.set_checked(self.params.get_bool(self._param, False))
|
||||
|
||||
@@ -4,14 +4,13 @@ import pyray as rl
|
||||
from typing import Union
|
||||
from collections.abc import Callable
|
||||
from openpilot.system.ui.widgets.nav_widget import NavWidget
|
||||
from openpilot.system.ui.widgets.label import UnifiedLabel, gui_label
|
||||
from openpilot.system.ui.widgets.label import UnifiedLabel
|
||||
from openpilot.system.ui.widgets.mici_keyboard import MiciKeyboard
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.system.ui.lib.wrap_text import wrap_text
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
|
||||
from openpilot.system.ui.widgets.slider import RedBigSlider, BigSlider
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.selfdrive.ui.mici.widgets.button import BigButton
|
||||
from openpilot.selfdrive.ui.mici.widgets.button import BigCircleButton, BigButton, GreyBigButton
|
||||
|
||||
DEBUG = False
|
||||
|
||||
@@ -25,58 +24,31 @@ class BigDialogBase(NavWidget, abc.ABC):
|
||||
|
||||
|
||||
class BigDialog(BigDialogBase):
|
||||
def __init__(self,
|
||||
title: str,
|
||||
description: str):
|
||||
def __init__(self, title: str, description: str, icon: Union[rl.Texture, None] = None):
|
||||
super().__init__()
|
||||
self._title = title
|
||||
self._description = description
|
||||
self._card = GreyBigButton(title, description, icon)
|
||||
|
||||
def _render(self, _):
|
||||
super()._render(_)
|
||||
|
||||
# draw title
|
||||
# TODO: we desperately need layouts
|
||||
# TODO: coming up with these numbers manually is a pain and not scalable
|
||||
# TODO: no clue what any of these numbers mean. VBox and HBox would remove all of this shite
|
||||
max_width = self._rect.width - PADDING * 2
|
||||
|
||||
title_wrapped = '\n'.join(wrap_text(gui_app.font(FontWeight.BOLD), self._title, 50, int(max_width)))
|
||||
title_size = measure_text_cached(gui_app.font(FontWeight.BOLD), title_wrapped, 50)
|
||||
text_x_offset = 0
|
||||
title_rect = rl.Rectangle(int(self._rect.x + text_x_offset + PADDING),
|
||||
int(self._rect.y + PADDING),
|
||||
int(max_width),
|
||||
int(title_size.y))
|
||||
gui_label(title_rect, title_wrapped, 50, font_weight=FontWeight.BOLD,
|
||||
alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
|
||||
|
||||
# draw description
|
||||
desc_wrapped = '\n'.join(wrap_text(gui_app.font(FontWeight.MEDIUM), self._description, 30, int(max_width)))
|
||||
desc_size = measure_text_cached(gui_app.font(FontWeight.MEDIUM), desc_wrapped, 30)
|
||||
desc_rect = rl.Rectangle(int(self._rect.x + text_x_offset + PADDING),
|
||||
int(self._rect.y + self._rect.height / 3),
|
||||
int(max_width),
|
||||
int(desc_size.y))
|
||||
# TODO: text align doesn't seem to work properly with newlines
|
||||
gui_label(desc_rect, desc_wrapped, 30, font_weight=FontWeight.MEDIUM,
|
||||
alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
|
||||
self._card.render(rl.Rectangle(
|
||||
self._rect.x + self._rect.width / 2 - self._card.rect.width / 2,
|
||||
self._rect.y + self._rect.height / 2 - self._card.rect.height / 2,
|
||||
self._card.rect.width,
|
||||
self._card.rect.height,
|
||||
))
|
||||
|
||||
|
||||
class BigConfirmationDialogV2(BigDialogBase):
|
||||
def __init__(self, title: str, icon: str, red: bool = False,
|
||||
exit_on_confirm: bool = True,
|
||||
confirm_callback: Callable | None = None):
|
||||
class BigConfirmationDialog(BigDialogBase):
|
||||
def __init__(self, title: str, icon: rl.Texture, confirm_callback: Callable[[], None],
|
||||
exit_on_confirm: bool = True, red: bool = False):
|
||||
super().__init__()
|
||||
self._confirm_callback = confirm_callback
|
||||
self._exit_on_confirm = exit_on_confirm
|
||||
|
||||
icon_txt = gui_app.texture(icon, 64, 53)
|
||||
self._slider: BigSlider | RedBigSlider
|
||||
if red:
|
||||
self._slider = RedBigSlider(title, icon_txt, confirm_callback=self._on_confirm)
|
||||
self._slider = self._child(RedBigSlider(title, icon, confirm_callback=self._on_confirm))
|
||||
else:
|
||||
self._slider = BigSlider(title, icon_txt, confirm_callback=self._on_confirm)
|
||||
self._slider = self._child(BigSlider(title, icon, confirm_callback=self._on_confirm))
|
||||
self._slider.set_enabled(lambda: self.enabled and not self.is_dismissing) # for nav stack + NavWidget
|
||||
|
||||
def _on_confirm(self):
|
||||
@@ -158,9 +130,9 @@ class BigInputDialog(BigDialogBase):
|
||||
|
||||
bg_block_margin = 5
|
||||
text_x = PADDING / 2 + self._enter_img.width + PADDING
|
||||
text_field_rect = rl.Rectangle(text_x, int(self._rect.y + PADDING) - bg_block_margin,
|
||||
int(self._rect.width - text_x * 2),
|
||||
int(text_size.y))
|
||||
text_field_rect = rl.Rectangle(text_x, self._rect.y + PADDING - bg_block_margin,
|
||||
self._rect.width - text_x * 2,
|
||||
text_size.y)
|
||||
|
||||
# draw text input
|
||||
# push text left with a gradient on left side if too long
|
||||
@@ -181,8 +153,8 @@ class BigInputDialog(BigDialogBase):
|
||||
|
||||
# draw gradient on left side to indicate more text
|
||||
if text_size.x > text_field_rect.width:
|
||||
rl.draw_rectangle_gradient_h(int(text_field_rect.x), int(text_field_rect.y), 80, int(text_field_rect.height),
|
||||
rl.BLACK, rl.BLANK)
|
||||
rl.draw_rectangle_gradient_ex(rl.Rectangle(text_field_rect.x, text_field_rect.y, 80, text_field_rect.height),
|
||||
rl.BLACK, rl.BLANK, rl.BLANK, rl.BLACK)
|
||||
|
||||
# draw cursor
|
||||
blink_alpha = (math.sin(rl.get_time() * 6) + 1) / 2
|
||||
@@ -190,14 +162,14 @@ class BigInputDialog(BigDialogBase):
|
||||
cursor_x = min(text_x + text_size.x + 3, text_field_rect.x + text_field_rect.width)
|
||||
else:
|
||||
cursor_x = text_field_rect.x - 6
|
||||
rl.draw_rectangle_rounded(rl.Rectangle(int(cursor_x), int(text_field_rect.y), 4, int(text_size.y)),
|
||||
rl.draw_rectangle_rounded(rl.Rectangle(cursor_x, text_field_rect.y, 4, text_size.y),
|
||||
1, 4, rl.Color(255, 255, 255, int(255 * blink_alpha)))
|
||||
|
||||
# draw backspace icon with nice fade
|
||||
self._backspace_img_alpha.update(255 * bool(text))
|
||||
if self._backspace_img_alpha.x > 1:
|
||||
color = rl.Color(255, 255, 255, int(self._backspace_img_alpha.x))
|
||||
rl.draw_texture(self._backspace_img, int(self._rect.width - self._backspace_img.width - 27), int(self._rect.y + 14), color)
|
||||
rl.draw_texture_ex(self._backspace_img, rl.Vector2(self._rect.width - self._backspace_img.width - 27, self._rect.y + 14), 0.0, 1.0, color)
|
||||
|
||||
if not text and self._hint_label.text and not candidate_char:
|
||||
# draw description if no text entered yet and not drawing candidate char
|
||||
@@ -215,9 +187,9 @@ class BigInputDialog(BigDialogBase):
|
||||
# draw enter button
|
||||
self._enter_img_alpha.update(255 if len(text) >= self._minimum_length else 0)
|
||||
color = rl.Color(255, 255, 255, int(self._enter_img_alpha.x))
|
||||
rl.draw_texture(self._enter_img, int(self._rect.x + PADDING / 2), int(self._rect.y), color)
|
||||
rl.draw_texture_ex(self._enter_img, rl.Vector2(self._rect.x + PADDING / 2, self._rect.y), 0.0, 1.0, color)
|
||||
color = rl.Color(255, 255, 255, 255 - int(self._enter_img_alpha.x))
|
||||
rl.draw_texture(self._enter_disabled_img, int(self._rect.x + PADDING / 2), int(self._rect.y), color)
|
||||
rl.draw_texture_ex(self._enter_disabled_img, rl.Vector2(self._rect.x + PADDING / 2, self._rect.y), 0.0, 1.0, color)
|
||||
|
||||
# keyboard goes over everything
|
||||
self._keyboard.render(self._rect)
|
||||
@@ -254,3 +226,15 @@ class BigDialogButton(BigButton):
|
||||
|
||||
dlg = BigDialog(self.text, self._description)
|
||||
gui_app.push_widget(dlg)
|
||||
|
||||
|
||||
class BigConfirmationCircleButton(BigCircleButton):
|
||||
def __init__(self, title: str, icon: rl.Texture, confirm_callback: Callable[[], None], exit_on_confirm: bool = True,
|
||||
red: bool = False, icon_offset: tuple[int, int] = (0, 0)):
|
||||
super().__init__(icon, red, icon_offset)
|
||||
|
||||
def show_confirm_dialog():
|
||||
gui_app.push_widget(BigConfirmationDialog(title, icon, confirm_callback,
|
||||
exit_on_confirm=exit_on_confirm, red=red))
|
||||
|
||||
self.set_click_callback(show_confirm_dialog)
|
||||
|
||||
@@ -9,7 +9,7 @@ from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.widgets.nav_widget import NavWidget
|
||||
from openpilot.system.ui.lib.application import FontWeight, gui_app
|
||||
from openpilot.system.ui.widgets.label import MiciLabel
|
||||
from openpilot.system.ui.widgets.label import UnifiedLabel
|
||||
|
||||
|
||||
class PairingDialog(NavWidget):
|
||||
@@ -24,8 +24,7 @@ class PairingDialog(NavWidget):
|
||||
self._last_qr_generation = float("-inf")
|
||||
|
||||
self._txt_pair = gui_app.texture("icons_mici/settings/device/pair.png", 33, 60)
|
||||
self._pair_label = MiciLabel("pair with comma connect", 48, font_weight=FontWeight.BOLD,
|
||||
color=rl.Color(255, 255, 255, int(255 * 0.9)), line_height=40, wrap_text=True)
|
||||
self._pair_label = UnifiedLabel("pair with comma connect", font_size=48, font_weight=FontWeight.BOLD, line_height=0.8)
|
||||
|
||||
def _get_pairing_url(self) -> str:
|
||||
try:
|
||||
@@ -77,7 +76,7 @@ class PairingDialog(NavWidget):
|
||||
self._render_qr_code()
|
||||
|
||||
label_x = self._rect.x + 8 + self._rect.height + 24
|
||||
self._pair_label.set_width(int(self._rect.width - label_x))
|
||||
self._pair_label.set_max_width(int(self._rect.width - label_x))
|
||||
self._pair_label.set_position(label_x, self._rect.y + 16)
|
||||
self._pair_label.render()
|
||||
|
||||
@@ -93,7 +92,7 @@ class PairingDialog(NavWidget):
|
||||
return
|
||||
|
||||
scale = self._rect.height / self._qr_texture.height
|
||||
pos = rl.Vector2(self._rect.x + 8, self._rect.y)
|
||||
pos = rl.Vector2(round(self._rect.x + 8), round(self._rect.y))
|
||||
rl.draw_texture_ex(self._qr_texture, pos, 0.0, scale, rl.WHITE)
|
||||
|
||||
def __del__(self):
|
||||
|
||||
@@ -50,7 +50,7 @@ class ExpButton(Widget):
|
||||
|
||||
texture = self._txt_exp if self._held_or_actual_mode() else self._txt_wheel
|
||||
rl.draw_circle(center_x, center_y, self._rect.width / 2, self._black_bg)
|
||||
rl.draw_texture(texture, center_x - texture.width // 2, center_y - texture.height // 2, self._white_color)
|
||||
rl.draw_texture_ex(texture, rl.Vector2(center_x - texture.width / 2, center_y - texture.height / 2), 0.0, 1.0, self._white_color)
|
||||
|
||||
def _held_or_actual_mode(self):
|
||||
now = time.monotonic()
|
||||
|
||||
@@ -20,6 +20,7 @@ class ExperimentalModeButton(Widget):
|
||||
self.experimental_pixmap = gui_app.texture("icons/experimental_grey.png", self.img_width, self.img_width)
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode")
|
||||
|
||||
def _get_gradient_colors(self):
|
||||
|
||||
@@ -118,6 +118,7 @@ class AbstractAlert(Widget, ABC):
|
||||
self.scroll_panel = GuiScrollPanel()
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self.scroll_panel.set_offset(0)
|
||||
|
||||
def set_dismiss_callback(self, callback: Callable):
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
import pyray as rl
|
||||
import requests
|
||||
import threading
|
||||
import copy
|
||||
from collections.abc import Callable
|
||||
from enum import Enum
|
||||
|
||||
@@ -25,6 +24,51 @@ from openpilot.system.ui.widgets.list_view import (
|
||||
VALUE_FONT_SIZE = 48
|
||||
|
||||
|
||||
class SshKeyFetcher:
|
||||
HTTP_TIMEOUT = 15 # seconds
|
||||
|
||||
def __init__(self, params: Params):
|
||||
self._params = params
|
||||
self._on_response: Callable[[str | None], None] | None = None
|
||||
self._done: bool = False
|
||||
self._error: str | None = None
|
||||
|
||||
def fetch(self, username: str, on_response: Callable[[str | None], None]):
|
||||
self._error = None
|
||||
self._on_response = on_response
|
||||
threading.Thread(target=self._fetch_thread, args=(username,), daemon=True).start()
|
||||
|
||||
def update(self):
|
||||
if not self._done:
|
||||
return
|
||||
self._done = False
|
||||
if self._error is not None:
|
||||
self.clear()
|
||||
if self._on_response:
|
||||
self._on_response(self._error)
|
||||
|
||||
def clear(self):
|
||||
self._params.remove("GithubUsername")
|
||||
self._params.remove("GithubSshKeys")
|
||||
|
||||
def _fetch_thread(self, username: str):
|
||||
try:
|
||||
response = requests.get(f"https://github.com/{username}.keys", timeout=self.HTTP_TIMEOUT)
|
||||
response.raise_for_status()
|
||||
keys = response.text.strip()
|
||||
if not keys:
|
||||
raise requests.exceptions.HTTPError("No SSH keys found")
|
||||
|
||||
self._params.put("GithubUsername", username)
|
||||
self._params.put("GithubSshKeys", keys)
|
||||
except requests.exceptions.Timeout:
|
||||
self._error = tr("Request timed out")
|
||||
except Exception:
|
||||
self._error = tr("No SSH keys found for user '{}'").format(username)
|
||||
finally:
|
||||
self._done = True
|
||||
|
||||
|
||||
class SshKeyActionState(Enum):
|
||||
LOADING = tr_noop("LOADING")
|
||||
ADD = tr_noop("ADD")
|
||||
@@ -32,7 +76,6 @@ class SshKeyActionState(Enum):
|
||||
|
||||
|
||||
class SshKeyAction(ItemAction):
|
||||
HTTP_TIMEOUT = 15 # seconds
|
||||
MAX_WIDTH = 500
|
||||
|
||||
def __init__(self):
|
||||
@@ -40,7 +83,7 @@ class SshKeyAction(ItemAction):
|
||||
|
||||
self._keyboard = Keyboard(min_text_size=1)
|
||||
self._params = Params()
|
||||
self._error_message: str = ""
|
||||
self._fetcher = SshKeyFetcher(self._params)
|
||||
self._text_font = gui_app.font(FontWeight.NORMAL)
|
||||
self._button = Button("", click_callback=self._handle_button_click, button_style=ButtonStyle.LIST_ACTION,
|
||||
border_radius=BUTTON_BORDER_RADIUS, font_size=BUTTON_FONT_SIZE)
|
||||
@@ -55,14 +98,11 @@ class SshKeyAction(ItemAction):
|
||||
self._username = self._params.get("GithubUsername")
|
||||
self._state = SshKeyActionState.REMOVE if self._params.get("GithubSshKeys") else SshKeyActionState.ADD
|
||||
|
||||
def _render(self, rect: rl.Rectangle) -> bool:
|
||||
# Show error dialog if there's an error
|
||||
if self._error_message:
|
||||
message = copy.copy(self._error_message)
|
||||
gui_app.push_widget(alert_dialog(message))
|
||||
self._username = ""
|
||||
self._error_message = ""
|
||||
def _update_state(self):
|
||||
super()._update_state()
|
||||
self._fetcher.update()
|
||||
|
||||
def _render(self, rect: rl.Rectangle) -> bool:
|
||||
# Draw username if exists
|
||||
if self._username:
|
||||
text_size = measure_text_cached(self._text_font, self._username, VALUE_FONT_SIZE)
|
||||
@@ -90,8 +130,7 @@ class SshKeyAction(ItemAction):
|
||||
self._keyboard.set_callback(self._on_username_submit)
|
||||
gui_app.push_widget(self._keyboard)
|
||||
elif self._state == SshKeyActionState.REMOVE:
|
||||
self._params.remove("GithubUsername")
|
||||
self._params.remove("GithubSshKeys")
|
||||
self._fetcher.clear()
|
||||
self._refresh_state()
|
||||
|
||||
def _on_username_submit(self, result: DialogResult):
|
||||
@@ -103,29 +142,16 @@ class SshKeyAction(ItemAction):
|
||||
return
|
||||
|
||||
self._state = SshKeyActionState.LOADING
|
||||
threading.Thread(target=lambda: self._fetch_ssh_key(username), daemon=True).start()
|
||||
self._fetcher.fetch(username, self._on_fetch_response)
|
||||
|
||||
def _fetch_ssh_key(self, username: str):
|
||||
try:
|
||||
url = f"https://github.com/{username}.keys"
|
||||
response = requests.get(url, timeout=self.HTTP_TIMEOUT)
|
||||
response.raise_for_status()
|
||||
keys = response.text.strip()
|
||||
if not keys:
|
||||
raise requests.exceptions.HTTPError(tr("No SSH keys found"))
|
||||
|
||||
# Success - save keys
|
||||
self._params.put("GithubUsername", username)
|
||||
self._params.put("GithubSshKeys", keys)
|
||||
def _on_fetch_response(self, error: str | None):
|
||||
if error is None:
|
||||
self._state = SshKeyActionState.REMOVE
|
||||
self._username = username
|
||||
|
||||
except requests.exceptions.Timeout:
|
||||
self._error_message = tr("Request timed out")
|
||||
self._state = SshKeyActionState.ADD
|
||||
except Exception:
|
||||
self._error_message = tr("No SSH keys found for user '{}'").format(username)
|
||||
self._username = self._params.get("GithubUsername")
|
||||
else:
|
||||
self._state = SshKeyActionState.ADD
|
||||
self._username = ""
|
||||
gui_app.push_widget(alert_dialog(error))
|
||||
|
||||
|
||||
def ssh_key_item(title: str | Callable[[], str], description: str | Callable[[], str]) -> ListItem:
|
||||
|
||||
@@ -25,6 +25,7 @@ struct CameraConfig {
|
||||
uint32_t phy;
|
||||
bool vignetting_correction;
|
||||
SpectraOutputType output_type;
|
||||
bool staggered_sof; // SOF is staggered (half-period offset) from other cameras
|
||||
};
|
||||
|
||||
// NOTE: to be able to disable road and wide road, we still have to configure the sensor over i2c
|
||||
@@ -39,6 +40,7 @@ const CameraConfig WIDE_ROAD_CAMERA_CONFIG = {
|
||||
.phy = CAM_ISP_IFE_IN_RES_PHY_0,
|
||||
.vignetting_correction = false,
|
||||
.output_type = ISP_IFE_PROCESSED,
|
||||
.staggered_sof = false,
|
||||
};
|
||||
|
||||
const CameraConfig ROAD_CAMERA_CONFIG = {
|
||||
@@ -51,6 +53,7 @@ const CameraConfig ROAD_CAMERA_CONFIG = {
|
||||
.phy = CAM_ISP_IFE_IN_RES_PHY_1,
|
||||
.vignetting_correction = true,
|
||||
.output_type = ISP_IFE_PROCESSED,
|
||||
.staggered_sof = false,
|
||||
};
|
||||
|
||||
const CameraConfig DRIVER_CAMERA_CONFIG = {
|
||||
@@ -63,6 +66,7 @@ const CameraConfig DRIVER_CAMERA_CONFIG = {
|
||||
.phy = CAM_ISP_IFE_IN_RES_PHY_2,
|
||||
.vignetting_correction = false,
|
||||
.output_type = ISP_BPS_PROCESSED,
|
||||
.staggered_sof = true,
|
||||
};
|
||||
|
||||
const CameraConfig ALL_CAMERA_CONFIGS[] = {WIDE_ROAD_CAMERA_CONFIG, ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG};
|
||||
|
||||
@@ -1436,7 +1436,7 @@ bool SpectraCamera::waitForFrameReady(uint64_t request_id) {
|
||||
}
|
||||
|
||||
bool SpectraCamera::processFrame(int buf_idx, uint64_t request_id, uint64_t frame_id_raw, uint64_t timestamp) {
|
||||
if (!syncFirstFrame(cc.camera_num, request_id, frame_id_raw, timestamp)) {
|
||||
if (!syncFirstFrame(cc.camera_num, request_id, frame_id_raw, timestamp, cc.staggered_sof)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -1455,23 +1455,31 @@ bool SpectraCamera::processFrame(int buf_idx, uint64_t request_id, uint64_t fram
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SpectraCamera::syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp) {
|
||||
bool SpectraCamera::syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp, bool staggered) {
|
||||
if (first_frame_synced) return true;
|
||||
|
||||
// Store the frame data for this camera
|
||||
camera_sync_data[camera_id] = SyncData{timestamp, raw_id + 1};
|
||||
camera_sync_data[camera_id] = SyncData{timestamp, raw_id + 1, staggered};
|
||||
|
||||
// Ensure all cameras are up
|
||||
int enabled_camera_count = std::count_if(std::begin(ALL_CAMERA_CONFIGS), std::end(ALL_CAMERA_CONFIGS),
|
||||
[](const auto &config) { return config.enabled; });
|
||||
bool all_cams_up = camera_sync_data.size() == enabled_camera_count;
|
||||
|
||||
// Wait until the timestamps line up
|
||||
// Check that camera timestamps are properly aligned:
|
||||
// - non-staggered cameras should be within 0.2ms of each other
|
||||
// - staggered cameras should be within 0.2ms of a 25ms offset from non-staggered cameras
|
||||
const uint64_t half_period_ns = 25 * 1000000ULL; // 25ms
|
||||
const uint64_t tolerance_ns = 200000ULL; // 0.2ms
|
||||
bool all_cams_synced = true;
|
||||
for (const auto &[_, sync_data] : camera_sync_data) {
|
||||
for (const auto &[cam, sync_data] : camera_sync_data) {
|
||||
if (cam == camera_id) continue;
|
||||
uint64_t diff = std::max(timestamp, sync_data.timestamp) -
|
||||
std::min(timestamp, sync_data.timestamp);
|
||||
if (diff > 0.2*1e6) { // milliseconds
|
||||
bool pair_staggered = staggered != sync_data.staggered;
|
||||
uint64_t expected_offset = pair_staggered ? half_period_ns : 0;
|
||||
uint64_t error = (diff > expected_offset) ? diff - expected_offset : expected_offset - diff;
|
||||
if (error > tolerance_ns) {
|
||||
all_cams_synced = false;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -194,10 +194,11 @@ private:
|
||||
bool validateEvent(uint64_t request_id, uint64_t frame_id_raw);
|
||||
bool waitForFrameReady(uint64_t request_id);
|
||||
bool processFrame(int buf_idx, uint64_t request_id, uint64_t frame_id_raw, uint64_t timestamp);
|
||||
static bool syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp);
|
||||
static bool syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp, bool staggered);
|
||||
struct SyncData {
|
||||
uint64_t timestamp;
|
||||
uint64_t frame_id_offset = 0;
|
||||
bool staggered = false;
|
||||
};
|
||||
inline static std::map<int, SyncData> camera_sync_data;
|
||||
inline static bool first_frame_synced = false;
|
||||
|
||||
@@ -105,17 +105,23 @@ class TestCamerad:
|
||||
assert set(np.diff(logs[c]['frameId'])) == {1, }, f"{c} has frame skips"
|
||||
|
||||
def test_frame_sync(self, logs):
|
||||
SYNCED_CAMS = ('roadCameraState', 'wideRoadCameraState')
|
||||
n = range(len(logs['roadCameraState']['t'][:-10]))
|
||||
|
||||
frame_ids = {i: [logs[cam]['frameId'][i] for cam in CAMERAS] for i in n}
|
||||
assert all(len(set(v)) == 1 for v in frame_ids.values()), "frame IDs not aligned"
|
||||
|
||||
frame_times = {i: [logs[cam]['timestampSof'][i] for cam in CAMERAS] for i in n}
|
||||
diffs = {i: (max(ts) - min(ts))/1e6 for i, ts in frame_times.items()}
|
||||
|
||||
# road and wide cameras should be synced within 1.1ms
|
||||
synced_times = {i: [logs[cam]['timestampSof'][i] for cam in SYNCED_CAMS] for i in n}
|
||||
diffs = {i: (max(ts) - min(ts))/1e6 for i, ts in synced_times.items()}
|
||||
laggy_frames = {k: v for k, v in diffs.items() if v > 1.1}
|
||||
assert len(laggy_frames) == 0, f"Frames not synced properly: {laggy_frames=}"
|
||||
|
||||
# driver camera should be staggered ~25ms from road camera
|
||||
for i in n:
|
||||
offset_ms = abs(logs['driverCameraState']['timestampSof'][i] - logs['roadCameraState']['timestampSof'][i]) / 1e6
|
||||
assert 20 < offset_ms < 30, f"driver camera stagger out of range at frame {i}: {offset_ms:.1f}ms (expected ~25ms)"
|
||||
|
||||
def test_sanity_checks(self, logs):
|
||||
self._sanity_checks(logs)
|
||||
|
||||
|
||||
@@ -56,28 +56,28 @@
|
||||
},
|
||||
{
|
||||
"name": "boot",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/boot-a0185fa5ffc860de2179e4d0fec703fef6d560eacd730f79f60891ca79c72756.img.xz",
|
||||
"hash": "a0185fa5ffc860de2179e4d0fec703fef6d560eacd730f79f60891ca79c72756",
|
||||
"hash_raw": "a0185fa5ffc860de2179e4d0fec703fef6d560eacd730f79f60891ca79c72756",
|
||||
"size": 17496064,
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/boot-d726315cf98a43e1090e5b49297404cf3d084cfbd42ad8bb7d8afb68136b9f51.img.xz",
|
||||
"hash": "d726315cf98a43e1090e5b49297404cf3d084cfbd42ad8bb7d8afb68136b9f51",
|
||||
"hash_raw": "d726315cf98a43e1090e5b49297404cf3d084cfbd42ad8bb7d8afb68136b9f51",
|
||||
"size": 17500160,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "0ee1ab104bb46d0f72e7d0b7d3e94629a7644a368896c6d4c558554fb955a08a"
|
||||
"ondevice_hash": "2454108de1161289bc4a75449ad6421f1772b13b3e5cba68a84fca7530557699"
|
||||
},
|
||||
{
|
||||
"name": "system",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd.img.xz",
|
||||
"hash": "7c58308be461126677ba02e9c9739556520ee02958934733867d86ecfe2e58e9",
|
||||
"hash_raw": "0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5.img.xz",
|
||||
"hash": "5f319030ad05942267b77f1a4686c4ca24cc09b2c2a4688e57342ffc9720fd49",
|
||||
"hash_raw": "dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5",
|
||||
"size": 4718592000,
|
||||
"sparse": true,
|
||||
"full_check": false,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "826790516410c325aa30265846946d06a556f0a7b23c957f65fd11c055a663da",
|
||||
"ondevice_hash": "c12f1b7d790a418aea17424accf4cd59c575e5745cad82bdc9452f384483648c",
|
||||
"alt": {
|
||||
"hash": "0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd.img",
|
||||
"hash": "dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5.img",
|
||||
"size": 4718592000
|
||||
}
|
||||
}
|
||||
|
||||
@@ -339,62 +339,51 @@
|
||||
},
|
||||
{
|
||||
"name": "boot",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/boot-a0185fa5ffc860de2179e4d0fec703fef6d560eacd730f79f60891ca79c72756.img.xz",
|
||||
"hash": "a0185fa5ffc860de2179e4d0fec703fef6d560eacd730f79f60891ca79c72756",
|
||||
"hash_raw": "a0185fa5ffc860de2179e4d0fec703fef6d560eacd730f79f60891ca79c72756",
|
||||
"size": 17496064,
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/boot-d726315cf98a43e1090e5b49297404cf3d084cfbd42ad8bb7d8afb68136b9f51.img.xz",
|
||||
"hash": "d726315cf98a43e1090e5b49297404cf3d084cfbd42ad8bb7d8afb68136b9f51",
|
||||
"hash_raw": "d726315cf98a43e1090e5b49297404cf3d084cfbd42ad8bb7d8afb68136b9f51",
|
||||
"size": 17500160,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "0ee1ab104bb46d0f72e7d0b7d3e94629a7644a368896c6d4c558554fb955a08a"
|
||||
"ondevice_hash": "2454108de1161289bc4a75449ad6421f1772b13b3e5cba68a84fca7530557699"
|
||||
},
|
||||
{
|
||||
"name": "system",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd.img.xz",
|
||||
"hash": "7c58308be461126677ba02e9c9739556520ee02958934733867d86ecfe2e58e9",
|
||||
"hash_raw": "0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5.img.xz",
|
||||
"hash": "5f319030ad05942267b77f1a4686c4ca24cc09b2c2a4688e57342ffc9720fd49",
|
||||
"hash_raw": "dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5",
|
||||
"size": 4718592000,
|
||||
"sparse": true,
|
||||
"full_check": false,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "826790516410c325aa30265846946d06a556f0a7b23c957f65fd11c055a663da",
|
||||
"ondevice_hash": "c12f1b7d790a418aea17424accf4cd59c575e5745cad82bdc9452f384483648c",
|
||||
"alt": {
|
||||
"hash": "0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-0cf8cb01e40d05d6d325afe68b934a6c0dda3a56703b2ef3e3de637d754ae5dd.img",
|
||||
"hash": "dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5.img",
|
||||
"size": 4718592000
|
||||
}
|
||||
},
|
||||
{
|
||||
"name": "userdata_90",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/userdata_90-ec31b8116125a95755adb32853c401c462a14a74f538535532bf2c34d72c60eb.img.xz",
|
||||
"hash": "aa0f0fe32187493e6135aee9e984d3f9705fc58560d537b34687bb6b51a38428",
|
||||
"hash_raw": "ec31b8116125a95755adb32853c401c462a14a74f538535532bf2c34d72c60eb",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/userdata_90-a7b25ea29255f4fd3a2da99e037f40b4ca10bd4afd57dd96563353b8dfb0f634.img.xz",
|
||||
"hash": "7ea9d7d4685ec36bbfdf06afe0b51650d567416c3092fef96bd97158ed322742",
|
||||
"hash_raw": "a7b25ea29255f4fd3a2da99e037f40b4ca10bd4afd57dd96563353b8dfb0f634",
|
||||
"size": 96636764160,
|
||||
"sparse": true,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "9c916b7d05543d4608b0401bc867639f44ce9671639a1a6da83b6d58b4eaa1b4"
|
||||
"ondevice_hash": "79ed653c1679d84b13ee23083a511b0e668454e4af9b0db99a3279072ed041c1"
|
||||
},
|
||||
{
|
||||
"name": "userdata_89",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/userdata_89-7f092cc841124c10300e43574e90e3367e983bfbe4faa0969024e79e5ce90b11.img.xz",
|
||||
"hash": "fa83d4b7096857136820b0b0a8785c90677256b054c5c14039cd7b9b1065a90b",
|
||||
"hash_raw": "7f092cc841124c10300e43574e90e3367e983bfbe4faa0969024e79e5ce90b11",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/userdata_89-8e428632c967aa609cac184bff938a90240e53ffd3b4fca40bc94c33c81202ba.img.xz",
|
||||
"hash": "7104cdb0384e4ecb1ebfa6136a2330251bc8aa829b9ec48c4b740f656252d382",
|
||||
"hash_raw": "8e428632c967aa609cac184bff938a90240e53ffd3b4fca40bc94c33c81202ba",
|
||||
"size": 95563022336,
|
||||
"sparse": true,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "1699e38de769eb32c21dfa6a5ac21eb3ad620a362c7b8abf1a2c0afe0f717530"
|
||||
},
|
||||
{
|
||||
"name": "userdata_30",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/userdata_30-3df2dcd5e1f426c90b090fdbcd1a95b035d96a4bdaf88d5517245db5ee84f5ed.img.xz",
|
||||
"hash": "890910f20b1ad88a728ee822a47b1234eb3d70cab28ca8a935679c8c2d33cbe9",
|
||||
"hash_raw": "3df2dcd5e1f426c90b090fdbcd1a95b035d96a4bdaf88d5517245db5ee84f5ed",
|
||||
"size": 32212254720,
|
||||
"sparse": true,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "8e7cb392dd6e49c7d59fa850be7d1f44901314c86ba9c88be5bb27a0cd1123c9"
|
||||
"ondevice_hash": "fbede3b0831dbc4a4edd336e5f547f4978902b9421fb1484e86c416192c59165"
|
||||
}
|
||||
]
|
||||
BIN
system/hardware/tici/updater_magic
Normal file → Executable file
0
system/hardware/tici/updater_weston
Normal file → Executable file
@@ -7,7 +7,7 @@ import math
|
||||
import time
|
||||
import requests
|
||||
import shutil
|
||||
import subprocess
|
||||
from serial import Serial
|
||||
import datetime
|
||||
from multiprocessing import Process, Event
|
||||
from typing import NoReturn
|
||||
@@ -90,9 +90,24 @@ measurementStatusGlonassFields = {
|
||||
def try_setup_logs(diag, logs):
|
||||
return setup_logs(diag, logs)
|
||||
|
||||
@retry(attempts=3, delay=1.0)
|
||||
def at_cmd(cmd: str) -> str | None:
|
||||
return subprocess.check_output(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True, encoding='utf8')
|
||||
AT_PORT = "/dev/modem_at0"
|
||||
|
||||
@retry(attempts=5, delay=1.0)
|
||||
def at_cmd(cmd: str) -> str:
|
||||
with Serial(AT_PORT, baudrate=115200, timeout=5) as ser:
|
||||
ser.reset_input_buffer()
|
||||
ser.write(f"{cmd}\r".encode())
|
||||
lines = []
|
||||
while True:
|
||||
line = ser.readline()
|
||||
if not line:
|
||||
raise RuntimeError(f"AT command timeout: {cmd}")
|
||||
line = line.decode('utf-8', errors='replace').strip()
|
||||
if line in ("OK", "ERROR") or line.startswith("+CME ERROR"):
|
||||
break
|
||||
if line and line != cmd:
|
||||
lines.append(line)
|
||||
return '\n'.join(lines)
|
||||
|
||||
def gps_enabled() -> bool:
|
||||
return "QGPS: 1" in at_cmd("AT+QGPS?")
|
||||
@@ -131,6 +146,7 @@ def downloader_loop(event):
|
||||
|
||||
@retry(attempts=5, delay=0.2, ignore_failure=True)
|
||||
def inject_assistance():
|
||||
import subprocess
|
||||
cmd = f"mmcli -m any --timeout 30 --location-inject-assistance-data={ASSIST_DATA_FILE}"
|
||||
subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True)
|
||||
cloudlog.info("successfully loaded assistance data")
|
||||
@@ -207,13 +223,19 @@ def teardown_quectel(diag):
|
||||
try_setup_logs(diag, [])
|
||||
|
||||
|
||||
def wait_for_modem(cmd="AT+QGPS?"):
|
||||
def wait_for_modem():
|
||||
cloudlog.warning("waiting for modem to come up")
|
||||
while not os.path.exists(AT_PORT):
|
||||
time.sleep(0.5)
|
||||
# wait until the modem GNSS subsystem responds
|
||||
while True:
|
||||
ret = subprocess.call(f"mmcli -m any --timeout 10 --command=\"{cmd}\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True)
|
||||
if ret == 0:
|
||||
return
|
||||
time.sleep(0.1)
|
||||
try:
|
||||
resp = at_cmd("AT+QGPS?")
|
||||
if "+QGPS:" in resp:
|
||||
return
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.5)
|
||||
|
||||
|
||||
def main() -> NoReturn:
|
||||
@@ -335,6 +357,9 @@ def main() -> NoReturn:
|
||||
report = unpack_position(log_payload)
|
||||
if report["u_PosSource"] != 2:
|
||||
continue
|
||||
# uint16_t max is an invalid sentinel value from the modem
|
||||
if report['w_GpsWeekNumber'] >= 0xFFFF:
|
||||
continue
|
||||
vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]]
|
||||
vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]]
|
||||
|
||||
|
||||
@@ -5,7 +5,7 @@ import time
|
||||
from typing import NoReturn
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.time_helpers import min_date, system_time_valid
|
||||
from openpilot.common.time_helpers import min_date, MAX_DATE, system_time_valid
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
@@ -52,7 +52,7 @@ def main() -> NoReturn:
|
||||
continue
|
||||
if not gps.hasFix:
|
||||
continue
|
||||
if gps_time < min_date():
|
||||
if gps_time < min_date() or gps_time > MAX_DATE:
|
||||
continue
|
||||
|
||||
set_time(gps_time)
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
import atexit
|
||||
import cffi
|
||||
import math
|
||||
import os
|
||||
import queue
|
||||
import time
|
||||
@@ -95,7 +96,6 @@ FONT_DIR = ASSETS_DIR.joinpath("fonts")
|
||||
|
||||
|
||||
class FontWeight(StrEnum):
|
||||
LIGHT = "Inter-Light.fnt"
|
||||
NORMAL = "Inter-Regular.fnt" if BIG_UI else "Inter-Medium.fnt"
|
||||
MEDIUM = "Inter-Medium.fnt"
|
||||
BOLD = "Inter-Bold.fnt"
|
||||
@@ -171,6 +171,10 @@ class MouseState:
|
||||
self._rk.keep_time()
|
||||
|
||||
def _handle_mouse_event(self):
|
||||
# TODO: read touch events from evdev directly to get real kernel timestamps.
|
||||
# Polling at 140Hz with time.monotonic() causes timing jitter that makes scroll
|
||||
# velocity oscillate (alternating high/low). Real timestamps would also let us
|
||||
# detect swipe-stop-lift via event gaps instead of the fragile decel heuristic.
|
||||
for slot in range(MAX_TOUCH_SLOTS):
|
||||
mouse_pos = rl.get_touch_position(slot)
|
||||
x = mouse_pos.x / self._scale if self._scale != 1.0 else mouse_pos.x
|
||||
@@ -402,7 +406,7 @@ class GuiApplication:
|
||||
if self._scale != 1.0:
|
||||
rl.set_mouse_scale(1 / self._scale, 1 / self._scale)
|
||||
if needs_render_texture:
|
||||
self._render_texture = rl.load_render_texture(self._width, self._height)
|
||||
self._render_texture = rl.load_render_texture(self._scaled_width, self._scaled_height)
|
||||
rl.set_texture_filter(self._render_texture.texture, rl.TextureFilter.TEXTURE_FILTER_BILINEAR)
|
||||
|
||||
if RECORD:
|
||||
@@ -413,13 +417,13 @@ class GuiApplication:
|
||||
'-nostats', # Suppress encoding progress
|
||||
'-f', 'rawvideo', # Input format
|
||||
'-pix_fmt', 'rgba', # Input pixel format
|
||||
'-s', f'{self._width}x{self._height}', # Input resolution
|
||||
'-s', f'{self._scaled_width}x{self._scaled_height}', # Input resolution
|
||||
'-r', str(fps), # Input frame rate
|
||||
'-i', 'pipe:0', # Input from stdin
|
||||
'-vf', 'vflip,format=yuv420p', # Flip vertically and convert to yuv420p
|
||||
'-r', str(output_fps), # Output frame rate (for speed multiplier)
|
||||
'-c:v', 'libx264',
|
||||
'-preset', 'ultrafast',
|
||||
'-preset', 'veryfast',
|
||||
'-crf', str(RECORD_QUALITY)
|
||||
]
|
||||
if RECORD_BITRATE:
|
||||
@@ -443,6 +447,7 @@ class GuiApplication:
|
||||
self._set_styles()
|
||||
self._load_fonts()
|
||||
self._patch_text_functions()
|
||||
self._patch_scissor_mode()
|
||||
if BURN_IN_MODE and self._burn_in_shader is None:
|
||||
self._burn_in_shader = rl.load_shader_from_memory(BURN_IN_VERTEX_SHADER, BURN_IN_FRAGMENT_SHADER)
|
||||
|
||||
@@ -578,6 +583,12 @@ class GuiApplication:
|
||||
with as_file(ASSETS_DIR.joinpath(asset_path)) as fspath:
|
||||
image_obj = self._load_image_from_path(fspath.as_posix(), width, height, alpha_premultiply, keep_aspect_ratio, flip_x)
|
||||
texture_obj = self._load_texture_from_image(image_obj)
|
||||
|
||||
# Set logical size so widget layout math stays at 1x coordinates
|
||||
if self._scale != 1.0 and width is not None and height is not None:
|
||||
texture_obj.width = width
|
||||
texture_obj.height = height
|
||||
|
||||
self._textures[cache_key] = texture_obj
|
||||
return texture_obj
|
||||
|
||||
@@ -589,6 +600,11 @@ class GuiApplication:
|
||||
if alpha_premultiply:
|
||||
rl.image_alpha_premultiply(image)
|
||||
|
||||
# Scale up load size for sharper rendering, capped at source resolution
|
||||
if self._scale != 1.0 and width is not None and height is not None:
|
||||
width = min(int(width * self._scale), image.width)
|
||||
height = min(int(height * self._scale), image.height)
|
||||
|
||||
if width is not None and height is not None:
|
||||
same_dimensions = image.width == width and image.height == height
|
||||
|
||||
@@ -759,6 +775,10 @@ class GuiApplication:
|
||||
rl.begin_drawing()
|
||||
rl.clear_background(rl.BLACK)
|
||||
|
||||
if self._scale != 1.0:
|
||||
rl.rl_push_matrix()
|
||||
rl.rl_scalef(self._scale, self._scale, 1.0)
|
||||
|
||||
# Allow a Widget to still run a function regardless of the stack depth
|
||||
for tick in self._nav_stack_ticks:
|
||||
tick()
|
||||
@@ -769,11 +789,14 @@ class GuiApplication:
|
||||
|
||||
yield True
|
||||
|
||||
if self._scale != 1.0:
|
||||
rl.rl_pop_matrix()
|
||||
|
||||
if self._render_texture:
|
||||
rl.end_texture_mode()
|
||||
rl.begin_drawing()
|
||||
rl.clear_background(rl.BLACK)
|
||||
src_rect = rl.Rectangle(0, 0, float(self._width), -float(self._height))
|
||||
src_rect = rl.Rectangle(0, 0, float(self._scaled_width), -float(self._scaled_height))
|
||||
dst_rect = rl.Rectangle(0, 0, float(self._scaled_width), float(self._scaled_height))
|
||||
texture = self._render_texture.texture
|
||||
if texture:
|
||||
@@ -851,7 +874,8 @@ class GuiApplication:
|
||||
print(f"[FONTDBG] loaded {fw}: tex_id={tex_id} tex={tw}x{th}")
|
||||
|
||||
if fw != FontWeight.UNIFONT and tex_id > 0:
|
||||
rl.set_texture_filter(font.texture, rl.TextureFilter.TEXTURE_FILTER_BILINEAR)
|
||||
rl.gen_texture_mipmaps(font.texture)
|
||||
rl.set_texture_filter(font.texture, rl.TextureFilter.TEXTURE_FILTER_TRILINEAR)
|
||||
|
||||
self._fonts[fw] = font
|
||||
|
||||
@@ -877,6 +901,20 @@ class GuiApplication:
|
||||
|
||||
rl.draw_text_ex = _draw_text_ex_scaled
|
||||
|
||||
def _patch_scissor_mode(self):
|
||||
if self._scale == 1.0:
|
||||
return
|
||||
|
||||
if not hasattr(rl, "_orig_begin_scissor_mode"):
|
||||
rl._orig_begin_scissor_mode = rl.begin_scissor_mode
|
||||
|
||||
def _begin_scissor_mode_scaled(x, y, width, height):
|
||||
return rl._orig_begin_scissor_mode(
|
||||
int(x * self._scale), int(y * self._scale),
|
||||
int(math.ceil(width * self._scale)), int(math.ceil(height * self._scale)))
|
||||
|
||||
rl.begin_scissor_mode = _begin_scissor_mode_scaled
|
||||
|
||||
def _set_log_callback(self):
|
||||
ffi_libc = cffi.FFI()
|
||||
ffi_libc.cdef("""
|
||||
|
||||
@@ -20,6 +20,21 @@ MAX_SPEED = 10000.0 # px/s
|
||||
DEBUG = os.getenv("DEBUG_SCROLL", "0") == "1"
|
||||
|
||||
|
||||
# Weights older (steadier) velocity samples more heavily on release.
|
||||
# Finger-lift samples are noisy; trusting earlier samples gives consistent fling velocity.
|
||||
# Reverse-engineered from iOS UIScrollView (tuned at 120Hz touch) by Flutter team:
|
||||
# https://github.com/flutter/flutter/pull/60501
|
||||
# 3 samples ? 25ms at 120Hz (iOS) / ~21ms at 140Hz (comma). Scale if touch rate changes.
|
||||
def weighted_velocity(buffer: deque) -> float:
|
||||
if len(buffer) >= 3:
|
||||
return buffer[-3] * 0.6 + buffer[-2] * 0.35 + buffer[-1] * 0.05
|
||||
elif len(buffer) == 2:
|
||||
return buffer[-2] * 0.7 + buffer[-1] * 0.3
|
||||
elif len(buffer) == 1:
|
||||
return buffer[-1]
|
||||
return 0.0
|
||||
|
||||
|
||||
# from https://ariya.io/2011/10/flick-list-with-its-momentum-scrolling-and-deceleration
|
||||
class ScrollState(Enum):
|
||||
STEADY = 0
|
||||
@@ -151,7 +166,13 @@ class GuiScrollPanel2:
|
||||
# Touch rejection: when releasing finger after swiping and stopping, panel
|
||||
# reports a few erroneous touch events with high velocity, try to ignore.
|
||||
|
||||
# If velocity decelerates very quickly, assume user doesn't intend to auto scroll
|
||||
# If velocity decelerates very quickly, assume user doesn't intend to auto scroll.
|
||||
# Catches two cases: 1) swipe, stop finger, then lift (stale high velocity in buffer)
|
||||
# 2) dirty finger lift where finger rotates/slides producing spurious velocity spike.
|
||||
# TODO: this heuristic false-positives on fast swipes because 140Hz touch polling
|
||||
# jitter causes velocity to oscillate (not real deceleration). Better approaches:
|
||||
# - Use evdev kernel timestamps to eliminate velocity oscillation at the source
|
||||
# - Replace with a time-since-last-event check (40ms timeout) for swipe-stop-lift
|
||||
high_decel = False
|
||||
if len(self._velocity_buffer) > 2:
|
||||
# We limit max to first half since final few velocities can surpass first few
|
||||
@@ -166,6 +187,8 @@ class GuiScrollPanel2:
|
||||
print('deceleration too high, going to STEADY')
|
||||
high_decel = True
|
||||
|
||||
self._velocity = weighted_velocity(self._velocity_buffer)
|
||||
|
||||
# If final velocity is below some threshold, switch to steady state too
|
||||
low_speed = abs(self._velocity) <= MIN_VELOCITY_FOR_CLICKING * 1.5 # plus some margin
|
||||
|
||||
|
||||
@@ -854,8 +854,12 @@ class WifiManager:
|
||||
|
||||
# NOTE: AccessPoints property may exclude hidden APs (use GetAllAccessPoints method if needed)
|
||||
wifi_addr = DBusAddress(self._wifi_device, NM, interface=NM_WIRELESS_IFACE)
|
||||
wifi_props = self._router_main.send_and_get_reply(Properties(wifi_addr).get_all()).body[0]
|
||||
ap_paths = wifi_props.get('AccessPoints', ('ao', []))[1]
|
||||
wifi_props_reply = self._router_main.send_and_get_reply(Properties(wifi_addr).get_all())
|
||||
if wifi_props_reply.header.message_type == MessageType.error:
|
||||
cloudlog.warning(f"Failed to get WiFi properties: {wifi_props_reply}")
|
||||
return
|
||||
|
||||
ap_paths = wifi_props_reply.body[0].get('AccessPoints', ('ao', []))[1]
|
||||
|
||||
aps: dict[str, list[AccessPoint]] = {}
|
||||
|
||||
|
||||
@@ -1,18 +1,17 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
import threading
|
||||
from enum import IntEnum
|
||||
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.system.hardware import PC
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
from openpilot.system.ui.widgets.slider import SmallSlider
|
||||
from openpilot.system.ui.widgets.button import SmallButton, FullRoundedButton
|
||||
from openpilot.system.ui.widgets.label import gui_label, gui_text_box
|
||||
from openpilot.system.hardware import HARDWARE, PC
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.widgets.scroller import Scroller
|
||||
from openpilot.system.ui.mici_setup import GreyBigButton, FailedPage
|
||||
from openpilot.selfdrive.ui.mici.widgets.dialog import BigDialog, BigConfirmationCircleButton
|
||||
|
||||
USERDATA = "/dev/disk/by-partlabel/userdata"
|
||||
TIMEOUT = 3*60
|
||||
@@ -21,35 +20,85 @@ TIMEOUT = 3*60
|
||||
class ResetMode(IntEnum):
|
||||
USER_RESET = 0 # user initiated a factory reset from openpilot
|
||||
RECOVER = 1 # userdata is corrupt for some reason, give a chance to recover
|
||||
FORMAT = 2 # finish up a factory reset from a tool that doesn't flash an empty partition to userdata
|
||||
TAP_RESET = 2 # user initiated a factory reset by tapping the screen during boot
|
||||
|
||||
|
||||
class ResetState(IntEnum):
|
||||
NONE = 0
|
||||
RESETTING = 1
|
||||
FAILED = 2
|
||||
class ResetFailedPage(FailedPage):
|
||||
def __init__(self):
|
||||
super().__init__(None, "reset failed", "reboot to try again", icon="icons_mici/setup/reset_failed.png")
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._nav_bar._alpha = 0.0 # not dismissable
|
||||
|
||||
def _back_enabled(self) -> bool:
|
||||
return False
|
||||
|
||||
|
||||
class Reset(Widget):
|
||||
class ResettingPage(BigDialog):
|
||||
DOT_STEP = 0.6
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("resetting device", "this may take up to\na minute...",
|
||||
gui_app.texture("icons_mici/setup/factory_reset.png", 64, 64))
|
||||
self._show_time = 0.0
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._nav_bar._alpha = 0.0 # not dismissable
|
||||
self._show_time = rl.get_time()
|
||||
|
||||
def _back_enabled(self) -> bool:
|
||||
return False
|
||||
|
||||
def _render(self, _):
|
||||
t = (rl.get_time() - self._show_time) % (self.DOT_STEP * 2)
|
||||
dots = "." * min(int(t / (self.DOT_STEP / 4)), 3)
|
||||
self._card.set_value(f"this may take up to\na minute{dots}")
|
||||
super()._render(_)
|
||||
|
||||
|
||||
class Reset(Scroller):
|
||||
def __init__(self, mode):
|
||||
super().__init__()
|
||||
self._mode = mode
|
||||
self._previous_reset_state = None
|
||||
self._reset_state = ResetState.NONE
|
||||
self._previous_active_widget = None
|
||||
self._reset_failed = False
|
||||
self._timeout_st = time.monotonic()
|
||||
|
||||
self._cancel_button = SmallButton("cancel")
|
||||
self._cancel_button.set_click_callback(gui_app.request_close)
|
||||
self._resetting_page = ResettingPage()
|
||||
self._reset_failed_page = ResetFailedPage()
|
||||
|
||||
self._reboot_button = FullRoundedButton("reboot")
|
||||
self._reboot_button.set_click_callback(self._do_reboot)
|
||||
self._reset_button = BigConfirmationCircleButton("reset &\nerase", gui_app.texture("icons_mici/settings/device/uninstall.png", 70, 70),
|
||||
self._start_reset, exit_on_confirm=False, red=True)
|
||||
self._cancel_button = BigConfirmationCircleButton("cancel", gui_app.texture("icons_mici/setup/cancel.png", 64, 64),
|
||||
gui_app.request_close, exit_on_confirm=False)
|
||||
self._reboot_button = BigConfirmationCircleButton("reboot\ndevice", gui_app.texture("icons_mici/settings/device/reboot.png", 64, 70),
|
||||
HARDWARE.reboot, exit_on_confirm=False)
|
||||
|
||||
self._confirm_slider = SmallSlider("reset", self._confirm)
|
||||
# show reboot button if in recover mode
|
||||
self._cancel_button.set_visible(mode != ResetMode.RECOVER)
|
||||
self._reboot_button.set_visible(mode == ResetMode.RECOVER)
|
||||
|
||||
def _do_reboot(self):
|
||||
if PC:
|
||||
return
|
||||
main_card = GreyBigButton("factory reset", "resetting erases\nall user content & data",
|
||||
gui_app.texture("icons_mici/setup/factory_reset.png", 64, 64))
|
||||
self._scroller.add_widget(main_card)
|
||||
|
||||
os.system("sudo reboot")
|
||||
if mode != ResetMode.USER_RESET:
|
||||
self._scroller.add_widget(GreyBigButton("", "Resetting erases all user content & data."))
|
||||
if mode == ResetMode.RECOVER:
|
||||
main_card.set_value("user data partition\ncould not be mounted")
|
||||
elif mode == ResetMode.TAP_RESET:
|
||||
main_card.set_value("reset triggered by\ntapping the screen")
|
||||
|
||||
self._scroller.add_widgets([
|
||||
GreyBigButton("", "For a deeper reset, go to\nhttps://flash.comma.ai"),
|
||||
self._cancel_button,
|
||||
self._reboot_button,
|
||||
self._reset_button,
|
||||
])
|
||||
|
||||
gui_app.add_nav_stack_tick(self._nav_stack_tick)
|
||||
|
||||
def _do_erase(self):
|
||||
if PC:
|
||||
@@ -63,87 +112,38 @@ class Reset(Widget):
|
||||
if rm == 0 or fmt == 0:
|
||||
os.system("sudo reboot")
|
||||
else:
|
||||
self._reset_state = ResetState.FAILED
|
||||
self._reset_failed = True
|
||||
|
||||
def start_reset(self):
|
||||
self._reset_state = ResetState.RESETTING
|
||||
threading.Timer(0.1, self._do_erase).start()
|
||||
def _start_reset(self):
|
||||
def do_erase_thread():
|
||||
threading.Thread(target=self._do_erase, daemon=True).start()
|
||||
|
||||
def _update_state(self):
|
||||
if self._reset_state != self._previous_reset_state:
|
||||
self._previous_reset_state = self._reset_state
|
||||
self._resetting_page.set_shown_callback(do_erase_thread)
|
||||
gui_app.push_widget(self._resetting_page)
|
||||
|
||||
def _nav_stack_tick(self):
|
||||
if self._reset_failed:
|
||||
self._reset_failed = False
|
||||
gui_app.pop_widgets_to(self, lambda: gui_app.push_widget(self._reset_failed_page))
|
||||
|
||||
active_widget = gui_app.get_active_widget()
|
||||
if active_widget != self._previous_active_widget:
|
||||
self._previous_active_widget = active_widget
|
||||
self._timeout_st = time.monotonic()
|
||||
elif self._mode != ResetMode.RECOVER and self._reset_state != ResetState.RESETTING and (time.monotonic() - self._timeout_st) > TIMEOUT:
|
||||
elif self._mode != ResetMode.RECOVER and active_widget != self._resetting_page and (time.monotonic() - self._timeout_st) > TIMEOUT:
|
||||
exit(0)
|
||||
|
||||
def _render(self, rect: rl.Rectangle):
|
||||
label_rect = rl.Rectangle(rect.x + 8, rect.y + 8, rect.width, 50)
|
||||
gui_label(label_rect, "factory reset", 48, font_weight=FontWeight.BOLD,
|
||||
color=rl.Color(255, 255, 255, int(255 * 0.9)))
|
||||
|
||||
text_rect = rl.Rectangle(rect.x + 8, rect.y + 56, rect.width - 8 * 2, rect.height - 80)
|
||||
gui_text_box(text_rect, self._get_body_text(), 36, font_weight=FontWeight.ROMAN, line_scale=0.9)
|
||||
|
||||
if self._reset_state != ResetState.RESETTING:
|
||||
# fade out cancel button as slider is moved, set visible to prevent pressing invisible cancel
|
||||
self._cancel_button.set_opacity(1.0 - self._confirm_slider.slider_percentage)
|
||||
self._cancel_button.set_visible(self._confirm_slider.slider_percentage < 0.8)
|
||||
|
||||
if self._mode == ResetMode.RECOVER:
|
||||
self._cancel_button.set_text("reboot")
|
||||
self._cancel_button.set_click_callback(self._do_reboot)
|
||||
self._cancel_button.render(rl.Rectangle(
|
||||
rect.x + 8,
|
||||
rect.y + rect.height - self._cancel_button.rect.height,
|
||||
self._cancel_button.rect.width,
|
||||
self._cancel_button.rect.height))
|
||||
elif self._mode == ResetMode.USER_RESET and self._reset_state != ResetState.FAILED:
|
||||
self._cancel_button.render(rl.Rectangle(
|
||||
rect.x + 8,
|
||||
rect.y + rect.height - self._cancel_button.rect.height,
|
||||
self._cancel_button.rect.width,
|
||||
self._cancel_button.rect.height))
|
||||
|
||||
if self._reset_state != ResetState.FAILED:
|
||||
self._confirm_slider.render(rl.Rectangle(
|
||||
rect.x + rect.width - self._confirm_slider.rect.width,
|
||||
rect.y + rect.height - self._confirm_slider.rect.height,
|
||||
self._confirm_slider.rect.width,
|
||||
self._confirm_slider.rect.height))
|
||||
else:
|
||||
self._reboot_button.render(rl.Rectangle(
|
||||
rect.x + 8,
|
||||
rect.y + rect.height - self._reboot_button.rect.height,
|
||||
self._reboot_button.rect.width,
|
||||
self._reboot_button.rect.height))
|
||||
|
||||
def _confirm(self):
|
||||
self.start_reset()
|
||||
|
||||
def _get_body_text(self):
|
||||
if self._reset_state == ResetState.RESETTING:
|
||||
return "Resetting device... This may take up to a minute."
|
||||
if self._reset_state == ResetState.FAILED:
|
||||
return "Reset failed. Reboot to try again."
|
||||
if self._mode == ResetMode.RECOVER:
|
||||
return "Unable to mount data partition. It may be corrupted."
|
||||
return "All content and settings will be erased."
|
||||
|
||||
|
||||
def main():
|
||||
mode = ResetMode.USER_RESET
|
||||
if len(sys.argv) > 1:
|
||||
if sys.argv[1] == '--recover':
|
||||
mode = ResetMode.RECOVER
|
||||
elif sys.argv[1] == "--format":
|
||||
mode = ResetMode.FORMAT
|
||||
elif sys.argv[1] == '--tap-reset':
|
||||
mode = ResetMode.TAP_RESET
|
||||
|
||||
gui_app.init_window("System Reset")
|
||||
reset = Reset(mode)
|
||||
|
||||
if mode == ResetMode.FORMAT:
|
||||
reset.start_reset()
|
||||
|
||||
gui_app.push_widget(reset)
|
||||
|
||||
for _ in gui_app.render():
|
||||
|
||||