From 60da5dd39a767e1f6515c1adc0ebbe146867acd3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 17 Apr 2025 00:35:30 -0700 Subject: [PATCH 01/24] lagd: check sensors valid (#35027) * Check if sensors valid * Fix test * Fix static * Constants * time buffer for pose valid * Fix static --- selfdrive/locationd/lagd.py | 38 ++++++++++++++++++++------- selfdrive/locationd/test/test_lagd.py | 10 ++++--- 2 files changed, 34 insertions(+), 14 deletions(-) diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 5ce82309ea..b52a658d01 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -21,8 +21,11 @@ MIN_OKAY_WINDOW_SEC = 25.0 MIN_RECOVERY_BUFFER_SEC = 2.0 MIN_VEGO = 15.0 MIN_ABS_YAW_RATE = np.radians(1.0) +MAX_YAW_RATE_SANITY_CHECK = 1.0 MIN_NCC = 0.95 MAX_LAG = 1.0 +MAX_LAT_ACCEL = 2.0 +MAX_LAT_ACCEL_DIFF = 0.6 def masked_normalized_cross_correlation(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, n: int): @@ -139,7 +142,8 @@ class LateralLagEstimator: def __init__(self, CP: car.CarParams, dt: float, block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE, window_sec: float = MOVING_WINDOW_SEC, okay_window_sec: float = MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec: float = MIN_RECOVERY_BUFFER_SEC, - min_vego: float = MIN_VEGO, min_yr: float = MIN_ABS_YAW_RATE, min_ncc: float = MIN_NCC): + min_vego: float = MIN_VEGO, min_yr: float = MIN_ABS_YAW_RATE, min_ncc: float = MIN_NCC, + max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF): self.dt = dt self.window_sec = window_sec self.okay_window_sec = okay_window_sec @@ -151,6 +155,8 @@ class LateralLagEstimator: self.min_vego = min_vego self.min_yr = min_yr self.min_ncc = min_ncc + self.max_lat_accel = max_lat_accel + self.max_lat_accel_diff = max_lat_accel_diff self.t = 0.0 self.lat_active = False @@ -159,10 +165,13 @@ class LateralLagEstimator: self.desired_curvature = 0.0 self.v_ego = 0.0 self.yaw_rate = 0.0 + self.yaw_rate_std = 0.0 + self.pose_valid = False self.last_lat_inactive_t = 0.0 self.last_steering_pressed_t = 0.0 self.last_steering_saturated_t = 0.0 + self.last_pose_invalid_t = 0.0 self.last_estimate_t = 0.0 self.calibrator = PoseCalibrator() @@ -212,7 +221,9 @@ class LateralLagEstimator: elif which == "livePose": device_pose = Pose.from_live_pose(msg) calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) - self.yaw_rate = calibrated_pose.angular_velocity.z + self.yaw_rate = calibrated_pose.angular_velocity.yaw + self.yaw_rate_std = calibrated_pose.angular_velocity.yaw_std + self.pose_valid = msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK self.t = t def points_enough(self): @@ -222,23 +233,30 @@ class LateralLagEstimator: return self.points.num_okay >= int(self.okay_window_sec / self.dt) def update_points(self): + la_desired = self.desired_curvature * self.v_ego * self.v_ego + la_actual_pose = self.yaw_rate * self.v_ego + + fast = self.v_ego > self.min_vego + turning = np.abs(self.yaw_rate) >= self.min_yr + sensors_valid = self.pose_valid and np.abs(self.yaw_rate) < MAX_YAW_RATE_SANITY_CHECK and self.yaw_rate_std < MAX_YAW_RATE_SANITY_CHECK + la_valid = np.abs(la_actual_pose) <= self.max_lat_accel and np.abs(la_desired - la_actual_pose) <= self.max_lat_accel_diff + calib_valid = self.calibrator.calib_valid + if not self.lat_active: self.last_lat_inactive_t = self.t if self.steering_pressed: self.last_steering_pressed_t = self.t if self.steering_saturated: self.last_steering_saturated_t = self.t + if not sensors_valid or not la_valid: + self.last_pose_invalid_t = self.t - la_desired = self.desired_curvature * self.v_ego * self.v_ego - la_actual_pose = self.yaw_rate * self.v_ego - - fast = self.v_ego > self.min_vego - turning = np.abs(self.yaw_rate) >= self.min_yr - has_recovered = all( # wait for recovery after !lat_active, steering_pressed, steering_saturated + has_recovered = all( # wait for recovery after !lat_active, steering_pressed, steering_saturated, !sensors/la_valid self.t - last_t >= self.min_recovery_buffer_sec - for last_t in [self.last_lat_inactive_t, self.last_steering_pressed_t, self.last_steering_saturated_t] + for last_t in [self.last_lat_inactive_t, self.last_steering_pressed_t, self.last_steering_saturated_t, self.last_pose_invalid_t] ) - okay = self.lat_active and not self.steering_pressed and not self.steering_saturated and fast and turning and has_recovered + okay = self.lat_active and not self.steering_pressed and not self.steering_saturated and \ + fast and turning and has_recovered and calib_valid and sensors_valid and la_valid self.points.update(self.t, la_desired, la_actual_pose, okay) diff --git a/selfdrive/locationd/test/test_lagd.py b/selfdrive/locationd/test/test_lagd.py index 53af2d2573..bd5cfd6201 100644 --- a/selfdrive/locationd/test/test_lagd.py +++ b/selfdrive/locationd/test/test_lagd.py @@ -3,7 +3,7 @@ import numpy as np import time import pytest -from cereal import messaging +from cereal import messaging, log 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 @@ -23,8 +23,8 @@ def process_messages(mocker, estimator, lag_frames, n_frames, vego=20.0, rejecti for i in range(n_frames): t = i * estimator.dt - desired_la = np.cos(t) - actual_la = np.cos(t - lag_frames * estimator.dt) + desired_la = np.cos(t) * 0.1 + actual_la = np.cos(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 @@ -41,7 +41,9 @@ def process_messages(mocker, estimator, lag_frames, n_frames, vego=20.0, rejecti (t, "livePose", mocker.Mock(orientationNED=ZeroMock(), velocityDevice=ZeroMock(), accelerationDevice=ZeroMock(), - angularVelocityDevice=ZeroMock(z=actual_yr))), + angularVelocityDevice=ZeroMock(z=actual_yr, valid=True), + posenetOK=True, inputsOK=True)), + (t, "liveCalibration", mocker.Mock(rpyCalib=[0, 0, 0], calStatus=log.LiveCalibrationData.Status.calibrated)), ] for t, w, m in msgs: estimator.handle_log(t, w, m) From c933914f8679836f2b7a5b733a90086b266d5bc7 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Thu, 17 Apr 2025 23:11:19 +0100 Subject: [PATCH 02/24] ui(raylib): update spinner progress bar to match Qt (#35028) --- system/ui/spinner.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/system/ui/spinner.py b/system/ui/spinner.py index 9809307474..951c6f697e 100755 --- a/system/ui/spinner.py +++ b/system/ui/spinner.py @@ -13,6 +13,7 @@ ROTATION_TIME_SECONDS = 1.0 # Time for one full circle MARGIN = 200 TEXTURE_SIZE = 360 FONT_SIZE = 80 +DARKGRAY = (55, 55, 55, 255) def clamp(value, min_value, max_value): @@ -57,10 +58,10 @@ class Spinner: if text.isdigit(): progress = clamp(int(text), 0, 100) bar = rl.Rectangle(center.x - PROGRESS_BAR_WIDTH / 2.0, y_pos, PROGRESS_BAR_WIDTH, PROGRESS_BAR_HEIGHT) - rl.draw_rectangle_rounded(bar, 0.5, 10, rl.GRAY) + rl.draw_rectangle_rounded(bar, 1, 10, DARKGRAY) bar.width *= progress / 100.0 - rl.draw_rectangle_rounded(bar, 0.5, 10, rl.WHITE) + rl.draw_rectangle_rounded(bar, 1, 10, rl.WHITE) else: text_size = rl.measure_text_ex(gui_app.font(), text, FONT_SIZE, 1.0) rl.draw_text_ex(gui_app.font(), text, From 06a9483a24d9d5e9fa2b32222d77c89e9ea16148 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 17 Apr 2025 16:49:30 -0700 Subject: [PATCH 03/24] lagd: estimate std (#35009) * Std * Fix static * Refactor * Assert std zero --- cereal/log.capnp | 1 + selfdrive/locationd/lagd.py | 39 ++++++++++++++++++++------- selfdrive/locationd/test/test_lagd.py | 2 ++ 3 files changed, 33 insertions(+), 9 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index 797c2cd8ac..c624eb0052 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2284,6 +2284,7 @@ struct LiveDelayData { status @2 :Status; lateralDelayEstimate @3 :Float32; + lateralDelayEstimateStd @5 :Float32; points @4 :List(Float32); enum Status { diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index b52a658d01..0a7c71a7ea 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -24,6 +24,7 @@ MIN_ABS_YAW_RATE = np.radians(1.0) MAX_YAW_RATE_SANITY_CHECK = 1.0 MIN_NCC = 0.95 MAX_LAG = 1.0 +MAX_LAG_STD = 0.1 MAX_LAT_ACCEL = 2.0 MAX_LAT_ACCEL_DIFF = 0.6 @@ -127,13 +128,23 @@ class BlockAverage: self.block_idx = (self.block_idx + 1) % self.num_blocks self.valid_blocks = min(self.valid_blocks + 1, self.num_blocks) - def get(self) -> tuple[float, float]: + def get(self) -> tuple[float, float, float, float]: valid_block_idx = [i for i in range(self.valid_blocks) if i != self.block_idx] valid_and_current_idx = valid_block_idx + ([self.block_idx] if self.idx > 0 else []) - valid_mean = float(np.mean(self.values[valid_block_idx], axis=0).item()) if len(valid_block_idx) > 0 else float('nan') - current_mean = float(np.mean(self.values[valid_and_current_idx], axis=0).item()) if len(valid_and_current_idx) > 0 else float('nan') - return valid_mean, current_mean + if len(valid_block_idx) > 0: + valid_mean = float(np.mean(self.values[valid_block_idx], axis=0).item()) + valid_std = float(np.std(self.values[valid_block_idx], axis=0).item()) + else: + valid_mean, valid_std = float('nan'), float('nan') + + if len(valid_and_current_idx) > 0: + current_mean = float(np.mean(self.values[valid_and_current_idx], axis=0).item()) + current_std = float(np.std(self.values[valid_and_current_idx], axis=0).item()) + else: + current_mean, current_std = float('nan'), float('nan') + + return valid_mean, valid_std, current_mean, current_std class LateralLagEstimator: @@ -190,17 +201,27 @@ class LateralLagEstimator: liveDelay = msg.liveDelay - valid_mean_lag, current_mean_lag = self.block_avg.get() - if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag): - liveDelay.status = log.LiveDelayData.Status.estimated - liveDelay.lateralDelay = valid_mean_lag + valid_mean_lag, valid_std, current_mean_lag, current_std = self.block_avg.get() + if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std): + if valid_std > MAX_LAG_STD: + liveDelay.status = log.LiveDelayData.Status.invalid + else: + liveDelay.status = log.LiveDelayData.Status.estimated else: liveDelay.status = log.LiveDelayData.Status.unestimated + + if liveDelay.status == log.LiveDelayData.Status.estimated: + liveDelay.lateralDelay = valid_mean_lag + else: liveDelay.lateralDelay = self.initial_lag - if not np.isnan(current_mean_lag): + + if not np.isnan(current_mean_lag) and not np.isnan(current_std): liveDelay.lateralDelayEstimate = current_mean_lag + liveDelay.lateralDelayEstimateStd = current_std else: liveDelay.lateralDelayEstimate = self.initial_lag + liveDelay.lateralDelayEstimateStd = 0.0 + liveDelay.validBlocks = self.block_avg.valid_blocks if debug: liveDelay.points = self.block_avg.values.flatten().tolist() diff --git a/selfdrive/locationd/test/test_lagd.py b/selfdrive/locationd/test/test_lagd.py index bd5cfd6201..13aea60a26 100644 --- a/selfdrive/locationd/test/test_lagd.py +++ b/selfdrive/locationd/test/test_lagd.py @@ -113,6 +113,7 @@ class TestLagd: 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 def test_estimator_masking(self, mocker): @@ -121,6 +122,7 @@ class TestLagd: process_messages(mocker, 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) @pytest.mark.skipif(PC, reason="only on device") @pytest.mark.timeout(60) From a4cdc96a99be6a2da574d3327f7e76fbd6f63dd3 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 17 Apr 2025 16:58:00 -0700 Subject: [PATCH 04/24] update --- cereal/log.capnp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cereal/log.capnp b/cereal/log.capnp index c624eb0052..a5828d61cb 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1174,6 +1174,8 @@ struct ModelDataV2 { struct Action { desiredCurvature @0 :Float32; + desiredAcceleration @1 :Float32; + shouldStop @2 :Bool; } } From 4f913f0cfba188f95bc565e225c41f842168a982 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Fri, 18 Apr 2025 01:11:22 +0100 Subject: [PATCH 05/24] ui(raylib): fix typos (#35030) --- system/ui/lib/application.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/system/ui/lib/application.py b/system/ui/lib/application.py index fd1ad52068..470396bbc7 100644 --- a/system/ui/lib/application.py +++ b/system/ui/lib/application.py @@ -73,7 +73,7 @@ class GuiApplication: for font in self._fonts.values(): rl.unload_font(font) - self._fonts = [] + self._fonts = {} rl.close_window() @@ -90,8 +90,8 @@ class GuiApplication: rl.end_drawing() self._monitor_fps() - def font(self, font_wight: FontWeight=FontWeight.NORMAL): - return self._fonts[font_wight] + def font(self, font_weight: FontWeight=FontWeight.NORMAL): + return self._fonts[font_weight] @property def width(self): From b42ec33a6375e831f11ca88f5d92c1f0d14efa64 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Thu, 17 Apr 2025 17:59:15 -0700 Subject: [PATCH 06/24] Update modeld action logic (#35032) * add action logic * magic numbers * unused * plot accel * more changes --- selfdrive/controls/lib/drive_helpers.py | 18 +++++++- .../controls/lib/longitudinal_planner.py | 21 +--------- selfdrive/modeld/fill_model_msg.py | 9 ++-- selfdrive/modeld/modeld.py | 42 ++++++++++++++++--- selfdrive/test/process_replay/model_replay.py | 1 + 5 files changed, 62 insertions(+), 29 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 41384abae8..9958755577 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,7 +1,7 @@ import numpy as np from cereal import log from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY -from openpilot.common.realtime import DT_CTRL +from openpilot.common.realtime import DT_CTRL, DT_MDL MIN_SPEED = 1.0 CONTROL_N = 17 @@ -43,3 +43,19 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: vel_err = np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) return float(vel_err) return 0.0 + + +def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05): + if len(speeds) == len(t_idxs): + v_now = speeds[0] + a_now = accels[0] + v_target = np.interp(action_t, t_idxs, speeds) + a_target = 2 * (v_target - v_now) / (action_t) - a_now + v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds) + else: + v_target = 0.0 + v_target_1sec = 0.0 + a_target = 0.0 + should_stop = (v_target < vEgoStopping and + v_target_1sec < vEgoStopping) + return a_target, should_stop diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c7a2295a5f..8cbf0beee9 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -11,7 +11,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error, get_accel_from_plan from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog @@ -48,23 +48,6 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): return [a_target[0], min(a_target[1], a_x_allowed)] -def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05): - if len(speeds) == CONTROL_N: - v_now = speeds[0] - a_now = accels[0] - - v_target = np.interp(action_t, CONTROL_N_T_IDX, speeds) - a_target = 2 * (v_target - v_now) / (action_t) - a_now - v_target_1sec = np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds) - else: - v_target = 0.0 - v_target_1sec = 0.0 - a_target = 0.0 - should_stop = (v_target < vEgoStopping and - v_target_1sec < vEgoStopping) - return a_target, should_stop - - class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP @@ -176,7 +159,7 @@ class LongitudinalPlanner: self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 action_t = self.CP.longitudinalActuatorDelay + DT_MDL - output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, + output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, action_t=action_t, vEgoStopping=self.CP.vEgoStopping) for idx in range(2): diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 9dc2641c9b..a91c6395c7 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -56,7 +56,7 @@ def fill_lane_line_meta(builder, lane_lines, lane_line_probs): builder.rightProb = lane_line_probs[2] def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder, - net_output_data: dict[str, np.ndarray], v_ego: float, delay: float, + net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float, valid: bool) -> None: @@ -71,7 +71,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.frameIdExtra = vipc_frame_id_extra driving_model_data.frameDropPerc = frame_drop_perc driving_model_data.modelExecutionTime = model_execution_time - driving_model_data.action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + + driving_model_data.action = action modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -98,8 +99,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # poly path fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) - # lateral planning - modelV2.action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + # action + modelV2.action = action # times at X_IDXS of edges and lines aren't used LINE_T_IDXS: list[float] = [] diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 1e557942fe..f5007273ec 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -21,17 +21,20 @@ from opendbc.car.car_helpers import get_demo_car_params from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.realtime import config_realtime_process +from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix from openpilot.system import sentry from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState -from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.selfdrive.modeld.constants import ModelConstants, Plan from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext + + PROCESS_NAME = "selfdrive.modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -40,6 +43,30 @@ POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl' VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl' POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl' +LAT_SMOOTH_SECONDS = 0.0 +LONG_SMOOTH_SECONDS = 0.0 + +def smooth_value(val, prev_val, tau): + alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1 + return alpha * val + (1 - alpha) * prev_val + + +def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, + lat_action_t: float, long_action_t: float,) -> log.ModelDataV2.Action: + plan = model_output['plan'][0] + desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0], + plan[:,Plan.ACCELERATION][:,0], + ModelConstants.T_IDXS, + action_t=long_action_t) + desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) + + desired_curvature = model_output['desired_curvature'][0, 0] + desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS) + + return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), + desiredAcceleration=float(desired_accel), + shouldStop=bool(should_stop)) + class FrameMeta: frame_id: int = 0 timestamp_sof: int = 0 @@ -221,7 +248,10 @@ def main(demo=False): cloudlog.info("modeld got CarParams: %s", CP.brand) # TODO this needs more thought, use .2s extra for now to estimate other delays - steer_delay = CP.steerActuatorDelay + .2 + # TODO Move smooth seconds to action function + lat_delay = CP.steerActuatorDelay + .2 + LAT_SMOOTH_SECONDS + long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS + prev_action = log.ModelDataV2.Action() DH = DesireHelper() @@ -263,7 +293,7 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32) + lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32) if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] @@ -306,7 +336,9 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay, + + action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL) + fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen) diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 36398af8ca..af626e2da9 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -64,6 +64,7 @@ def generate_report(proposed, master, tmp, commit): ModelV2_Plots = zl([ (lambda x: get_idx_if_non_empty(x.velocity.x, 0), "velocity.x"), (lambda x: get_idx_if_non_empty(x.action.desiredCurvature), "desiredCurvature"), + (lambda x: get_idx_if_non_empty(x.action.desiredAcceleration), "desiredAcceleration"), (lambda x: get_idx_if_non_empty(x.leadsV3[0].x, 0), "leadsV3.x"), (lambda x: get_idx_if_non_empty(x.laneLines[1].y, 0), "laneLines.y"), (lambda x: get_idx_if_non_empty(x.meta.desireState, 3), "desireState.laneChangeLeft"), From 15326c2d301427ce525bce464d8d1ca84fd3c4af Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 17 Apr 2025 18:37:48 -0700 Subject: [PATCH 07/24] lagd: check for validity of the estimate when restoring state (#35034) Do not restore if invalid --- selfdrive/locationd/lagd.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 0a7c71a7ea..f4f46b7469 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -327,8 +327,9 @@ def retrieve_initial_lag(params_reader: Params, CP: car.CarParams): if last_CP.carFingerprint != CP.carFingerprint: raise Exception("Car model mismatch") - lag, valid_blocks = ld.lateralDelayEstimate, ld.validBlocks + lag, valid_blocks, status = ld.lateralDelayEstimate, ld.validBlocks, ld.status assert valid_blocks <= BLOCK_NUM, "Invalid number of valid blocks" + assert status != log.LiveDelayData.Status.invalid, "Lag estimate is invalid" return lag, valid_blocks except Exception as e: cloudlog.error(f"Failed to retrieve initial lag: {e}") From 65c210265b23ac6ca16a0f5532c880465aa41ab1 Mon Sep 17 00:00:00 2001 From: niko001 Date: Fri, 18 Apr 2025 05:55:32 +0200 Subject: [PATCH 08/24] Multilang: Update de translation (#35025) added missing German translations --- selfdrive/ui/translations/main_de.ts | 598 ++++++++++++++------------- 1 file changed, 301 insertions(+), 297 deletions(-) diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index 560e1df2cc..f279a8d9f1 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -3,14 +3,14 @@ AbstractAlert - + Close Schließen - - + + Snooze Update Update pausieren - + Reboot and Update Aktualisieren und neu starten @@ -66,26 +66,26 @@ Prevent large data uploads when on a metered connection Hochladen großer Dateien über getaktete Verbindungen unterbinden - - Hidden Network - - - - CONNECT - CONNECT - - - Enter SSID - SSID eingeben - - - Enter password - Passwort eingeben - - - for "%1" - für "%1" - + + Hidden Network + Verborgenes Netzwerk + + + CONNECT + VERBINDEN + + + Enter SSID + SSID eingeben + + + Enter password + Passwort eingeben + + + for "%1" + für "%1" + ConfirmationDialog @@ -115,34 +115,34 @@ DeveloperPanel - - Joystick Debug Mode - - - - Longitudinal Maneuver Mode - - - - openpilot Longitudinal Control (Alpha) - - - - WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB). - - - - On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. - - - - Enable ADB - - - - ADB (Android Debug Bridge) allows connecting to your device over USB or over the network. See https://docs.comma.ai/how-to/connect-to-comma for more info. - - + + Joystick Debug Mode + Joystick Debug-Modus + + + Longitudinal Maneuver Mode + Längsmanöver-Modus + + + openpilot Longitudinal Control (Alpha) + openpilot Längsregelung (Alpha) + + + WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB). + WARNUNG: Die openpilot Längsregelung befindet sich für dieses Fahrzeug im Alpha-Stadium und deaktiviert das automatische Notbremsen (AEB). + + + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. + Bei diesem Fahrzeug verwendet openpilot standardmäßig den eingebauten Tempomaten anstelle der openpilot Längsregelung. Aktiviere diese Option, um auf die openpilot Längsregelung umzuschalten. Es wird empfohlen, den experimentellen Modus zu aktivieren, wenn die openpilot Längsregelung (Alpha) aktiviert wird. + + + Enable ADB + ADB aktivieren + + + ADB (Android Debug Bridge) allows connecting to your device over USB or over the network. See https://docs.comma.ai/how-to/connect-to-comma for more info. + ADB (Android Debug Bridge) ermöglicht die Verbindung zu deinem Gerät über USB oder Netzwerk. Siehe https://docs.comma.ai/how-to/connect-to-comma für weitere Informationen. + DevicePanel @@ -278,14 +278,14 @@ Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. Koppele dein Gerät mit Comma Connect (connect.comma.ai) und sichere dir dein Comma Prime Angebot. - - Pair Device - - - - PAIR - - + + Pair Device + Gerät koppeln + + + PAIR + KOPPELN + DriverViewWindow @@ -307,39 +307,41 @@ FirehosePanel - - 🔥 Firehose Mode 🔥 - - - - openpilot learns to drive by watching humans, like you, drive. + + 🔥 Firehose Mode 🔥 + 🔥 Firehose-Modus 🔥 + + + openpilot learns to drive by watching humans, like you, drive. Firehose Mode allows you to maximize your training data uploads to improve openpilot's driving models. More data means bigger models, which means better Experimental Mode. - - - - Firehose Mode: ACTIVE - - - - ACTIVE - - - - For maximum effectiveness, bring your device inside and connect to a good USB-C adapter and Wi-Fi weekly.<br><br>Firehose Mode can also work while you're driving if connected to a hotspot or unlimited SIM card.<br><br><br><b>Frequently Asked Questions</b><br><br><i>Does it matter how or where I drive?</i> Nope, just drive as you normally would.<br><br><i>Do all of my segments get pulled in Firehose Mode?</i> No, we selectively pull a subset of your segments.<br><br><i>What's a good USB-C adapter?</i> Any fast phone or laptop charger should be fine.<br><br><i>Does it matter which software I run?</i> Yes, only upstream openpilot (and particular forks) are able to be used for training. - - - - <b>%n segment(s)</b> of your driving is in the training dataset so far. - - - - - - - <span stylesheet='font-size: 60px; font-weight: bold; color: #e74c3c;'>INACTIVE</span>: connect to an unmetered network - - + openpilot lernt das Fahren, indem es Menschen wie dir beim Fahren zuschaut. + +Der Firehose-Modus ermöglicht es dir, deine Trainingsdaten-Uploads zu maximieren, um die Fahrmodelle von openpilot zu verbessern. Mehr Daten bedeuten größere Modelle, was zu einem besseren Experimentellen Modus führt. + + + Firehose Mode: ACTIVE + Firehose-Modus: AKTIV + + + ACTIVE + AKTIV + + + For maximum effectiveness, bring your device inside and connect to a good USB-C adapter and Wi-Fi weekly.<br><br>Firehose Mode can also work while you're driving if connected to a hotspot or unlimited SIM card.<br><br><br><b>Frequently Asked Questions</b><br><br><i>Does it matter how or where I drive?</i> Nope, just drive as you normally would.<br><br><i>Do all of my segments get pulled in Firehose Mode?</i> No, we selectively pull a subset of your segments.<br><br><i>What's a good USB-C adapter?</i> Any fast phone or laptop charger should be fine.<br><br><i>Does it matter which software I run?</i> Yes, only upstream openpilot (and particular forks) are able to be used for training. + Für maximale Effektivität bring dein Gerät jede Woche nach drinnen und verbinde es mit einem guten USB-C-Adapter und WLAN.<br><br>Der Firehose-Modus funktioniert auch während der Fahrt, wenn das Gerät mit einem Hotspot oder einer ungedrosselten SIM-Karte verbunden ist.<br><br><br><b>Häufig gestellte Fragen</b><br><br><i>Spielt es eine Rolle, wie oder wo ich fahre?</i> Nein, fahre einfach wie gewohnt.<br><br><i>Werden im Firehose-Modus alle meine Segmente hochgeladen?</i> Nein, wir wählen selektiv nur einen Teil deiner Segmente aus.<br><br><i>Welcher USB-C-Adapter ist gut?</i> Jedes Schnellladegerät für Handy oder Laptop sollte ausreichen.<br><br><i>Spielt es eine Rolle, welche Software ich nutze?</i> Ja, nur das offizielle Upstream‑openpilot (und bestimmte Forks) kann für das Training verwendet werden. + + + <b>%n segment(s)</b> of your driving is in the training dataset so far. + + <b>%n Segment</b> deiner Fahrten ist bisher im Trainingsdatensatz. + <b>%n Segmente</b> deiner Fahrten sind bisher im Trainingsdatensatz. + + + + <span stylesheet='font-size: 60px; font-weight: bold; color: #e74c3c;'>INACTIVE</span>: connect to an unmetered network + <span stylesheet='font-size: 60px; font-weight: bold; color: #e74c3c;'>INAKTIV</span>: Verbinde dich mit einem ungedrosselten Netzwerk + HudRenderer @@ -411,49 +413,50 @@ Firehose Mode allows you to maximize your training data uploads to improve openp OffroadAlert Immediately connect to the internet to check for updates. If you do not connect to the internet, openpilot won't engage in %1 - + Stelle sofort eine Internetverbindung her, um nach Updates zu suchen. Wenn du keine Verbindung herstellst, kann openpilot in %1 nicht mehr aktiviert werden. Connect to internet to check for updates. openpilot won't automatically start until it connects to internet to check for updates. - + Verbinde dich mit dem Internet, um nach Updates zu suchen. openpilot startet nicht automatisch, bis eine Internetverbindung besteht und nach Updates gesucht wurde. - - Unable to download updates + + Unable to download updates %1 - - - - Taking camera snapshots. System won't start until finished. - - - - An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. - - - - Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, visit https://comma.ai/support. - - - - NVMe drive not mounted. - - - - Unsupported NVMe drive detected. Device may draw significantly more power and overheat due to the unsupported NVMe. - - - - openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai. - - - - openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield. - - - - Device temperature too high. System cooling down before starting. Current internal component temperature: %1 - - + Updates konnten nicht heruntergeladen werden +%1 + + + Taking camera snapshots. System won't start until finished. + Kamera-Snapshots werden aufgenommen. Das System startet erst, wenn dies abgeschlossen ist. + + + An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. + Ein Update für das Betriebssystem deines Geräts wird im Hintergrund heruntergeladen. Du wirst aufgefordert, das Update zu installieren, sobald es bereit ist. + + + Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, visit https://comma.ai/support. + Gerät konnte nicht registriert werden. Es wird keine Verbindung zu den comma.ai-Servern herstellen oder Daten hochladen und erhält keinen Support von comma.ai. Wenn dies ein offizielles Gerät ist, besuche https://comma.ai/support. + + + NVMe drive not mounted. + NVMe-Laufwerk nicht gemounted. + + + Unsupported NVMe drive detected. Device may draw significantly more power and overheat due to the unsupported NVMe. + Nicht unterstütztes NVMe-Laufwerk erkannt. Das Gerät kann dadurch deutlich mehr Strom verbrauchen und überhitzen. + + + openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai. + openpilot konnte dein Auto nicht identifizieren. Dein Auto wird entweder nicht unterstützt oder die Steuergeräte (ECUs) werden nicht erkannt. Bitte reiche einen Pull Request ein, um die Firmware-Versionen für das richtige Fahrzeug hinzuzufügen. Hilfe findest du auf discord.comma.ai. + + + openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield. + openpilot hat eine Änderung der Montageposition des Geräts erkannt. Stelle sicher, dass das Gerät vollständig in der Halterung sitzt und die Halterung fest an der Windschutzscheibe befestigt ist. + + + Device temperature too high. System cooling down before starting. Current internal component temperature: %1 + Gerätetemperatur zu hoch. Das System kühlt ab, bevor es startet. Aktuelle interne Komponententemperatur: %1 + OffroadHome @@ -472,26 +475,26 @@ Firehose Mode allows you to maximize your training data uploads to improve openp OnroadAlerts - - openpilot Unavailable - - - - TAKE CONTROL IMMEDIATELY - - - - Reboot Device - - - - Waiting to start - - - - System Unresponsive - - + + openpilot Unavailable + openpilot nicht verfügbar + + + TAKE CONTROL IMMEDIATELY + ÜBERNIMM SOFORT DIE KONTROLLE + + + Reboot Device + Gerät neu starten + + + Waiting to start + Warten auf Start + + + System Unresponsive + System reagiert nicht + PairingPopup @@ -511,10 +514,10 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Bookmark connect.comma.ai to your home screen to use it like an app Füge connect.comma.ai als Lesezeichen auf deinem Homescreen hinzu um es wie eine App zu verwenden - - Please connect to Wi-Fi to complete initial pairing - - + + Please connect to Wi-Fi to complete initial pairing + Bitte verbinde dich mit WLAN, um die Koppelung abzuschließen. + ParamControl @@ -545,18 +548,18 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Remote access Fernzugriff - - 24/7 LTE connectivity - - - - 1 year of drive storage - - - - Remote snapshots - - + + 24/7 LTE connectivity + 24/7 LTE-Verbindung + + + 1 year of drive storage + Fahrdaten-Speicherung für 1 Jahr + + + Remote snapshots + Remote-Snapshots + PrimeUserWidget @@ -604,10 +607,10 @@ Firehose Mode allows you to maximize your training data uploads to improve openp vor %n Tagen - - now - - + + now + jetzt + Reset @@ -635,19 +638,20 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Confirm Bestätigen - - Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. - - - - Resetting device... + + Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. + Datenpartition konnte nicht gemounted werden. Die Partition ist möglicherweise beschädigt. Drücke Bestätigen, um das Gerät zu löschen und zurückzusetzen. + + + Resetting device... This may take up to a minute. - - - - System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - - + Gerät wird zurückgesetzt... +Dies kann bis zu einer Minute dauern. + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + System-Reset ausgelöst. Drücke Bestätigen, um alle Inhalte und Einstellungen zu löschen. Drücke Abbrechen, um den Startvorgang fortzusetzen. + SettingsWindow @@ -671,14 +675,14 @@ This may take up to a minute. Software Software - - Developer - - - - Firehose - - + + Developer + Entwickler + + + Firehose + Firehose + Setup @@ -750,30 +754,30 @@ This may take up to a minute. Start over Von neuem beginnen - - No custom software found at this URL. - - - - Something went wrong. Reboot the device. - - + + No custom software found at this URL. + Keine benutzerdefinierte Software unter dieser URL gefunden. + + + Something went wrong. Reboot the device. + Etwas ist schiefgelaufen. Starte das Gerät neu. + Select a language Sprache wählen - - Choose Software to Install - - - - openpilot - openpilot - - - Custom Software - - + + Choose Software to Install + Wähle die zu installierende Software + + + openpilot + openpilot + + + Custom Software + Benutzerdefinierte Software + SetupWidget @@ -921,26 +925,26 @@ This may take up to a minute. Uninstall Deinstallieren - - failed to check for update - - - - up to date, last checked %1 - - - - DOWNLOAD - - - - update available - - - - never - - + + failed to check for update + Update-Prüfung fehlgeschlagen + + + up to date, last checked %1 + Auf dem neuesten Stand, zuletzt geprüft am %1 + + + DOWNLOAD + HERUNTERLADEN + + + update available + Update verfügbar + + + never + nie + SshControl @@ -998,14 +1002,14 @@ This may take up to a minute. Agree Zustimmen - - Welcome to openpilot - - - - You must accept the Terms and Conditions to use openpilot. Read the latest terms at <span style='color: #465BEA;'>https://comma.ai/terms</span> before continuing. - - + + Welcome to openpilot + Willkommen bei openpilot + + + You must accept the Terms and Conditions to use openpilot. Read the latest terms at <span style='color: #465BEA;'>https://comma.ai/terms</span> before continuing. + Du musst die Nutzungsbedingungen akzeptieren, um openpilot zu verwenden. Lies die aktuellen Bedingungen unter <span style='color: #465BEA;'>https://comma.ai/terms</span>, bevor du fortfährst. + TogglesPanel @@ -1069,54 +1073,54 @@ This may take up to a minute. Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. Der experimentelle Modus ist momentan für dieses Auto nicht verfügbar da es den eingebauten adaptiven Tempomaten des Autos benutzt. - - Aggressive - - - - Standard - - - - Relaxed - - - - Driving Personality - - - - End-to-End Longitudinal Control - - - - openpilot longitudinal control may come in a future update. - - - - An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. - - - - Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. - - - - Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. - - - - The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. - - - - Always-On Driver Monitoring - - - - Enable driver monitoring even when openpilot is not engaged. - - + + Aggressive + Aggressiv + + + Standard + Standard + + + Relaxed + Entspannt + + + Driving Personality + Fahrstil + + + End-to-End Longitudinal Control + Ende-zu-Ende Längsregelung + + + openpilot longitudinal control may come in a future update. + Die openpilot Längsregelung könnte in einem zukünftigen Update verfügbar sein. + + + An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + Eine Alpha-Version der openpilot Längsregelung kann zusammen mit dem Experimentellen Modus auf non-stable Branches getestet werden. + + + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. + Aktiviere den Schalter für openpilot Längsregelung (Alpha), um den Experimentellen Modus zu erlauben. + + + Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. + Standard wird empfohlen. Im aggressiven Modus folgt openpilot vorausfahrenden Fahrzeugen enger und ist beim Gasgeben und Bremsen aggressiver. Im entspannten Modus hält openpilot mehr Abstand zu vorausfahrenden Fahrzeugen. Bei unterstützten Fahrzeugen kannst du mit der Abstandstaste am Lenkrad zwischen diesen Fahrstilen wechseln. + + + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + Die Fahrvisualisierung wechselt bei niedrigen Geschwindigkeiten auf die nach vorne gerichtete Weitwinkelkamera, um Kurven besser darzustellen. Das Logo des Experimentellen Modus wird außerdem oben rechts angezeigt. + + + Always-On Driver Monitoring + Dauerhaft aktive Fahrerüberwachung + + + Enable driver monitoring even when openpilot is not engaged. + Fahrerüberwachung auch aktivieren, wenn openpilot nicht aktiv ist. + Updater @@ -1155,18 +1159,18 @@ This may take up to a minute. WiFiPromptWidget - - Open - - - - Maximize your training data uploads to improve openpilot's driving models. - - - - <span style='font-family: "Noto Color Emoji";'>🔥</span> Firehose Mode <span style='font-family: Noto Color Emoji;'>🔥</span> - - + + Open + Öffnen + + + Maximize your training data uploads to improve openpilot's driving models. + Maximiere deine Trainingsdaten-Uploads, um die Fahrmodelle von openpilot zu verbessern. + + + <span style='font-family: "Noto Color Emoji";'>🔥</span> Firehose Mode <span style='font-family: Noto Color Emoji;'>🔥</span> + <span style='font-family: "Noto Color Emoji";'>🔥</span> Firehose-Modus <span style='font-family: Noto Color Emoji;'>🔥</span> + WifiUI From 2c162d9b75ae4e77bdf5bc3225e880d352ec0f5c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Thu, 17 Apr 2025 23:21:25 -0700 Subject: [PATCH 09/24] Tomb raider 2 (#35029) * db56b8fb-9135-4ab6-af18-99b7df7b2245/400 * fixes * linter unhappy * 6dbe0991-baa1-49ad-836a-ab370d1f0d92/400 * This one is good: 19387087-1005-475e-9015-9458dd8e7c5f/400 * Better every day: 39ed911c-0937-417f-97d2-58a8bb3caa53/400 * Actually end-to-end * typo * smooooooth: 94e23541-eb84-4fef-9f51-6a2d82aff314/360 * Revert "smooooooth: 94e23541-eb84-4fef-9f51-6a2d82aff314/360" This reverts commit edd4f02386d83d82dd8a188985cde80ed1646b7f. * 11632ef7-f555-489c-8480-e3bf97d9285e/400 * 08712d27-f6bd-4536-a30e-c729e5f62356/400 * 0a92a35e-1f72-476a-8cb6-c9f103f36822/400 * ee6d2394-2072-420c-a664-b4c0d4ed0b61/400 * no prev curv * No double work * fix bug * smooth * update prev action * whitespace * add little accel * new ref * Update plant.py --- .../controls/lib/longitudinal_planner.py | 20 ++++++++++++++----- selfdrive/modeld/fill_model_msg.py | 10 +++++----- selfdrive/modeld/modeld.py | 7 ++++--- selfdrive/modeld/models/driving_policy.onnx | 4 ++-- selfdrive/modeld/models/driving_vision.onnx | 4 ++-- selfdrive/modeld/parse_model_outputs.py | 13 ++++++------ .../test/longitudinal_maneuvers/plant.py | 1 + selfdrive/test/process_replay/ref_commit | 2 +- 8 files changed, 36 insertions(+), 25 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 8cbf0beee9..104c7c9d49 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -89,14 +89,15 @@ class LongitudinalPlanner: return x, v, a, j, throttle_prob def update(self, sm): - self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' + self.mpc.mode = 'acc' + self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) else: accel_coast = ACCEL_MAX - v_ego = sm['carState'].vEgo + v_ego = sm['modelV2'].velocity.x[0] v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET @@ -112,7 +113,7 @@ class LongitudinalPlanner: # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) - if self.mpc.mode == 'acc': + if self.mode == 'acc': accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) @@ -128,7 +129,7 @@ class LongitudinalPlanner: self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) # Compute model v_ego error self.v_model_error = get_speed_error(sm['modelV2'], v_ego) - x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error) + x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], 0) # Don't clip at low speeds since throttle_prob doesn't account for creep self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED @@ -159,8 +160,17 @@ class LongitudinalPlanner: self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 action_t = self.CP.longitudinalActuatorDelay + DT_MDL - output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, + output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + output_a_target_e2e = sm['modelV2'].action.desiredAcceleration + output_should_stop_e2e = sm['modelV2'].action.shouldStop + + if self.mode == 'acc': + output_a_target = output_a_target_mpc + self.output_should_stop = output_should_stop_mpc + else: + output_a_target = min(output_a_target_mpc, output_a_target_e2e) + self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc for idx in range(2): accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index a91c6395c7..36bd724d01 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -90,11 +90,11 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T) # temporal pose - temporal_pose = modelV2.temporalPose - temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist() - temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist() - temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist() - temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist() + #temporal_pose = modelV2.temporalPose + #temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist() + #temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist() + #temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist() + #temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist() # poly path fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index f5007273ec..0ad629f519 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -43,8 +43,8 @@ POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl' VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl' POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl' -LAT_SMOOTH_SECONDS = 0.0 -LONG_SMOOTH_SECONDS = 0.0 +LAT_SMOOTH_SECONDS = 0.3 +LONG_SMOOTH_SECONDS = 0.3 def smooth_value(val, prev_val, tau): alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1 @@ -174,7 +174,7 @@ class ModelState: # TODO model only uses last value now self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:] self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :] - self.numpy_inputs['prev_desired_curv'][:] = self.full_prev_desired_curv[0, self.temporal_idxs] + self.numpy_inputs['prev_desired_curv'][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs] combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict} if SEND_RAW_PRED: @@ -338,6 +338,7 @@ def main(demo=False): posenet_send = messaging.new_message('cameraOdometry') action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL) + prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen) diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index 3601bbb5da..3bf2e01b5e 100644 --- a/selfdrive/modeld/models/driving_policy.onnx +++ b/selfdrive/modeld/models/driving_policy.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:98f0121ccb6f850077b04cc91bd33d370fc6cbdc2bd35f1ab55628a15a813f36 -size 15966721 +oid sha256:fa00a07a4307b0f1638a10ec33b651d8a102e38371289b744f43215c89dfd342 +size 15578328 diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx index aee6d8f1bf..5e08b78472 100644 --- a/selfdrive/modeld/models/driving_vision.onnx +++ b/selfdrive/modeld/models/driving_vision.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:897f80d0388250f99bba69b6a8434560cc0fd83157cbeb0bc134c67fe4e64624 -size 34882971 +oid sha256:f222d2c528f1763828de01bb55e8979b1e4056e1dbb41350f521d2d2bb09d177 +size 46265585 diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py index 810c44ccb9..783572d436 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/selfdrive/modeld/parse_model_outputs.py @@ -88,6 +88,12 @@ class Parser: self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,)) self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) + self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) + self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) + self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION, + out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH)) + for k in ['lead_prob', 'lane_lines_prob']: + self.parse_binary_crossentropy(k, outs) self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH)) self.parse_binary_crossentropy('meta', outs) return outs @@ -95,17 +101,10 @@ class Parser: def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION, out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH)) - self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) - self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) - self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) - self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION, - out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH)) if 'lat_planner_solution' in outs: self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH)) if 'desired_curvature' in outs: self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,)) - for k in ['lead_prob', 'lane_lines_prob']: - self.parse_binary_crossentropy(k, outs) self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,)) return outs diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 989b84dee3..b8c6adb436 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -107,6 +107,7 @@ class Plant: position = log.XYZTData.new_message() position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)] model.modelV2.position = position + model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.1) velocity = log.XYZTData.new_message() velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] velocity.x[0] = float(self.speed) # always start at current speed diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 632922a8a4..5f89d7802d 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -3d3f875dc211ab0ca8a8e564ecf6dd3f52fcf7d9 \ No newline at end of file +0800e73b5d9c801f161b9559557c0559b0682fec \ No newline at end of file From 3b60b22ceeb090812e5fa948ac6e56442d681dc4 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Fri, 18 Apr 2025 14:29:37 -0700 Subject: [PATCH 10/24] OS04C10: use IFE downscaler for road cameras (#35023) * squashh * wrong * clean up * rename --- system/camerad/cameras/camera_common.cc | 3 -- system/camerad/cameras/camera_common.h | 2 +- system/camerad/cameras/camera_qcom2.cc | 10 ++-- system/camerad/cameras/ife.h | 18 ++++---- system/camerad/cameras/spectra.cc | 40 +++++++++------- system/camerad/sensors/os04c10.cc | 11 +++++ system/camerad/sensors/os04c10_registers.h | 54 ++++++++++++++-------- system/camerad/sensors/sensor.h | 2 + 8 files changed, 87 insertions(+), 53 deletions(-) diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index f6c2681dea..1f6ad9b4be 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -26,9 +26,6 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, SpectraCamera * LOGD("allocated %d CL buffers", frame_buf_count); } - out_img_width = sensor->frame_width; - out_img_height = sensor->hdr_offset > 0 ? (sensor->frame_height - sensor->hdr_offset) / 2 : sensor->frame_height; - // the encoder HW tells us the size it wants after setting it up. // TODO: VENUS_BUFFER_SIZE should give the size, but it's too small. dependent on encoder settings? size_t nv12_size = (out_img_width <= 1344 ? 2900 : 2346)*cam->stride; diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index c0426678b6..c26859cbc4 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -32,7 +32,7 @@ public: VisionBuf *cur_yuv_buf; VisionBuf *cur_camera_buf; std::unique_ptr camera_bufs_raw; - int out_img_width, out_img_height; + uint32_t out_img_width, out_img_height; CameraBuf() = default; ~CameraBuf(); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 18a42f05d8..8c4602bb31 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -73,7 +73,7 @@ void CameraState::init(VisionIpcServer *v, cl_device_id device_id, cl_context ct if (!camera.enabled) return; - fl_pix = camera.cc.focal_len / camera.sensor->pixel_size_mm; + fl_pix = camera.cc.focal_len / camera.sensor->pixel_size_mm / camera.sensor->out_scale; set_exposure_rect(); dc_gain_weight = camera.sensor->dc_gain_min_weight; @@ -107,10 +107,10 @@ void CameraState::set_exposure_rect() { float fl_ref = ae_target.second; ae_xywh = (Rect){ - std::max(0, camera.buf.out_img_width / 2 - (int)(fl_pix / fl_ref * xywh_ref.w / 2)), - std::max(0, camera.buf.out_img_height / 2 - (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))), - std::min((int)(fl_pix / fl_ref * xywh_ref.w), camera.buf.out_img_width / 2 + (int)(fl_pix / fl_ref * xywh_ref.w / 2)), - std::min((int)(fl_pix / fl_ref * xywh_ref.h), camera.buf.out_img_height / 2 + (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))) + std::max(0, (int)camera.buf.out_img_width / 2 - (int)(fl_pix / fl_ref * xywh_ref.w / 2)), + std::max(0, (int)camera.buf.out_img_height / 2 - (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))), + std::min((int)(fl_pix / fl_ref * xywh_ref.w), (int)camera.buf.out_img_width / 2 + (int)(fl_pix / fl_ref * xywh_ref.w / 2)), + std::min((int)(fl_pix / fl_ref * xywh_ref.h), (int)camera.buf.out_img_height / 2 + (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))) }; } diff --git a/system/camerad/cameras/ife.h b/system/camerad/cameras/ife.h index 49737f2db7..fd87d2baa4 100644 --- a/system/camerad/cameras/ife.h +++ b/system/camerad/cameras/ife.h @@ -105,7 +105,7 @@ int build_update(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std: } -int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector &patches) { +int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector &patches, uint32_t out_width, uint32_t out_height) { uint8_t *start = dst; // start with the every frame config @@ -185,12 +185,12 @@ int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo // output size/scaling dst += write_cont(dst, 0xa3c, { 0x00000003, - ((s->frame_width - 1) << 16) | (s->frame_width - 1), + ((out_width - 1) << 16) | (s->frame_width - 1), 0x30036666, 0x00000000, 0x00000000, s->frame_width - 1, - ((s->frame_height - 1) << 16) | (s->frame_height - 1), + ((out_height - 1) << 16) | (s->frame_height - 1), 0x30036666, 0x00000000, 0x00000000, @@ -198,12 +198,12 @@ int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo }); dst += write_cont(dst, 0xa68, { 0x00000003, - ((s->frame_width/2 - 1) << 16) | (s->frame_width - 1), + ((out_width / 2 - 1) << 16) | (s->frame_width - 1), 0x3006cccc, 0x00000000, 0x00000000, s->frame_width - 1, - ((s->frame_height/2 - 1) << 16) | (s->frame_height - 1), + ((out_height / 2 - 1) << 16) | (s->frame_height - 1), 0x3006cccc, 0x00000000, 0x00000000, @@ -212,12 +212,12 @@ int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo // cropping dst += write_cont(dst, 0xe10, { - s->frame_height - 1, - s->frame_width - 1, + out_height - 1, + out_width - 1, }); dst += write_cont(dst, 0xe30, { - s->frame_height/2 - 1, - s->frame_width - 1, + out_height / 2 - 1, + out_width - 1, }); dst += write_cont(dst, 0xe18, { 0x0ff00000, diff --git a/system/camerad/cameras/spectra.cc b/system/camerad/cameras/spectra.cc index 4535078771..47ae9061f4 100644 --- a/system/camerad/cameras/spectra.cc +++ b/system/camerad/cameras/spectra.cc @@ -281,18 +281,21 @@ void SpectraCamera::camera_open(VisionIpcServer *v, cl_device_id device_id, cl_c if (!enabled) return; + buf.out_img_width = sensor->frame_width / sensor->out_scale; + buf.out_img_height = (sensor->hdr_offset > 0 ? (sensor->frame_height - sensor->hdr_offset) / 2 : sensor->frame_height) / sensor->out_scale; + // size is driven by all the HW that handles frames, // the video encoder has certain alignment requirements in this case - stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, sensor->frame_width); - y_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, sensor->frame_height); - uv_height = VENUS_UV_SCANLINES(COLOR_FMT_NV12, sensor->frame_height); + stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, buf.out_img_width); + y_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, buf.out_img_height); + uv_height = VENUS_UV_SCANLINES(COLOR_FMT_NV12, buf.out_img_height); uv_offset = stride*y_height; yuv_size = uv_offset + stride*uv_height; if (cc.output_type != ISP_RAW_OUTPUT) { uv_offset = ALIGNED_SIZE(uv_offset, 0x1000); yuv_size = uv_offset + ALIGNED_SIZE(stride*uv_height, 0x1000); } - assert(stride == VENUS_UV_STRIDE(COLOR_FMT_NV12, sensor->frame_width)); + assert(stride == VENUS_UV_STRIDE(COLOR_FMT_NV12, buf.out_img_width)); assert(y_height/2 == uv_height); open = true; @@ -645,14 +648,14 @@ void SpectraCamera::config_bps(int idx, int request_id) { io_cfg[1].mem_handle[0] = buf_handle_yuv[idx]; io_cfg[1].mem_handle[1] = buf_handle_yuv[idx]; io_cfg[1].planes[0] = (struct cam_plane_cfg){ - .width = sensor->frame_width, - .height = sensor->frame_height, + .width = buf.out_img_width, + .height = buf.out_img_height, .plane_stride = stride, .slice_height = y_height, }; io_cfg[1].planes[1] = (struct cam_plane_cfg){ - .width = sensor->frame_width, - .height = sensor->frame_height/2, + .width = buf.out_img_width, + .height = buf.out_img_height / 2, .plane_stride = stride, .slice_height = uv_height, }; @@ -737,7 +740,7 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) { bool is_raw = cc.output_type != ISP_IFE_PROCESSED; if (!is_raw) { if (init) { - buf_desc[0].length = build_initial_config((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches); + buf_desc[0].length = build_initial_config((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches, buf.out_img_width, buf.out_img_height); } else { buf_desc[0].length = build_update((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches); } @@ -844,14 +847,14 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) { io_cfg[0].mem_handle[0] = buf_handle_yuv[idx]; io_cfg[0].mem_handle[1] = buf_handle_yuv[idx]; io_cfg[0].planes[0] = (struct cam_plane_cfg){ - .width = sensor->frame_width, - .height = sensor->frame_height, + .width = buf.out_img_width, + .height = buf.out_img_height, .plane_stride = stride, .slice_height = y_height, }; io_cfg[0].planes[1] = (struct cam_plane_cfg){ - .width = sensor->frame_width, - .height = sensor->frame_height/2, + .width = buf.out_img_width, + .height = buf.out_img_height / 2, .plane_stride = stride, .slice_height = uv_height, }; @@ -993,6 +996,9 @@ bool SpectraCamera::openSensor() { LOGD("-- Probing sensor %d", cc.camera_num); auto init_sensor_lambda = [this](SensorInfo *s) { + if (s->image_sensor == cereal::FrameData::ImageSensor::OS04C10 && cc.output_type == ISP_IFE_PROCESSED) { + ((OS04C10*)s)->ife_downscale_configure(); + } sensor.reset(s); return (sensors_init() == 0); }; @@ -1065,8 +1071,8 @@ void SpectraCamera::configISP() { .data[0] = (struct cam_isp_out_port_info){ .res_type = CAM_ISP_IFE_OUT_RES_FULL, .format = CAM_FORMAT_NV12, - .width = sensor->frame_width, - .height = sensor->frame_height + sensor->extra_height, + .width = buf.out_img_width, + .height = buf.out_img_height + sensor->extra_height, .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, }, }; @@ -1141,8 +1147,8 @@ void SpectraCamera::configICP() { }, .out_res[0] = (struct cam_icp_res_info){ .format = 0x3, // YUV420NV12 - .width = sensor->frame_width, - .height = sensor->frame_height, + .width = buf.out_img_width, + .height = buf.out_img_height, .fps = 20, }, }; diff --git a/system/camerad/sensors/os04c10.cc b/system/camerad/sensors/os04c10.cc index d008e1d07b..38be4ecca4 100644 --- a/system/camerad/sensors/os04c10.cc +++ b/system/camerad/sensors/os04c10.cc @@ -20,6 +20,17 @@ const uint32_t os04c10_analog_gains_reg[] = { } // namespace +void OS04C10::ife_downscale_configure() { + out_scale = 2; + + pixel_size_mm = 0.002; + frame_width = 2688; + frame_height = 1520; + exposure_time_max = 2352; + + init_reg_array.insert(init_reg_array.end(), std::begin(ife_downscale_override_array_os04c10), std::end(ife_downscale_override_array_os04c10)); +} + OS04C10::OS04C10() { image_sensor = cereal::FrameData::ImageSensor::OS04C10; bayer_pattern = CAM_ISP_PATTERN_BAYER_BGBGBG; diff --git a/system/camerad/sensors/os04c10_registers.h b/system/camerad/sensors/os04c10_registers.h index 8b1c78c69d..b501656090 100644 --- a/system/camerad/sensors/os04c10_registers.h +++ b/system/camerad/sensors/os04c10_registers.h @@ -88,8 +88,6 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x37c7, 0xa8}, {0x37da, 0x11}, {0x381f, 0x08}, - // {0x3829, 0x03}, - // {0x3832, 0x00}, {0x3881, 0x00}, {0x3888, 0x04}, {0x388b, 0x00}, @@ -199,7 +197,6 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x370b, 0x48}, {0x370c, 0x01}, {0x370f, 0x00}, - {0x3714, 0x28}, {0x3716, 0x04}, {0x3719, 0x11}, {0x371a, 0x1e}, @@ -231,7 +228,6 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x37bd, 0x01}, {0x37bf, 0x26}, {0x37c0, 0x11}, - {0x37c2, 0x14}, {0x37cd, 0x19}, {0x37e0, 0x08}, {0x37e6, 0x04}, @@ -241,14 +237,9 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x37d8, 0x02}, {0x37e2, 0x10}, {0x3739, 0x10}, - {0x3662, 0x08}, {0x37e4, 0x20}, {0x37e3, 0x08}, - {0x37d9, 0x04}, {0x4040, 0x00}, - {0x4041, 0x03}, - {0x4008, 0x01}, - {0x4009, 0x06}, // FSIN {0x3002, 0x22}, @@ -269,20 +260,11 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x3802, 0x00}, {0x3803, 0x00}, {0x3804, 0x0a}, {0x3805, 0x8f}, {0x3806, 0x05}, {0x3807, 0xff}, - {0x3808, 0x05}, {0x3809, 0x40}, - {0x380a, 0x02}, {0x380b, 0xf8}, {0x3811, 0x08}, {0x3813, 0x08}, - {0x3814, 0x03}, {0x3815, 0x01}, - {0x3816, 0x03}, {0x3817, 0x01}, - {0x380c, 0x0b}, {0x380d, 0xac}, // HTS - {0x380e, 0x06}, {0x380f, 0x9c}, // VTS - - {0x3820, 0xb3}, - {0x3821, 0x01}, {0x3880, 0x00}, {0x3882, 0x20}, {0x3c91, 0x0b}, @@ -331,4 +313,40 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { // r {0x5104, 0x08}, {0x5105, 0xd6}, {0x5144, 0x08}, {0x5145, 0xd6}, + + {0x3714, 0x28}, + {0x37c2, 0x14}, + {0x3662, 0x08}, + {0x37d9, 0x04}, + {0x4041, 0x03}, + {0x4008, 0x01}, + {0x4009, 0x06}, + {0x3808, 0x05}, {0x3809, 0x40}, + {0x380a, 0x02}, {0x380b, 0xf8}, + {0x3814, 0x03}, + {0x3816, 0x03}, + {0x380c, 0x0b}, {0x380d, 0xac}, // HTS + {0x380e, 0x06}, {0x380f, 0x9c}, // VTS + {0x3820, 0xb3}, + {0x3821, 0x01}, +}; + +const struct i2c_random_wr_payload ife_downscale_override_array_os04c10[] = { + // OS04C10_AA_00_02_17_wAO_2688x1524_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz + {0x3829, 0x03}, + {0x3714, 0x24}, + {0x37c2, 0x04}, + {0x3662, 0x10}, + {0x37d9, 0x08}, + {0x4041, 0x07}, + {0x4008, 0x02}, + {0x4009, 0x0d}, + {0x3808, 0x0a}, {0x3809, 0x80}, + {0x380a, 0x05}, {0x380b, 0xf0}, + {0x3814, 0x01}, + {0x3816, 0x01}, + {0x380c, 0x08}, {0x380d, 0x5c}, // HTS + {0x380e, 0x09}, {0x380f, 0x38}, // VTS + {0x3820, 0xb0}, + {0x3821, 0x00}, }; diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h index c1131aafdf..d4be3cf036 100644 --- a/system/camerad/sensors/sensor.h +++ b/system/camerad/sensors/sensor.h @@ -29,6 +29,7 @@ public: uint32_t frame_stride; uint32_t frame_offset = 0; uint32_t extra_height = 0; + int out_scale = 1; int registers_offset = -1; int stats_offset = -1; int hdr_offset = -1; @@ -109,6 +110,7 @@ public: class OS04C10 : public SensorInfo { public: OS04C10(); + void ife_downscale_configure(); std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override; float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override; int getSlaveAddress(int port) const override; From 7ceb284d6d3557713ec58f4726f864a0e5f1ebaa Mon Sep 17 00:00:00 2001 From: YassineYousfi Date: Fri, 18 Apr 2025 19:21:52 -0700 Subject: [PATCH 11/24] process replay migration: fix longitudinalPlan (#35035) --- selfdrive/test/process_replay/migration.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index 39d5452b6b..02cd9b6d71 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -13,7 +13,7 @@ from opendbc.car.gm.values import GMSafetyFlags from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.modeld.fill_model_msg import fill_xyz_poly, fill_lane_line_meta from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index -from openpilot.selfdrive.controls.lib.longitudinal_planner import get_accel_from_plan +from openpilot.selfdrive.controls.lib.longitudinal_planner import get_accel_from_plan, CONTROL_N_T_IDX from openpilot.system.manager.process_config import managed_processes from openpilot.tools.lib.logreader import LogIterable @@ -109,7 +109,7 @@ def migrate_longitudinalPlan(msgs): if msg.which() != 'longitudinalPlan': continue new_msg = msg.as_builder() - a_target, should_stop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels) + a_target, should_stop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels, CONTROL_N_T_IDX) new_msg.longitudinalPlan.aTarget, new_msg.longitudinalPlan.shouldStop = float(a_target), bool(should_stop) ops.append((index, new_msg.as_reader())) return ops, [], [] From fb5fa70e78c18566ce79ebdb554d02ceec07d176 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Sat, 19 Apr 2025 16:17:18 +0100 Subject: [PATCH 12/24] ui(raylib): update text window font, new lines and indentation (#35031) --- system/ui/text.py | 31 +++++++++++++++++++------------ 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/system/ui/text.py b/system/ui/text.py index 97d0ebcce9..4ea886ba1e 100755 --- a/system/ui/text.py +++ b/system/ui/text.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import re import pyray as rl from openpilot.system.hardware import HARDWARE from openpilot.system.ui.lib.button import gui_button, ButtonStyle @@ -7,8 +8,8 @@ from openpilot.system.ui.lib.application import gui_app MARGIN = 50 SPACING = 50 -FONT_SIZE = 60 -LINE_HEIGHT = 64 +FONT_SIZE = 72 +LINE_HEIGHT = 80 BUTTON_SIZE = rl.Vector2(310, 160) DEMO_TEXT = """This is a sample text that will be wrapped and scrolled if necessary. @@ -16,18 +17,24 @@ DEMO_TEXT = """This is a sample text that will be wrapped and scrolled if necess def wrap_text(text, font_size, max_width): lines = [] - current_line = "" font = gui_app.font() - for word in text.split(): - test_line = current_line + word + " " - if rl.measure_text_ex(font, test_line, font_size, 0).x <= max_width: - current_line = test_line - else: + for paragraph in text.split("\n"): + if not paragraph.strip(): + lines.append("") + continue + indent = re.match(r"^\s*", paragraph).group() + current_line = indent + for word in paragraph.split(): + test_line = current_line + word + " " + if rl.measure_text_ex(font, test_line, font_size, 0).x <= max_width: + current_line = test_line + else: + lines.append(current_line) + current_line = word + " " + current_line = current_line.rstrip() + if current_line: lines.append(current_line) - current_line = word + " " - if current_line: - lines.append(current_line) return lines @@ -45,7 +52,7 @@ class TextWindow: position = rl.Vector2(self._textarea_rect.x + scroll.x, self._textarea_rect.y + scroll.y + i * LINE_HEIGHT) if position.y + LINE_HEIGHT < self._textarea_rect.y or position.y > self._textarea_rect.y + self._textarea_rect.height: continue - rl.draw_text_ex(gui_app.font(), line.strip(), position, FONT_SIZE, 0, rl.WHITE) + rl.draw_text_ex(gui_app.font(), line, position, FONT_SIZE, 0, rl.WHITE) rl.end_scissor_mode() button_bounds = rl.Rectangle(gui_app.width - MARGIN - BUTTON_SIZE.x, gui_app.height - MARGIN - BUTTON_SIZE.y, BUTTON_SIZE.x, BUTTON_SIZE.y) From 618b7ccf230d4048f85924c7b1bbd0f9f1b6533a Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Sat, 19 Apr 2025 17:08:33 +0100 Subject: [PATCH 13/24] ui(raylib): calculate spinner progress in set_text (#35036) --- system/ui/spinner.py | 32 ++++++++++++++++++-------------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/system/ui/spinner.py b/system/ui/spinner.py index 951c6f697e..edf1fe5ca9 100755 --- a/system/ui/spinner.py +++ b/system/ui/spinner.py @@ -26,11 +26,17 @@ class Spinner: self._spinner_texture = gui_app.load_texture_from_image(os.path.join(BASEDIR, "selfdrive/assets/img_spinner_track.png"), TEXTURE_SIZE, TEXTURE_SIZE) self._rotation = 0.0 self._text: str = "" + self._progress: int | None = None self._lock = threading.Lock() def set_text(self, text: str) -> None: with self._lock: - self._text = text + if text.isdigit(): + self._progress = clamp(int(text), 0, 100) + self._text = "" + else: + self._progress = None + self._text = text def render(self): center = rl.Vector2(gui_app.width / 2.0, gui_app.height / 2.0) @@ -49,23 +55,21 @@ class Spinner: rl.draw_texture_v(self._comma_texture, comma_position, rl.WHITE) # Display progress bar or text based on user input - text = None + y_pos = rl.get_screen_height() - MARGIN - PROGRESS_BAR_HEIGHT with self._lock: + progress = self._progress text = self._text - if text: - y_pos = rl.get_screen_height() - MARGIN - PROGRESS_BAR_HEIGHT - if text.isdigit(): - progress = clamp(int(text), 0, 100) - bar = rl.Rectangle(center.x - PROGRESS_BAR_WIDTH / 2.0, y_pos, PROGRESS_BAR_WIDTH, PROGRESS_BAR_HEIGHT) - rl.draw_rectangle_rounded(bar, 1, 10, DARKGRAY) + if progress is not None: + bar = rl.Rectangle(center.x - PROGRESS_BAR_WIDTH / 2.0, y_pos, PROGRESS_BAR_WIDTH, PROGRESS_BAR_HEIGHT) + rl.draw_rectangle_rounded(bar, 1, 10, DARKGRAY) - bar.width *= progress / 100.0 - rl.draw_rectangle_rounded(bar, 1, 10, rl.WHITE) - else: - text_size = rl.measure_text_ex(gui_app.font(), text, FONT_SIZE, 1.0) - rl.draw_text_ex(gui_app.font(), text, - rl.Vector2(center.x - text_size.x / 2, y_pos), FONT_SIZE, 1.0, rl.WHITE) + bar.width *= progress / 100.0 + rl.draw_rectangle_rounded(bar, 1, 10, rl.WHITE) + elif text: + text_size = rl.measure_text_ex(gui_app.font(), text, FONT_SIZE, 1.0) + rl.draw_text_ex(gui_app.font(), text, + rl.Vector2(center.x - text_size.x / 2, y_pos), FONT_SIZE, 1.0, rl.WHITE) if __name__ == "__main__": From d56baa0fbb8f99f3ee6dc6ad3f9fbc5aee2b01e2 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Sun, 20 Apr 2025 15:03:49 +0100 Subject: [PATCH 14/24] ui(raylib): constant spinner rotation speed (#35037) --- system/ui/spinner.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/system/ui/spinner.py b/system/ui/spinner.py index edf1fe5ca9..adfb604de9 100755 --- a/system/ui/spinner.py +++ b/system/ui/spinner.py @@ -9,7 +9,7 @@ from openpilot.system.ui.lib.application import gui_app # Constants PROGRESS_BAR_WIDTH = 1000 PROGRESS_BAR_HEIGHT = 20 -ROTATION_TIME_SECONDS = 1.0 # Time for one full circle +DEGREES_PER_SECOND = 360.0 # one full rotation per second MARGIN = 200 TEXTURE_SIZE = 360 FONT_SIZE = 80 @@ -43,10 +43,8 @@ class Spinner: spinner_origin = rl.Vector2(TEXTURE_SIZE / 2.0, TEXTURE_SIZE / 2.0) comma_position = rl.Vector2(center.x - TEXTURE_SIZE / 2.0, center.y - TEXTURE_SIZE / 2.0) - fps = rl.get_fps() - if fps > 0: - degrees_per_frame = 360.0 / (ROTATION_TIME_SECONDS * fps) - self._rotation = (self._rotation + degrees_per_frame) % 360.0 + delta_time = rl.get_frame_time() + self._rotation = (self._rotation + DEGREES_PER_SECOND * delta_time) % 360.0 # Draw rotating spinner and static comma logo rl.draw_texture_pro(self._spinner_texture, rl.Rectangle(0, 0, TEXTURE_SIZE, TEXTURE_SIZE), From ed555cb948ffcfd8a233938a6caba7fa93d30d2d Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 21 Apr 2025 12:48:11 -0700 Subject: [PATCH 15/24] [bot] Update translations (#35040) Update translations Co-authored-by: Vehicle Researcher --- selfdrive/ui/translations/main_de.ts | 590 +++++++++++++-------------- 1 file changed, 295 insertions(+), 295 deletions(-) diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index f279a8d9f1..986c0fa54f 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -3,14 +3,14 @@ AbstractAlert - + Close Schließen - - + + Snooze Update Update pausieren - + Reboot and Update Aktualisieren und neu starten @@ -66,26 +66,26 @@ Prevent large data uploads when on a metered connection Hochladen großer Dateien über getaktete Verbindungen unterbinden - - Hidden Network - Verborgenes Netzwerk - - - CONNECT - VERBINDEN - - - Enter SSID - SSID eingeben - - - Enter password - Passwort eingeben - - - for "%1" - für "%1" - + + Hidden Network + Verborgenes Netzwerk + + + CONNECT + VERBINDEN + + + Enter SSID + SSID eingeben + + + Enter password + Passwort eingeben + + + for "%1" + für "%1" + ConfirmationDialog @@ -115,34 +115,34 @@ DeveloperPanel - - Joystick Debug Mode - Joystick Debug-Modus - - - Longitudinal Maneuver Mode - Längsmanöver-Modus - - - openpilot Longitudinal Control (Alpha) - openpilot Längsregelung (Alpha) - - - WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB). - WARNUNG: Die openpilot Längsregelung befindet sich für dieses Fahrzeug im Alpha-Stadium und deaktiviert das automatische Notbremsen (AEB). - - - On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. - Bei diesem Fahrzeug verwendet openpilot standardmäßig den eingebauten Tempomaten anstelle der openpilot Längsregelung. Aktiviere diese Option, um auf die openpilot Längsregelung umzuschalten. Es wird empfohlen, den experimentellen Modus zu aktivieren, wenn die openpilot Längsregelung (Alpha) aktiviert wird. - - - Enable ADB - ADB aktivieren - - - ADB (Android Debug Bridge) allows connecting to your device over USB or over the network. See https://docs.comma.ai/how-to/connect-to-comma for more info. - ADB (Android Debug Bridge) ermöglicht die Verbindung zu deinem Gerät über USB oder Netzwerk. Siehe https://docs.comma.ai/how-to/connect-to-comma für weitere Informationen. - + + Joystick Debug Mode + Joystick Debug-Modus + + + Longitudinal Maneuver Mode + Längsmanöver-Modus + + + openpilot Longitudinal Control (Alpha) + openpilot Längsregelung (Alpha) + + + WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB). + WARNUNG: Die openpilot Längsregelung befindet sich für dieses Fahrzeug im Alpha-Stadium und deaktiviert das automatische Notbremsen (AEB). + + + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. + Bei diesem Fahrzeug verwendet openpilot standardmäßig den eingebauten Tempomaten anstelle der openpilot Längsregelung. Aktiviere diese Option, um auf die openpilot Längsregelung umzuschalten. Es wird empfohlen, den experimentellen Modus zu aktivieren, wenn die openpilot Längsregelung (Alpha) aktiviert wird. + + + Enable ADB + ADB aktivieren + + + ADB (Android Debug Bridge) allows connecting to your device over USB or over the network. See https://docs.comma.ai/how-to/connect-to-comma for more info. + ADB (Android Debug Bridge) ermöglicht die Verbindung zu deinem Gerät über USB oder Netzwerk. Siehe https://docs.comma.ai/how-to/connect-to-comma für weitere Informationen. + DevicePanel @@ -278,14 +278,14 @@ Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. Koppele dein Gerät mit Comma Connect (connect.comma.ai) und sichere dir dein Comma Prime Angebot. - - Pair Device - Gerät koppeln - - - PAIR - KOPPELN - + + Pair Device + Gerät koppeln + + + PAIR + KOPPELN + DriverViewWindow @@ -307,41 +307,41 @@ FirehosePanel - - 🔥 Firehose Mode 🔥 - 🔥 Firehose-Modus 🔥 - - - openpilot learns to drive by watching humans, like you, drive. + + 🔥 Firehose Mode 🔥 + 🔥 Firehose-Modus 🔥 + + + openpilot learns to drive by watching humans, like you, drive. Firehose Mode allows you to maximize your training data uploads to improve openpilot's driving models. More data means bigger models, which means better Experimental Mode. - openpilot lernt das Fahren, indem es Menschen wie dir beim Fahren zuschaut. + openpilot lernt das Fahren, indem es Menschen wie dir beim Fahren zuschaut. Der Firehose-Modus ermöglicht es dir, deine Trainingsdaten-Uploads zu maximieren, um die Fahrmodelle von openpilot zu verbessern. Mehr Daten bedeuten größere Modelle, was zu einem besseren Experimentellen Modus führt. - - - Firehose Mode: ACTIVE - Firehose-Modus: AKTIV - - - ACTIVE - AKTIV - - - For maximum effectiveness, bring your device inside and connect to a good USB-C adapter and Wi-Fi weekly.<br><br>Firehose Mode can also work while you're driving if connected to a hotspot or unlimited SIM card.<br><br><br><b>Frequently Asked Questions</b><br><br><i>Does it matter how or where I drive?</i> Nope, just drive as you normally would.<br><br><i>Do all of my segments get pulled in Firehose Mode?</i> No, we selectively pull a subset of your segments.<br><br><i>What's a good USB-C adapter?</i> Any fast phone or laptop charger should be fine.<br><br><i>Does it matter which software I run?</i> Yes, only upstream openpilot (and particular forks) are able to be used for training. - Für maximale Effektivität bring dein Gerät jede Woche nach drinnen und verbinde es mit einem guten USB-C-Adapter und WLAN.<br><br>Der Firehose-Modus funktioniert auch während der Fahrt, wenn das Gerät mit einem Hotspot oder einer ungedrosselten SIM-Karte verbunden ist.<br><br><br><b>Häufig gestellte Fragen</b><br><br><i>Spielt es eine Rolle, wie oder wo ich fahre?</i> Nein, fahre einfach wie gewohnt.<br><br><i>Werden im Firehose-Modus alle meine Segmente hochgeladen?</i> Nein, wir wählen selektiv nur einen Teil deiner Segmente aus.<br><br><i>Welcher USB-C-Adapter ist gut?</i> Jedes Schnellladegerät für Handy oder Laptop sollte ausreichen.<br><br><i>Spielt es eine Rolle, welche Software ich nutze?</i> Ja, nur das offizielle Upstream‑openpilot (und bestimmte Forks) kann für das Training verwendet werden. - - - <b>%n segment(s)</b> of your driving is in the training dataset so far. - - <b>%n Segment</b> deiner Fahrten ist bisher im Trainingsdatensatz. - <b>%n Segmente</b> deiner Fahrten sind bisher im Trainingsdatensatz. - - - - <span stylesheet='font-size: 60px; font-weight: bold; color: #e74c3c;'>INACTIVE</span>: connect to an unmetered network - <span stylesheet='font-size: 60px; font-weight: bold; color: #e74c3c;'>INAKTIV</span>: Verbinde dich mit einem ungedrosselten Netzwerk - + + + Firehose Mode: ACTIVE + Firehose-Modus: AKTIV + + + ACTIVE + AKTIV + + + For maximum effectiveness, bring your device inside and connect to a good USB-C adapter and Wi-Fi weekly.<br><br>Firehose Mode can also work while you're driving if connected to a hotspot or unlimited SIM card.<br><br><br><b>Frequently Asked Questions</b><br><br><i>Does it matter how or where I drive?</i> Nope, just drive as you normally would.<br><br><i>Do all of my segments get pulled in Firehose Mode?</i> No, we selectively pull a subset of your segments.<br><br><i>What's a good USB-C adapter?</i> Any fast phone or laptop charger should be fine.<br><br><i>Does it matter which software I run?</i> Yes, only upstream openpilot (and particular forks) are able to be used for training. + Für maximale Effektivität bring dein Gerät jede Woche nach drinnen und verbinde es mit einem guten USB-C-Adapter und WLAN.<br><br>Der Firehose-Modus funktioniert auch während der Fahrt, wenn das Gerät mit einem Hotspot oder einer ungedrosselten SIM-Karte verbunden ist.<br><br><br><b>Häufig gestellte Fragen</b><br><br><i>Spielt es eine Rolle, wie oder wo ich fahre?</i> Nein, fahre einfach wie gewohnt.<br><br><i>Werden im Firehose-Modus alle meine Segmente hochgeladen?</i> Nein, wir wählen selektiv nur einen Teil deiner Segmente aus.<br><br><i>Welcher USB-C-Adapter ist gut?</i> Jedes Schnellladegerät für Handy oder Laptop sollte ausreichen.<br><br><i>Spielt es eine Rolle, welche Software ich nutze?</i> Ja, nur das offizielle Upstream‑openpilot (und bestimmte Forks) kann für das Training verwendet werden. + + + <b>%n segment(s)</b> of your driving is in the training dataset so far. + + <b>%n Segment</b> deiner Fahrten ist bisher im Trainingsdatensatz. + <b>%n Segmente</b> deiner Fahrten sind bisher im Trainingsdatensatz. + + + + <span stylesheet='font-size: 60px; font-weight: bold; color: #e74c3c;'>INACTIVE</span>: connect to an unmetered network + <span stylesheet='font-size: 60px; font-weight: bold; color: #e74c3c;'>INAKTIV</span>: Verbinde dich mit einem ungedrosselten Netzwerk + HudRenderer @@ -419,44 +419,44 @@ Der Firehose-Modus ermöglicht es dir, deine Trainingsdaten-Uploads zu maximiere Connect to internet to check for updates. openpilot won't automatically start until it connects to internet to check for updates. Verbinde dich mit dem Internet, um nach Updates zu suchen. openpilot startet nicht automatisch, bis eine Internetverbindung besteht und nach Updates gesucht wurde. - - Unable to download updates + + Unable to download updates %1 - Updates konnten nicht heruntergeladen werden + Updates konnten nicht heruntergeladen werden %1 - - - Taking camera snapshots. System won't start until finished. - Kamera-Snapshots werden aufgenommen. Das System startet erst, wenn dies abgeschlossen ist. - - - An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. - Ein Update für das Betriebssystem deines Geräts wird im Hintergrund heruntergeladen. Du wirst aufgefordert, das Update zu installieren, sobald es bereit ist. - - - Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, visit https://comma.ai/support. - Gerät konnte nicht registriert werden. Es wird keine Verbindung zu den comma.ai-Servern herstellen oder Daten hochladen und erhält keinen Support von comma.ai. Wenn dies ein offizielles Gerät ist, besuche https://comma.ai/support. - - - NVMe drive not mounted. - NVMe-Laufwerk nicht gemounted. - - - Unsupported NVMe drive detected. Device may draw significantly more power and overheat due to the unsupported NVMe. - Nicht unterstütztes NVMe-Laufwerk erkannt. Das Gerät kann dadurch deutlich mehr Strom verbrauchen und überhitzen. - - - openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai. - openpilot konnte dein Auto nicht identifizieren. Dein Auto wird entweder nicht unterstützt oder die Steuergeräte (ECUs) werden nicht erkannt. Bitte reiche einen Pull Request ein, um die Firmware-Versionen für das richtige Fahrzeug hinzuzufügen. Hilfe findest du auf discord.comma.ai. - - - openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield. - openpilot hat eine Änderung der Montageposition des Geräts erkannt. Stelle sicher, dass das Gerät vollständig in der Halterung sitzt und die Halterung fest an der Windschutzscheibe befestigt ist. - - - Device temperature too high. System cooling down before starting. Current internal component temperature: %1 - Gerätetemperatur zu hoch. Das System kühlt ab, bevor es startet. Aktuelle interne Komponententemperatur: %1 - + + + Taking camera snapshots. System won't start until finished. + Kamera-Snapshots werden aufgenommen. Das System startet erst, wenn dies abgeschlossen ist. + + + An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. + Ein Update für das Betriebssystem deines Geräts wird im Hintergrund heruntergeladen. Du wirst aufgefordert, das Update zu installieren, sobald es bereit ist. + + + Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, visit https://comma.ai/support. + Gerät konnte nicht registriert werden. Es wird keine Verbindung zu den comma.ai-Servern herstellen oder Daten hochladen und erhält keinen Support von comma.ai. Wenn dies ein offizielles Gerät ist, besuche https://comma.ai/support. + + + NVMe drive not mounted. + NVMe-Laufwerk nicht gemounted. + + + Unsupported NVMe drive detected. Device may draw significantly more power and overheat due to the unsupported NVMe. + Nicht unterstütztes NVMe-Laufwerk erkannt. Das Gerät kann dadurch deutlich mehr Strom verbrauchen und überhitzen. + + + openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai. + openpilot konnte dein Auto nicht identifizieren. Dein Auto wird entweder nicht unterstützt oder die Steuergeräte (ECUs) werden nicht erkannt. Bitte reiche einen Pull Request ein, um die Firmware-Versionen für das richtige Fahrzeug hinzuzufügen. Hilfe findest du auf discord.comma.ai. + + + openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield. + openpilot hat eine Änderung der Montageposition des Geräts erkannt. Stelle sicher, dass das Gerät vollständig in der Halterung sitzt und die Halterung fest an der Windschutzscheibe befestigt ist. + + + Device temperature too high. System cooling down before starting. Current internal component temperature: %1 + Gerätetemperatur zu hoch. Das System kühlt ab, bevor es startet. Aktuelle interne Komponententemperatur: %1 + OffroadHome @@ -475,26 +475,26 @@ Der Firehose-Modus ermöglicht es dir, deine Trainingsdaten-Uploads zu maximiere OnroadAlerts - - openpilot Unavailable - openpilot nicht verfügbar - - - TAKE CONTROL IMMEDIATELY - ÜBERNIMM SOFORT DIE KONTROLLE - - - Reboot Device - Gerät neu starten - - - Waiting to start - Warten auf Start - - - System Unresponsive - System reagiert nicht - + + openpilot Unavailable + openpilot nicht verfügbar + + + TAKE CONTROL IMMEDIATELY + ÜBERNIMM SOFORT DIE KONTROLLE + + + Reboot Device + Gerät neu starten + + + Waiting to start + Warten auf Start + + + System Unresponsive + System reagiert nicht + PairingPopup @@ -514,10 +514,10 @@ Der Firehose-Modus ermöglicht es dir, deine Trainingsdaten-Uploads zu maximiere Bookmark connect.comma.ai to your home screen to use it like an app Füge connect.comma.ai als Lesezeichen auf deinem Homescreen hinzu um es wie eine App zu verwenden - - Please connect to Wi-Fi to complete initial pairing - Bitte verbinde dich mit WLAN, um die Koppelung abzuschließen. - + + Please connect to Wi-Fi to complete initial pairing + Bitte verbinde dich mit WLAN, um die Koppelung abzuschließen. + ParamControl @@ -548,18 +548,18 @@ Der Firehose-Modus ermöglicht es dir, deine Trainingsdaten-Uploads zu maximiere Remote access Fernzugriff - - 24/7 LTE connectivity - 24/7 LTE-Verbindung - - - 1 year of drive storage - Fahrdaten-Speicherung für 1 Jahr - - - Remote snapshots - Remote-Snapshots - + + 24/7 LTE connectivity + 24/7 LTE-Verbindung + + + 1 year of drive storage + Fahrdaten-Speicherung für 1 Jahr + + + Remote snapshots + Remote-Snapshots + PrimeUserWidget @@ -607,10 +607,10 @@ Der Firehose-Modus ermöglicht es dir, deine Trainingsdaten-Uploads zu maximiere vor %n Tagen - - now - jetzt - + + now + jetzt + Reset @@ -638,20 +638,20 @@ Der Firehose-Modus ermöglicht es dir, deine Trainingsdaten-Uploads zu maximiere Confirm Bestätigen - - Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. - Datenpartition konnte nicht gemounted werden. Die Partition ist möglicherweise beschädigt. Drücke Bestätigen, um das Gerät zu löschen und zurückzusetzen. - - - Resetting device... + + Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. + Datenpartition konnte nicht gemounted werden. Die Partition ist möglicherweise beschädigt. Drücke Bestätigen, um das Gerät zu löschen und zurückzusetzen. + + + Resetting device... This may take up to a minute. - Gerät wird zurückgesetzt... + Gerät wird zurückgesetzt... Dies kann bis zu einer Minute dauern. - - - System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - System-Reset ausgelöst. Drücke Bestätigen, um alle Inhalte und Einstellungen zu löschen. Drücke Abbrechen, um den Startvorgang fortzusetzen. - + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + System-Reset ausgelöst. Drücke Bestätigen, um alle Inhalte und Einstellungen zu löschen. Drücke Abbrechen, um den Startvorgang fortzusetzen. + SettingsWindow @@ -675,14 +675,14 @@ Dies kann bis zu einer Minute dauern. Software Software - - Developer - Entwickler - - - Firehose - Firehose - + + Developer + Entwickler + + + Firehose + Firehose + Setup @@ -754,30 +754,30 @@ Dies kann bis zu einer Minute dauern. Start over Von neuem beginnen - - No custom software found at this URL. - Keine benutzerdefinierte Software unter dieser URL gefunden. - - - Something went wrong. Reboot the device. - Etwas ist schiefgelaufen. Starte das Gerät neu. - + + No custom software found at this URL. + Keine benutzerdefinierte Software unter dieser URL gefunden. + + + Something went wrong. Reboot the device. + Etwas ist schiefgelaufen. Starte das Gerät neu. + Select a language Sprache wählen - - Choose Software to Install - Wähle die zu installierende Software - - - openpilot - openpilot - - - Custom Software - Benutzerdefinierte Software - + + Choose Software to Install + Wähle die zu installierende Software + + + openpilot + openpilot + + + Custom Software + Benutzerdefinierte Software + SetupWidget @@ -925,26 +925,26 @@ Dies kann bis zu einer Minute dauern. Uninstall Deinstallieren - - failed to check for update - Update-Prüfung fehlgeschlagen - - - up to date, last checked %1 - Auf dem neuesten Stand, zuletzt geprüft am %1 - - - DOWNLOAD - HERUNTERLADEN - - - update available - Update verfügbar - - - never - nie - + + failed to check for update + Update-Prüfung fehlgeschlagen + + + up to date, last checked %1 + Auf dem neuesten Stand, zuletzt geprüft am %1 + + + DOWNLOAD + HERUNTERLADEN + + + update available + Update verfügbar + + + never + nie + SshControl @@ -1002,14 +1002,14 @@ Dies kann bis zu einer Minute dauern. Agree Zustimmen - - Welcome to openpilot - Willkommen bei openpilot - - - You must accept the Terms and Conditions to use openpilot. Read the latest terms at <span style='color: #465BEA;'>https://comma.ai/terms</span> before continuing. - Du musst die Nutzungsbedingungen akzeptieren, um openpilot zu verwenden. Lies die aktuellen Bedingungen unter <span style='color: #465BEA;'>https://comma.ai/terms</span>, bevor du fortfährst. - + + Welcome to openpilot + Willkommen bei openpilot + + + You must accept the Terms and Conditions to use openpilot. Read the latest terms at <span style='color: #465BEA;'>https://comma.ai/terms</span> before continuing. + Du musst die Nutzungsbedingungen akzeptieren, um openpilot zu verwenden. Lies die aktuellen Bedingungen unter <span style='color: #465BEA;'>https://comma.ai/terms</span>, bevor du fortfährst. + TogglesPanel @@ -1073,54 +1073,54 @@ Dies kann bis zu einer Minute dauern. Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. Der experimentelle Modus ist momentan für dieses Auto nicht verfügbar da es den eingebauten adaptiven Tempomaten des Autos benutzt. - - Aggressive - Aggressiv - - - Standard - Standard - - - Relaxed - Entspannt - - - Driving Personality - Fahrstil - - - End-to-End Longitudinal Control - Ende-zu-Ende Längsregelung - - - openpilot longitudinal control may come in a future update. - Die openpilot Längsregelung könnte in einem zukünftigen Update verfügbar sein. - - - An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. - Eine Alpha-Version der openpilot Längsregelung kann zusammen mit dem Experimentellen Modus auf non-stable Branches getestet werden. - - - Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. - Aktiviere den Schalter für openpilot Längsregelung (Alpha), um den Experimentellen Modus zu erlauben. - - - Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. - Standard wird empfohlen. Im aggressiven Modus folgt openpilot vorausfahrenden Fahrzeugen enger und ist beim Gasgeben und Bremsen aggressiver. Im entspannten Modus hält openpilot mehr Abstand zu vorausfahrenden Fahrzeugen. Bei unterstützten Fahrzeugen kannst du mit der Abstandstaste am Lenkrad zwischen diesen Fahrstilen wechseln. - - - The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. - Die Fahrvisualisierung wechselt bei niedrigen Geschwindigkeiten auf die nach vorne gerichtete Weitwinkelkamera, um Kurven besser darzustellen. Das Logo des Experimentellen Modus wird außerdem oben rechts angezeigt. - - - Always-On Driver Monitoring - Dauerhaft aktive Fahrerüberwachung - - - Enable driver monitoring even when openpilot is not engaged. - Fahrerüberwachung auch aktivieren, wenn openpilot nicht aktiv ist. - + + Aggressive + Aggressiv + + + Standard + Standard + + + Relaxed + Entspannt + + + Driving Personality + Fahrstil + + + End-to-End Longitudinal Control + Ende-zu-Ende Längsregelung + + + openpilot longitudinal control may come in a future update. + Die openpilot Längsregelung könnte in einem zukünftigen Update verfügbar sein. + + + An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + Eine Alpha-Version der openpilot Längsregelung kann zusammen mit dem Experimentellen Modus auf non-stable Branches getestet werden. + + + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. + Aktiviere den Schalter für openpilot Längsregelung (Alpha), um den Experimentellen Modus zu erlauben. + + + Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. + Standard wird empfohlen. Im aggressiven Modus folgt openpilot vorausfahrenden Fahrzeugen enger und ist beim Gasgeben und Bremsen aggressiver. Im entspannten Modus hält openpilot mehr Abstand zu vorausfahrenden Fahrzeugen. Bei unterstützten Fahrzeugen kannst du mit der Abstandstaste am Lenkrad zwischen diesen Fahrstilen wechseln. + + + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + Die Fahrvisualisierung wechselt bei niedrigen Geschwindigkeiten auf die nach vorne gerichtete Weitwinkelkamera, um Kurven besser darzustellen. Das Logo des Experimentellen Modus wird außerdem oben rechts angezeigt. + + + Always-On Driver Monitoring + Dauerhaft aktive Fahrerüberwachung + + + Enable driver monitoring even when openpilot is not engaged. + Fahrerüberwachung auch aktivieren, wenn openpilot nicht aktiv ist. + Updater @@ -1159,18 +1159,18 @@ Dies kann bis zu einer Minute dauern. WiFiPromptWidget - - Open - Öffnen - - - Maximize your training data uploads to improve openpilot's driving models. - Maximiere deine Trainingsdaten-Uploads, um die Fahrmodelle von openpilot zu verbessern. - - - <span style='font-family: "Noto Color Emoji";'>🔥</span> Firehose Mode <span style='font-family: Noto Color Emoji;'>🔥</span> - <span style='font-family: "Noto Color Emoji";'>🔥</span> Firehose-Modus <span style='font-family: Noto Color Emoji;'>🔥</span> - + + Open + Öffnen + + + Maximize your training data uploads to improve openpilot's driving models. + Maximiere deine Trainingsdaten-Uploads, um die Fahrmodelle von openpilot zu verbessern. + + + <span style='font-family: "Noto Color Emoji";'>🔥</span> Firehose Mode <span style='font-family: Noto Color Emoji;'>🔥</span> + <span style='font-family: "Noto Color Emoji";'>🔥</span> Firehose-Modus <span style='font-family: Noto Color Emoji;'>🔥</span> + WifiUI From 34514ef1761037127cc009e5e95b7478b5fc1ad6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Mon, 21 Apr 2025 14:11:57 -0700 Subject: [PATCH 16/24] Long planner: make work with training (#35043) * revert useless * update ref --- selfdrive/controls/lib/longitudinal_planner.py | 6 +++--- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 104c7c9d49..4219e80f33 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -52,6 +52,7 @@ class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) + self.mpc.mode = 'acc' self.fcw = False self.dt = dt self.allow_throttle = True @@ -89,7 +90,6 @@ class LongitudinalPlanner: return x, v, a, j, throttle_prob def update(self, sm): - self.mpc.mode = 'acc' self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' if len(sm['carControl'].orientationNED) == 3: @@ -97,7 +97,7 @@ class LongitudinalPlanner: else: accel_coast = ACCEL_MAX - v_ego = sm['modelV2'].velocity.x[0] + v_ego = sm['carState'].vEgo v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET @@ -129,7 +129,7 @@ class LongitudinalPlanner: self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) # Compute model v_ego error self.v_model_error = get_speed_error(sm['modelV2'], v_ego) - x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], 0) + x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error) # Don't clip at low speeds since throttle_prob doesn't account for creep self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 5f89d7802d..4b3ef33407 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -0800e73b5d9c801f161b9559557c0559b0682fec \ No newline at end of file +fcc771b9ceb487a61035885acbd84e592fb316bf \ No newline at end of file From 33a7d853f00d7f6f9d1ffc13160816b6106aec72 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Mon, 21 Apr 2025 15:10:41 -0700 Subject: [PATCH 17/24] Modeld: small refactor (#35044) refactor --- selfdrive/controls/lib/drive_helpers.py | 3 +++ selfdrive/modeld/modeld.py | 8 +------- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 9958755577..038061b7e6 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -19,6 +19,9 @@ def clamp(val, min_val, max_val): clamped_val = float(np.clip(val, min_val, max_val)) return clamped_val, clamped_val != val +def smooth_value(val, prev_val, tau): + alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1 + return alpha * val + (1 - alpha) * prev_val def clip_curvature(v_ego, prev_curvature, new_curvature, roll): # This function respects ISO lateral jerk and acceleration limits + a max curvature diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 0ad629f519..1060b36d28 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -26,15 +26,13 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix from openpilot.system import sentry from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper -from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState from openpilot.selfdrive.modeld.constants import ModelConstants, Plan from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext - - PROCESS_NAME = "selfdrive.modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -46,10 +44,6 @@ POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.p LAT_SMOOTH_SECONDS = 0.3 LONG_SMOOTH_SECONDS = 0.3 -def smooth_value(val, prev_val, tau): - alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1 - return alpha * val + (1 - alpha) * prev_val - def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, lat_action_t: float, long_action_t: float,) -> log.ModelDataV2.Action: From a029d13398c0408b4ebcf7fa916474f5e3d79129 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Mon, 21 Apr 2025 15:44:30 -0700 Subject: [PATCH 18/24] update smooth --- selfdrive/controls/lib/drive_helpers.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 038061b7e6..3f99f1bbbb 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -19,8 +19,8 @@ def clamp(val, min_val, max_val): clamped_val = float(np.clip(val, min_val, max_val)) return clamped_val, clamped_val != val -def smooth_value(val, prev_val, tau): - alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1 +def smooth_value(val, prev_val, tau, dt=DT_MDL): + alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1 return alpha * val + (1 - alpha) * prev_val def clip_curvature(v_ego, prev_curvature, new_curvature, roll): From a9b9e0bb54dda1613e8347efe146a78d6ce6bc71 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 21 Apr 2025 16:13:55 -0700 Subject: [PATCH 19/24] raylib: init updater (#35045) * raylib: init updater * cleanup --- system/ui/updater.py | 197 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 197 insertions(+) create mode 100755 system/ui/updater.py diff --git a/system/ui/updater.py b/system/ui/updater.py new file mode 100755 index 0000000000..eb9d766a16 --- /dev/null +++ b/system/ui/updater.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python3 +import sys +import subprocess +import threading +import pyray as rl +from enum import IntEnum + +from openpilot.system.hardware import HARDWARE +from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.lib.button import gui_button, ButtonStyle +from openpilot.system.ui.lib.label import gui_text_box, gui_label + +# Constants +MARGIN = 50 +BUTTON_HEIGHT = 160 +BUTTON_WIDTH = 400 +PROGRESS_BAR_HEIGHT = 72 +TITLE_FONT_SIZE = 80 +BODY_FONT_SIZE = 65 +BACKGROUND_COLOR = rl.BLACK +PROGRESS_BG_COLOR = rl.Color(41, 41, 41, 255) +PROGRESS_COLOR = rl.Color(54, 77, 239, 255) + + +class Screen(IntEnum): + PROMPT = 0 + WIFI = 1 + PROGRESS = 2 + + +class Updater: + def __init__(self, updater_path, manifest_path): + self.updater = updater_path + self.manifest = manifest_path + self.current_screen = Screen.PROMPT + + self.progress_value = 0 + self.progress_text = "Loading..." + self.show_reboot_button = False + self.process = None + self.update_thread = None + + def install_update(self): + self.current_screen = Screen.PROGRESS + self.progress_value = 0 + self.progress_text = "Downloading..." + self.show_reboot_button = False + + # Start the update process in a separate thread + self.update_thread = threading.Thread(target=self._run_update_process) + self.update_thread.daemon = True + self.update_thread.start() + + def _run_update_process(self): + # TODO: just import it and run in a thread without a subprocess + cmd = [self.updater, "--swap", self.manifest] + self.process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, + text=True, bufsize=1, universal_newlines=True) + + for line in self.process.stdout: + parts = line.strip().split(":") + if len(parts) == 2: + self.progress_text = parts[0] + try: + self.progress_value = int(float(parts[1])) + except ValueError: + pass + + exit_code = self.process.wait() + if exit_code == 0: + HARDWARE.reboot() + else: + self.progress_text = "Update failed" + self.show_reboot_button = True + + def render_prompt_screen(self): + # Title + title_rect = rl.Rectangle(MARGIN + 50, 250, gui_app.width - MARGIN * 2 - 100, TITLE_FONT_SIZE) + gui_label(title_rect, "Update Required", TITLE_FONT_SIZE, font_weight=FontWeight.BOLD) + + # Description + desc_text = "An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. \ + The download size is approximately 1GB." + desc_rect = rl.Rectangle(MARGIN + 50, 250 + TITLE_FONT_SIZE + 75, gui_app.width - MARGIN * 2 - 100, BODY_FONT_SIZE * 3) + gui_text_box(desc_rect, desc_text, BODY_FONT_SIZE) + + # Buttons at the bottom + button_y = gui_app.height - MARGIN - BUTTON_HEIGHT + button_width = (gui_app.width - MARGIN * 3) // 2 + + # WiFi button + wifi_button_rect = rl.Rectangle(MARGIN, button_y, button_width, BUTTON_HEIGHT) + if gui_button(wifi_button_rect, "Connect to Wi-Fi"): + self.current_screen = Screen.WIFI + return # Return to avoid processing other buttons after screen change + + # Install button + install_button_rect = rl.Rectangle(MARGIN * 2 + button_width, button_y, button_width, BUTTON_HEIGHT) + if gui_button(install_button_rect, "Install", button_style=ButtonStyle.PRIMARY): + self.install_update() + return # Return to avoid further processing after action + + def render_wifi_screen(self): + # Title and back button + title_rect = rl.Rectangle(MARGIN + 50, MARGIN, gui_app.width - MARGIN * 2 - 100, 60) + gui_label(title_rect, "Wi-Fi Networks", 60, font_weight=FontWeight.BOLD) + + back_button_rect = rl.Rectangle(MARGIN, gui_app.height - MARGIN - BUTTON_HEIGHT, BUTTON_WIDTH, BUTTON_HEIGHT) + if gui_button(back_button_rect, "Back"): + self.current_screen = Screen.PROMPT + return # Return to avoid processing other interactions after screen change + + # Draw placeholder for WiFi implementation + placeholder_rect = rl.Rectangle( + MARGIN, + title_rect.y + title_rect.height + MARGIN, + gui_app.width - MARGIN * 2, + gui_app.height - title_rect.height - MARGIN * 3 - BUTTON_HEIGHT + ) + + # Draw rounded rectangle background + rl.draw_rectangle_rounded( + placeholder_rect, + 0.1, + 10, + rl.Color(41, 41, 41, 255) + ) + + # Draw placeholder text + placeholder_text = "WiFi Implementation Placeholder" + text_size = rl.measure_text_ex(gui_app.font(), placeholder_text, 80, 1) + text_pos = rl.Vector2( + placeholder_rect.x + (placeholder_rect.width - text_size.x) / 2, + placeholder_rect.y + (placeholder_rect.height - text_size.y) / 2 + ) + rl.draw_text_ex(gui_app.font(), placeholder_text, text_pos, 80, 1, rl.WHITE) + + # Draw instructions + instructions_text = "Real WiFi functionality would be implemented here" + instructions_size = rl.measure_text_ex(gui_app.font(), instructions_text, 40, 1) + instructions_pos = rl.Vector2( + placeholder_rect.x + (placeholder_rect.width - instructions_size.x) / 2, + text_pos.y + text_size.y + 20 + ) + rl.draw_text_ex(gui_app.font(), instructions_text, instructions_pos, 40, 1, rl.GRAY) + + def render_progress_screen(self): + title_rect = rl.Rectangle(MARGIN + 100, 330, gui_app.width - MARGIN * 2 - 200, 100) + gui_label(title_rect, self.progress_text, 90, font_weight=FontWeight.SEMI_BOLD) + + # Progress bar + bar_rect = rl.Rectangle(MARGIN + 100, 330 + 100 + 100, gui_app.width - MARGIN * 2 - 200, PROGRESS_BAR_HEIGHT) + rl.draw_rectangle_rounded(bar_rect, 0.5, 10, PROGRESS_BG_COLOR) + + # Calculate the width of the progress chunk + progress_width = (bar_rect.width * self.progress_value) / 100 + if progress_width > 0: + progress_rect = rl.Rectangle(bar_rect.x, bar_rect.y, progress_width, bar_rect.height) + rl.draw_rectangle_rounded(progress_rect, 0.5, 10, PROGRESS_COLOR) + + # Show reboot button if needed + if self.show_reboot_button: + reboot_rect = rl.Rectangle(MARGIN + 100, gui_app.height - MARGIN - BUTTON_HEIGHT, BUTTON_WIDTH, BUTTON_HEIGHT) + if gui_button(reboot_rect, "Reboot"): + # Return True to signal main loop to exit before rebooting + HARDWARE.reboot() + return + + def render(self): + if self.current_screen == Screen.PROMPT: + self.render_prompt_screen() + elif self.current_screen == Screen.WIFI: + self.render_wifi_screen() + elif self.current_screen == Screen.PROGRESS: + self.render_progress_screen() + + +def main(): + if len(sys.argv) < 3: + print("Usage: updater.py ") + sys.exit(1) + + updater_path = sys.argv[1] + manifest_path = sys.argv[2] + + try: + gui_app.init_window("System Update") + updater = Updater(updater_path, manifest_path) + for _ in gui_app.render(): + updater.render() + finally: + # Make sure we clean up even if there's an error + gui_app.close() + + +if __name__ == "__main__": + main() From aaaa2d0dd064364d647fb381cc488fe6fe4d44e2 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Mon, 21 Apr 2025 17:02:36 -0700 Subject: [PATCH 20/24] Revert "OS04C10: use IFE downscaler for road cameras" (#35046) Revert "OS04C10: use IFE downscaler for road cameras (#35023)" This reverts commit 3b60b22ceeb090812e5fa948ac6e56442d681dc4. --- system/camerad/cameras/camera_common.cc | 3 ++ system/camerad/cameras/camera_common.h | 2 +- system/camerad/cameras/camera_qcom2.cc | 10 ++-- system/camerad/cameras/ife.h | 18 ++++---- system/camerad/cameras/spectra.cc | 40 +++++++--------- system/camerad/sensors/os04c10.cc | 11 ----- system/camerad/sensors/os04c10_registers.h | 54 ++++++++-------------- system/camerad/sensors/sensor.h | 2 - 8 files changed, 53 insertions(+), 87 deletions(-) diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 1f6ad9b4be..f6c2681dea 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -26,6 +26,9 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, SpectraCamera * LOGD("allocated %d CL buffers", frame_buf_count); } + out_img_width = sensor->frame_width; + out_img_height = sensor->hdr_offset > 0 ? (sensor->frame_height - sensor->hdr_offset) / 2 : sensor->frame_height; + // the encoder HW tells us the size it wants after setting it up. // TODO: VENUS_BUFFER_SIZE should give the size, but it's too small. dependent on encoder settings? size_t nv12_size = (out_img_width <= 1344 ? 2900 : 2346)*cam->stride; diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index c26859cbc4..c0426678b6 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -32,7 +32,7 @@ public: VisionBuf *cur_yuv_buf; VisionBuf *cur_camera_buf; std::unique_ptr camera_bufs_raw; - uint32_t out_img_width, out_img_height; + int out_img_width, out_img_height; CameraBuf() = default; ~CameraBuf(); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 8c4602bb31..18a42f05d8 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -73,7 +73,7 @@ void CameraState::init(VisionIpcServer *v, cl_device_id device_id, cl_context ct if (!camera.enabled) return; - fl_pix = camera.cc.focal_len / camera.sensor->pixel_size_mm / camera.sensor->out_scale; + fl_pix = camera.cc.focal_len / camera.sensor->pixel_size_mm; set_exposure_rect(); dc_gain_weight = camera.sensor->dc_gain_min_weight; @@ -107,10 +107,10 @@ void CameraState::set_exposure_rect() { float fl_ref = ae_target.second; ae_xywh = (Rect){ - std::max(0, (int)camera.buf.out_img_width / 2 - (int)(fl_pix / fl_ref * xywh_ref.w / 2)), - std::max(0, (int)camera.buf.out_img_height / 2 - (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))), - std::min((int)(fl_pix / fl_ref * xywh_ref.w), (int)camera.buf.out_img_width / 2 + (int)(fl_pix / fl_ref * xywh_ref.w / 2)), - std::min((int)(fl_pix / fl_ref * xywh_ref.h), (int)camera.buf.out_img_height / 2 + (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))) + std::max(0, camera.buf.out_img_width / 2 - (int)(fl_pix / fl_ref * xywh_ref.w / 2)), + std::max(0, camera.buf.out_img_height / 2 - (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))), + std::min((int)(fl_pix / fl_ref * xywh_ref.w), camera.buf.out_img_width / 2 + (int)(fl_pix / fl_ref * xywh_ref.w / 2)), + std::min((int)(fl_pix / fl_ref * xywh_ref.h), camera.buf.out_img_height / 2 + (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))) }; } diff --git a/system/camerad/cameras/ife.h b/system/camerad/cameras/ife.h index fd87d2baa4..49737f2db7 100644 --- a/system/camerad/cameras/ife.h +++ b/system/camerad/cameras/ife.h @@ -105,7 +105,7 @@ int build_update(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std: } -int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector &patches, uint32_t out_width, uint32_t out_height) { +int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector &patches) { uint8_t *start = dst; // start with the every frame config @@ -185,12 +185,12 @@ int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo // output size/scaling dst += write_cont(dst, 0xa3c, { 0x00000003, - ((out_width - 1) << 16) | (s->frame_width - 1), + ((s->frame_width - 1) << 16) | (s->frame_width - 1), 0x30036666, 0x00000000, 0x00000000, s->frame_width - 1, - ((out_height - 1) << 16) | (s->frame_height - 1), + ((s->frame_height - 1) << 16) | (s->frame_height - 1), 0x30036666, 0x00000000, 0x00000000, @@ -198,12 +198,12 @@ int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo }); dst += write_cont(dst, 0xa68, { 0x00000003, - ((out_width / 2 - 1) << 16) | (s->frame_width - 1), + ((s->frame_width/2 - 1) << 16) | (s->frame_width - 1), 0x3006cccc, 0x00000000, 0x00000000, s->frame_width - 1, - ((out_height / 2 - 1) << 16) | (s->frame_height - 1), + ((s->frame_height/2 - 1) << 16) | (s->frame_height - 1), 0x3006cccc, 0x00000000, 0x00000000, @@ -212,12 +212,12 @@ int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo // cropping dst += write_cont(dst, 0xe10, { - out_height - 1, - out_width - 1, + s->frame_height - 1, + s->frame_width - 1, }); dst += write_cont(dst, 0xe30, { - out_height / 2 - 1, - out_width - 1, + s->frame_height/2 - 1, + s->frame_width - 1, }); dst += write_cont(dst, 0xe18, { 0x0ff00000, diff --git a/system/camerad/cameras/spectra.cc b/system/camerad/cameras/spectra.cc index 47ae9061f4..4535078771 100644 --- a/system/camerad/cameras/spectra.cc +++ b/system/camerad/cameras/spectra.cc @@ -281,21 +281,18 @@ void SpectraCamera::camera_open(VisionIpcServer *v, cl_device_id device_id, cl_c if (!enabled) return; - buf.out_img_width = sensor->frame_width / sensor->out_scale; - buf.out_img_height = (sensor->hdr_offset > 0 ? (sensor->frame_height - sensor->hdr_offset) / 2 : sensor->frame_height) / sensor->out_scale; - // size is driven by all the HW that handles frames, // the video encoder has certain alignment requirements in this case - stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, buf.out_img_width); - y_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, buf.out_img_height); - uv_height = VENUS_UV_SCANLINES(COLOR_FMT_NV12, buf.out_img_height); + stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, sensor->frame_width); + y_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, sensor->frame_height); + uv_height = VENUS_UV_SCANLINES(COLOR_FMT_NV12, sensor->frame_height); uv_offset = stride*y_height; yuv_size = uv_offset + stride*uv_height; if (cc.output_type != ISP_RAW_OUTPUT) { uv_offset = ALIGNED_SIZE(uv_offset, 0x1000); yuv_size = uv_offset + ALIGNED_SIZE(stride*uv_height, 0x1000); } - assert(stride == VENUS_UV_STRIDE(COLOR_FMT_NV12, buf.out_img_width)); + assert(stride == VENUS_UV_STRIDE(COLOR_FMT_NV12, sensor->frame_width)); assert(y_height/2 == uv_height); open = true; @@ -648,14 +645,14 @@ void SpectraCamera::config_bps(int idx, int request_id) { io_cfg[1].mem_handle[0] = buf_handle_yuv[idx]; io_cfg[1].mem_handle[1] = buf_handle_yuv[idx]; io_cfg[1].planes[0] = (struct cam_plane_cfg){ - .width = buf.out_img_width, - .height = buf.out_img_height, + .width = sensor->frame_width, + .height = sensor->frame_height, .plane_stride = stride, .slice_height = y_height, }; io_cfg[1].planes[1] = (struct cam_plane_cfg){ - .width = buf.out_img_width, - .height = buf.out_img_height / 2, + .width = sensor->frame_width, + .height = sensor->frame_height/2, .plane_stride = stride, .slice_height = uv_height, }; @@ -740,7 +737,7 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) { bool is_raw = cc.output_type != ISP_IFE_PROCESSED; if (!is_raw) { if (init) { - buf_desc[0].length = build_initial_config((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches, buf.out_img_width, buf.out_img_height); + buf_desc[0].length = build_initial_config((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches); } else { buf_desc[0].length = build_update((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches); } @@ -847,14 +844,14 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) { io_cfg[0].mem_handle[0] = buf_handle_yuv[idx]; io_cfg[0].mem_handle[1] = buf_handle_yuv[idx]; io_cfg[0].planes[0] = (struct cam_plane_cfg){ - .width = buf.out_img_width, - .height = buf.out_img_height, + .width = sensor->frame_width, + .height = sensor->frame_height, .plane_stride = stride, .slice_height = y_height, }; io_cfg[0].planes[1] = (struct cam_plane_cfg){ - .width = buf.out_img_width, - .height = buf.out_img_height / 2, + .width = sensor->frame_width, + .height = sensor->frame_height/2, .plane_stride = stride, .slice_height = uv_height, }; @@ -996,9 +993,6 @@ bool SpectraCamera::openSensor() { LOGD("-- Probing sensor %d", cc.camera_num); auto init_sensor_lambda = [this](SensorInfo *s) { - if (s->image_sensor == cereal::FrameData::ImageSensor::OS04C10 && cc.output_type == ISP_IFE_PROCESSED) { - ((OS04C10*)s)->ife_downscale_configure(); - } sensor.reset(s); return (sensors_init() == 0); }; @@ -1071,8 +1065,8 @@ void SpectraCamera::configISP() { .data[0] = (struct cam_isp_out_port_info){ .res_type = CAM_ISP_IFE_OUT_RES_FULL, .format = CAM_FORMAT_NV12, - .width = buf.out_img_width, - .height = buf.out_img_height + sensor->extra_height, + .width = sensor->frame_width, + .height = sensor->frame_height + sensor->extra_height, .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, }, }; @@ -1147,8 +1141,8 @@ void SpectraCamera::configICP() { }, .out_res[0] = (struct cam_icp_res_info){ .format = 0x3, // YUV420NV12 - .width = buf.out_img_width, - .height = buf.out_img_height, + .width = sensor->frame_width, + .height = sensor->frame_height, .fps = 20, }, }; diff --git a/system/camerad/sensors/os04c10.cc b/system/camerad/sensors/os04c10.cc index 38be4ecca4..d008e1d07b 100644 --- a/system/camerad/sensors/os04c10.cc +++ b/system/camerad/sensors/os04c10.cc @@ -20,17 +20,6 @@ const uint32_t os04c10_analog_gains_reg[] = { } // namespace -void OS04C10::ife_downscale_configure() { - out_scale = 2; - - pixel_size_mm = 0.002; - frame_width = 2688; - frame_height = 1520; - exposure_time_max = 2352; - - init_reg_array.insert(init_reg_array.end(), std::begin(ife_downscale_override_array_os04c10), std::end(ife_downscale_override_array_os04c10)); -} - OS04C10::OS04C10() { image_sensor = cereal::FrameData::ImageSensor::OS04C10; bayer_pattern = CAM_ISP_PATTERN_BAYER_BGBGBG; diff --git a/system/camerad/sensors/os04c10_registers.h b/system/camerad/sensors/os04c10_registers.h index b501656090..8b1c78c69d 100644 --- a/system/camerad/sensors/os04c10_registers.h +++ b/system/camerad/sensors/os04c10_registers.h @@ -88,6 +88,8 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x37c7, 0xa8}, {0x37da, 0x11}, {0x381f, 0x08}, + // {0x3829, 0x03}, + // {0x3832, 0x00}, {0x3881, 0x00}, {0x3888, 0x04}, {0x388b, 0x00}, @@ -197,6 +199,7 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x370b, 0x48}, {0x370c, 0x01}, {0x370f, 0x00}, + {0x3714, 0x28}, {0x3716, 0x04}, {0x3719, 0x11}, {0x371a, 0x1e}, @@ -228,6 +231,7 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x37bd, 0x01}, {0x37bf, 0x26}, {0x37c0, 0x11}, + {0x37c2, 0x14}, {0x37cd, 0x19}, {0x37e0, 0x08}, {0x37e6, 0x04}, @@ -237,9 +241,14 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x37d8, 0x02}, {0x37e2, 0x10}, {0x3739, 0x10}, + {0x3662, 0x08}, {0x37e4, 0x20}, {0x37e3, 0x08}, + {0x37d9, 0x04}, {0x4040, 0x00}, + {0x4041, 0x03}, + {0x4008, 0x01}, + {0x4009, 0x06}, // FSIN {0x3002, 0x22}, @@ -260,11 +269,20 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x3802, 0x00}, {0x3803, 0x00}, {0x3804, 0x0a}, {0x3805, 0x8f}, {0x3806, 0x05}, {0x3807, 0xff}, + {0x3808, 0x05}, {0x3809, 0x40}, + {0x380a, 0x02}, {0x380b, 0xf8}, {0x3811, 0x08}, {0x3813, 0x08}, + {0x3814, 0x03}, {0x3815, 0x01}, + {0x3816, 0x03}, {0x3817, 0x01}, + {0x380c, 0x0b}, {0x380d, 0xac}, // HTS + {0x380e, 0x06}, {0x380f, 0x9c}, // VTS + + {0x3820, 0xb3}, + {0x3821, 0x01}, {0x3880, 0x00}, {0x3882, 0x20}, {0x3c91, 0x0b}, @@ -313,40 +331,4 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { // r {0x5104, 0x08}, {0x5105, 0xd6}, {0x5144, 0x08}, {0x5145, 0xd6}, - - {0x3714, 0x28}, - {0x37c2, 0x14}, - {0x3662, 0x08}, - {0x37d9, 0x04}, - {0x4041, 0x03}, - {0x4008, 0x01}, - {0x4009, 0x06}, - {0x3808, 0x05}, {0x3809, 0x40}, - {0x380a, 0x02}, {0x380b, 0xf8}, - {0x3814, 0x03}, - {0x3816, 0x03}, - {0x380c, 0x0b}, {0x380d, 0xac}, // HTS - {0x380e, 0x06}, {0x380f, 0x9c}, // VTS - {0x3820, 0xb3}, - {0x3821, 0x01}, -}; - -const struct i2c_random_wr_payload ife_downscale_override_array_os04c10[] = { - // OS04C10_AA_00_02_17_wAO_2688x1524_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz - {0x3829, 0x03}, - {0x3714, 0x24}, - {0x37c2, 0x04}, - {0x3662, 0x10}, - {0x37d9, 0x08}, - {0x4041, 0x07}, - {0x4008, 0x02}, - {0x4009, 0x0d}, - {0x3808, 0x0a}, {0x3809, 0x80}, - {0x380a, 0x05}, {0x380b, 0xf0}, - {0x3814, 0x01}, - {0x3816, 0x01}, - {0x380c, 0x08}, {0x380d, 0x5c}, // HTS - {0x380e, 0x09}, {0x380f, 0x38}, // VTS - {0x3820, 0xb0}, - {0x3821, 0x00}, }; diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h index d4be3cf036..c1131aafdf 100644 --- a/system/camerad/sensors/sensor.h +++ b/system/camerad/sensors/sensor.h @@ -29,7 +29,6 @@ public: uint32_t frame_stride; uint32_t frame_offset = 0; uint32_t extra_height = 0; - int out_scale = 1; int registers_offset = -1; int stats_offset = -1; int hdr_offset = -1; @@ -110,7 +109,6 @@ public: class OS04C10 : public SensorInfo { public: OS04C10(); - void ife_downscale_configure(); std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override; float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override; int getSlaveAddress(int port) const override; From 8cefc00a6e67499eddf4fadb8eb4f1f491a50bd7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Mon, 21 Apr 2025 17:06:53 -0700 Subject: [PATCH 21/24] Tomb Raider 3 (#35042) * 92b64884-4506-4a03-87ad-33e1a177fe73/400 * faster lat --- selfdrive/modeld/modeld.py | 2 +- selfdrive/modeld/models/driving_policy.onnx | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 1060b36d28..1d0facd133 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -41,7 +41,7 @@ POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl' VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl' POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl' -LAT_SMOOTH_SECONDS = 0.3 +LAT_SMOOTH_SECONDS = 0.1 LONG_SMOOTH_SECONDS = 0.3 diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index 3bf2e01b5e..df11e590dd 100644 --- a/selfdrive/modeld/models/driving_policy.onnx +++ b/selfdrive/modeld/models/driving_policy.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:fa00a07a4307b0f1638a10ec33b651d8a102e38371289b744f43215c89dfd342 +oid sha256:84061882148485dabfdd23ee7e0925284a4370d0ae5be804d6099ae847352891 size 15578328 From 23524e203826ca944ffbff9789ec9765ba2b6c8e Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Tue, 22 Apr 2025 15:28:54 +0100 Subject: [PATCH 22/24] ui(raylib): reduce spinner rotation artifact (#35048) * ui(raylib): reduce spinner rotation artifact A visual artifact (white pixels) appeared on the edge of the rotating spinner track texture, likely due to RGB color bleed during bilinear filtering in Raylib. Pre-multiplying the alpha channel of the spinner track image using `rl.image_alpha_premultiply` significantly reduces the visibility of the artifact. * lint --- system/ui/lib/application.py | 4 +++- system/ui/spinner.py | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/system/ui/lib/application.py b/system/ui/lib/application.py index 470396bbc7..23f9316c3a 100644 --- a/system/ui/lib/application.py +++ b/system/ui/lib/application.py @@ -50,9 +50,11 @@ class GuiApplication: self._set_styles() self._load_fonts() - def load_texture_from_image(self, file_name: str, width: int, height: int): + def load_texture_from_image(self, file_name: str, width: int, height: int, alpha_premultiply = False): """Load and resize a texture, storing it for later automatic unloading.""" image = rl.load_image(file_name) + if alpha_premultiply: + rl.image_alpha_premultiply(image) rl.image_resize(image, width, height) texture = rl.load_texture_from_image(image) # Set texture filtering to smooth the result diff --git a/system/ui/spinner.py b/system/ui/spinner.py index adfb604de9..f3724cbd1e 100755 --- a/system/ui/spinner.py +++ b/system/ui/spinner.py @@ -23,7 +23,8 @@ def clamp(value, min_value, max_value): class Spinner: def __init__(self): self._comma_texture = gui_app.load_texture_from_image(os.path.join(BASEDIR, "selfdrive/assets/img_spinner_comma.png"), TEXTURE_SIZE, TEXTURE_SIZE) - self._spinner_texture = gui_app.load_texture_from_image(os.path.join(BASEDIR, "selfdrive/assets/img_spinner_track.png"), TEXTURE_SIZE, TEXTURE_SIZE) + self._spinner_texture = gui_app.load_texture_from_image(os.path.join(BASEDIR, "selfdrive/assets/img_spinner_track.png"), TEXTURE_SIZE, TEXTURE_SIZE, + alpha_premultiply=True) self._rotation = 0.0 self._text: str = "" self._progress: int | None = None From 651ff78cb051280513c297ac1ab356eb164116e8 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Tue, 22 Apr 2025 16:07:05 +0100 Subject: [PATCH 23/24] ui(raylib): increase font size and wrap text in spinner (#35049) - Wrap text onto separate lines - Increase font size to be closer to Qt - Remove extra letter spacing - 0.0 should use font default spacing, and this is used in `wrap_text` Will fix vertical alignment separately, as both the text and progress bar layouts need to be considered --- system/ui/spinner.py | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/system/ui/spinner.py b/system/ui/spinner.py index f3724cbd1e..fd4264be6c 100755 --- a/system/ui/spinner.py +++ b/system/ui/spinner.py @@ -5,14 +5,17 @@ import threading from openpilot.common.basedir import BASEDIR from openpilot.system.ui.lib.application import gui_app +from openpilot.system.ui.text import wrap_text # Constants PROGRESS_BAR_WIDTH = 1000 PROGRESS_BAR_HEIGHT = 20 DEGREES_PER_SECOND = 360.0 # one full rotation per second -MARGIN = 200 +MARGIN_H = 100 +MARGIN_V = 200 TEXTURE_SIZE = 360 -FONT_SIZE = 80 +FONT_SIZE = 88 +LINE_HEIGHT = 96 DARKGRAY = (55, 55, 55, 255) @@ -26,18 +29,18 @@ class Spinner: self._spinner_texture = gui_app.load_texture_from_image(os.path.join(BASEDIR, "selfdrive/assets/img_spinner_track.png"), TEXTURE_SIZE, TEXTURE_SIZE, alpha_premultiply=True) self._rotation = 0.0 - self._text: str = "" self._progress: int | None = None + self._wrapped_lines: list[str] = [] self._lock = threading.Lock() def set_text(self, text: str) -> None: with self._lock: if text.isdigit(): self._progress = clamp(int(text), 0, 100) - self._text = "" + self._wrapped_lines = [] else: self._progress = None - self._text = text + self._wrapped_lines = wrap_text(text, FONT_SIZE, gui_app.width - MARGIN_H) def render(self): center = rl.Vector2(gui_app.width / 2.0, gui_app.height / 2.0) @@ -54,10 +57,10 @@ class Spinner: rl.draw_texture_v(self._comma_texture, comma_position, rl.WHITE) # Display progress bar or text based on user input - y_pos = rl.get_screen_height() - MARGIN - PROGRESS_BAR_HEIGHT + y_pos = rl.get_screen_height() - MARGIN_V - PROGRESS_BAR_HEIGHT with self._lock: progress = self._progress - text = self._text + wrapped_lines = self._wrapped_lines if progress is not None: bar = rl.Rectangle(center.x - PROGRESS_BAR_WIDTH / 2.0, y_pos, PROGRESS_BAR_WIDTH, PROGRESS_BAR_HEIGHT) @@ -65,10 +68,11 @@ class Spinner: bar.width *= progress / 100.0 rl.draw_rectangle_rounded(bar, 1, 10, rl.WHITE) - elif text: - text_size = rl.measure_text_ex(gui_app.font(), text, FONT_SIZE, 1.0) - rl.draw_text_ex(gui_app.font(), text, - rl.Vector2(center.x - text_size.x / 2, y_pos), FONT_SIZE, 1.0, rl.WHITE) + elif wrapped_lines: + for i, line in enumerate(wrapped_lines): + text_size = rl.measure_text_ex(gui_app.font(), line, FONT_SIZE, 0.0) + rl.draw_text_ex(gui_app.font(), line, rl.Vector2(center.x - text_size.x / 2, y_pos + i * LINE_HEIGHT), + FONT_SIZE, 0.0, rl.WHITE) if __name__ == "__main__": From c9259a9bcfd86f77c0d336b470c1f72cbfd073e8 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Tue, 22 Apr 2025 20:54:36 +0100 Subject: [PATCH 24/24] ui(raylib): update spinner vertical pos (#35051) Adjust the spinner vertical position when displaying text or a progress bar - When displaying the progress bar, center the comma logo and spinner in the middle of the screen - When displaying text, center the entire content vertically Also updated `wrap_text` to not include an empty line in the array if it's the first line, so that `wrap_text("")` always returns `[]` --- system/ui/spinner.py | 23 ++++++++++++++++------- system/ui/text.py | 4 +++- 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/system/ui/spinner.py b/system/ui/spinner.py index fd4264be6c..9c1882cf5f 100755 --- a/system/ui/spinner.py +++ b/system/ui/spinner.py @@ -12,7 +12,6 @@ PROGRESS_BAR_WIDTH = 1000 PROGRESS_BAR_HEIGHT = 20 DEGREES_PER_SECOND = 360.0 # one full rotation per second MARGIN_H = 100 -MARGIN_V = 200 TEXTURE_SIZE = 360 FONT_SIZE = 88 LINE_HEIGHT = 96 @@ -43,7 +42,22 @@ class Spinner: self._wrapped_lines = wrap_text(text, FONT_SIZE, gui_app.width - MARGIN_H) def render(self): - center = rl.Vector2(gui_app.width / 2.0, gui_app.height / 2.0) + with self._lock: + progress = self._progress + wrapped_lines = self._wrapped_lines + + if wrapped_lines: + # Calculate total height required for spinner and text + spacing = 50 + total_height = TEXTURE_SIZE + spacing + len(wrapped_lines) * LINE_HEIGHT + center_y = (gui_app.height - total_height) / 2.0 + TEXTURE_SIZE / 2.0 + else: + # Center spinner vertically + spacing = 150 + center_y = gui_app.height / 2.0 + y_pos = center_y + TEXTURE_SIZE / 2.0 + spacing + + center = rl.Vector2(gui_app.width / 2.0, center_y) spinner_origin = rl.Vector2(TEXTURE_SIZE / 2.0, TEXTURE_SIZE / 2.0) comma_position = rl.Vector2(center.x - TEXTURE_SIZE / 2.0, center.y - TEXTURE_SIZE / 2.0) @@ -57,11 +71,6 @@ class Spinner: rl.draw_texture_v(self._comma_texture, comma_position, rl.WHITE) # Display progress bar or text based on user input - y_pos = rl.get_screen_height() - MARGIN_V - PROGRESS_BAR_HEIGHT - with self._lock: - progress = self._progress - wrapped_lines = self._wrapped_lines - if progress is not None: bar = rl.Rectangle(center.x - PROGRESS_BAR_WIDTH / 2.0, y_pos, PROGRESS_BAR_WIDTH, PROGRESS_BAR_HEIGHT) rl.draw_rectangle_rounded(bar, 1, 10, DARKGRAY) diff --git a/system/ui/text.py b/system/ui/text.py index 4ea886ba1e..79c20ce8dc 100755 --- a/system/ui/text.py +++ b/system/ui/text.py @@ -21,7 +21,9 @@ def wrap_text(text, font_size, max_width): for paragraph in text.split("\n"): if not paragraph.strip(): - lines.append("") + # Don't add empty lines first, ensuring wrap_text("") returns [] + if lines: + lines.append("") continue indent = re.match(r"^\s*", paragraph).group() current_line = indent