diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 5227757f0e..b32ccbd237 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -1,9 +1,10 @@ import numpy as np from cereal import log -from common.filter_simple import FirstOrderFilter -from common.numpy_fast import interp -from common.realtime import DT_MDL -from system.swaglog import cloudlog +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.system.swaglog import cloudlog TRAJECTORY_SIZE = 33 @@ -33,10 +34,29 @@ class LanePlanner: self.l_lane_change_prob = 0. self.r_lane_change_prob = 0. - self.camera_offset = -CAMERA_OFFSET - self.path_offset = -PATH_OFFSET + self.camera_offset = CAMERA_OFFSET + self.path_offset = PATH_OFFSET + + self.param_s = Params() + self.custom_offsets = self.param_s.get_bool("CustomOffsets") + self._frame = 0 + + def update_custom_offsets(self): + self._frame += 1 + if self._frame % 50 == 0: + self.custom_offsets = self.param_s.get_bool("CustomOffsets") + if not self.custom_offsets: + self._frame = 0 + self.camera_offset = CAMERA_OFFSET + self.path_offset = PATH_OFFSET + return + + self.camera_offset = float(self.param_s.get("CameraOffset", encoding="utf8")) * 0.01 + self.path_offset = float(self.param_s.get("PathOffset", encoding="utf8")) * 0.01 + self._frame = 0 def parse_model(self, md): + self.update_custom_offsets() lane_lines = md.laneLines if len(lane_lines) == 4 and len(lane_lines[0].t) == TRAJECTORY_SIZE: self.ll_t = (np.array(lane_lines[1].t) + np.array(lane_lines[2].t))/2 diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 7a0f232aa7..e4aa6e328d 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -115,12 +115,11 @@ class LateralPlanner: low_speed = v_ego_car < 10 * CV.MPH_TO_MS if not self.get_dynamic_lane_profile(sm['longitudinalPlan']) and not low_speed: - d_path_xyz = self.LP.get_d_path(self.v_ego, self.t_idxs, self.path_xyz) + self.path_xyz = self.LP.get_d_path(self.v_ego, self.t_idxs, self.path_xyz) self.dynamic_lane_profile_status = False else: - d_path_xyz = self.path_xyz + self.path_xyz[:, 1] += self.LP.path_offset self.dynamic_lane_profile_status = True - self.path_xyz = d_path_xyz self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST, LATERAL_ACCEL_COST, LATERAL_JERK_COST, diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 031e7564e8..111017e822 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -58,7 +58,7 @@ def manager_init() -> None: ("CustomTorqueLateral", "0"), ("CameraControl", "2"), ("CameraControlToggle", "0"), - ("CameraOffset", "0"), + ("CameraOffset", "4"), ("CarModel", ""), ("CarModelText", ""), ("ChevronInfo", "1"), diff --git a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc index dc989452b8..ff4708996b 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc @@ -846,10 +846,10 @@ void BrightnessControl::refresh() { // Camera Offset Value CameraOffset::CameraOffset() : SPOptionControl ( "CameraOffset", - tr("Camera Offset (cm)"), - tr("Hack to trick vehicle to be left or right biased in its lane. Decreasing the value will make the car keep more left, increasing will make it keep more right. Changes take effect immediately."), + tr("Camera Offset - Laneful Only (cm)"), + tr("Hack to trick vehicle to be left or right biased in its lane. Decreasing the value will make the car keep more left, increasing will make it keep more right. Changes take effect immediately. Default: +4 cm"), "../assets/offroad/icon_blank.png", - {-10, 10}) { + {-20, 20}) { refresh(); }