From 65431f4e2f0a6835fa2f4a2a9d2dc5a0ce227e74 Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Fri, 18 Apr 2025 11:02:50 -0700 Subject: [PATCH 1/3] DEC: adjust dynamic speed adaptation parameters (#813) * early and better * Update sunnypilot/selfdrive/controls/lib/dec/constants.py --------- Co-authored-by: Jason Wen Co-authored-by: Discountchubbs <159560811+Discountchubbs@users.noreply.github.com> --- sunnypilot/selfdrive/controls/lib/dec/constants.py | 5 ++--- sunnypilot/selfdrive/controls/lib/dec/dec.py | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/constants.py b/sunnypilot/selfdrive/controls/lib/dec/constants.py index c1e372660..48f4203e9 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/constants.py +++ b/sunnypilot/selfdrive/controls/lib/dec/constants.py @@ -2,12 +2,11 @@ class WMACConstants: LEAD_WINDOW_SIZE = 5 LEAD_PROB = 0.5 - SLOW_DOWN_WINDOW_SIZE = 4 + SLOW_DOWN_WINDOW_SIZE = 5 SLOW_DOWN_PROB = 0.6 SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.] - #SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.] - SLOW_DOWN_DIST = [30., 45., 60., 80., 100., 120., 135., 150.] + SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.] SLOWNESS_WINDOW_SIZE = 12 SLOWNESS_PROB = 0.5 diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 0c33da500..812e5cc15 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -164,7 +164,7 @@ class DynamicExperimentalController: """ Adapts the slow-down threshold based on vehicle speed and recent behavior. """ - slowdown_scaling_factor: float = (1.0 + 0.05 * np.log(1 + len(self._slow_down_gmac.data))) + slowdown_scaling_factor: float = (1.0 + 0.03 * np.log(1 + len(self._slow_down_gmac.data))) adaptive_threshold: float = float( interp(self._v_ego_kph, WMACConstants.SLOW_DOWN_BP, WMACConstants.SLOW_DOWN_DIST) * slowdown_scaling_factor ) From 1db91d6257a21c428eb1851458a5bff02194ab45 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sat, 19 Apr 2025 07:53:20 +0200 Subject: [PATCH 2/3] ci: ui preview add unicode characters and emojis (#835) Add unicode characters and emojis! --- Dockerfile.openpilot_base | 1 + 1 file changed, 1 insertion(+) diff --git a/Dockerfile.openpilot_base b/Dockerfile.openpilot_base index 44d8d95e9..04fb589bf 100644 --- a/Dockerfile.openpilot_base +++ b/Dockerfile.openpilot_base @@ -60,6 +60,7 @@ ENV NVIDIA_DRIVER_CAPABILITIES=graphics,utility,compute ENV QTWEBENGINE_DISABLE_SANDBOX=1 RUN dbus-uuidgen > /etc/machine-id +RUN apt-get update && apt-get install -y fonts-noto-cjk fonts-noto-color-emoji ARG USER=batman ARG USER_UID=1001 From bcdb5466380e9e789afaa9b03620607c11d6719c Mon Sep 17 00:00:00 2001 From: Tim Wilson Date: Sat, 19 Apr 2025 09:47:47 -0600 Subject: [PATCH 3/3] NNLC: decreased low-speed factor (#822) * NNLC: decreased low-speed factor * np.float to float * format * add tests for sanity check --------- Co-authored-by: Discountchubbs <159560811+Discountchubbs@users.noreply.github.com> Co-authored-by: Jason Wen --- selfdrive/controls/lib/latcontrol_torque.py | 3 +- .../controls/lib/latcontrol_torque_ext.py | 5 +- .../lib/latcontrol_torque_ext_base.py | 2 + .../selfdrive/controls/lib/nnlc/nnlc.py | 7 +++ .../controls/lib/nnlc/tests/test_nnlc.py | 56 +++++++++++++++++++ 5 files changed, 71 insertions(+), 2 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 613824a4e..5edf4cf91 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -80,7 +80,8 @@ class LatControlTorque(LatControl): # Lateral acceleration torque controller extension updates # Overrides stock ff and pid_log.error ff, pid_log = self.extension.update(CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation, - desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel) + desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel, + desired_curvature, actual_curvature) freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(pid_log.error, diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py index ee8d64dcd..fe54f46ca 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py @@ -13,7 +13,8 @@ class LatControlTorqueExt(NeuralNetworkLateralControl): super().__init__(lac_torque, CP, CP_SP) def update(self, CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation, - desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel): + desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel, + desired_curvature, actual_curvature): self._ff = ff self._pid_log = pid_log self._setpoint = setpoint @@ -21,6 +22,8 @@ class LatControlTorqueExt(NeuralNetworkLateralControl): self._lateral_accel_deadzone = lateral_accel_deadzone self._desired_lateral_accel = desired_lateral_accel self._actual_lateral_accel = actual_lateral_accel + self._desired_curvature = desired_curvature + self._actual_curvature = actual_curvature self.update_calculations(CS, VM, desired_lateral_accel) self.update_neural_network_feedforward(CS, params, calibrated_pose) diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py index 48d98501b..800bbac67 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py @@ -62,6 +62,8 @@ class LatControlTorqueExtBase: self._lateral_accel_deadzone = 0.0 self._desired_lateral_accel = 0.0 self._actual_lateral_accel = 0.0 + self._desired_curvature = 0.0 + self._actual_curvature = 0.0 # twilsonco's Lateral Neural Network Feedforward # Instantaneous lateral jerk changes very rapidly, making it not useful on its own, diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py index aee359574..ba4285ba2 100644 --- a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py +++ b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py @@ -16,6 +16,9 @@ from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base impo from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import MOCK_MODEL_PATH from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.model import NNTorqueModel +LOW_SPEED_X = [0, 10, 20, 30] +LOW_SPEED_Y = [12, 3, 1, 0] + # At a given roll, if pitch magnitude increases, the # gravitational acceleration component starts pointing @@ -59,6 +62,10 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase): if not self.enabled or not self.model_valid or not self.has_nn_model: return + low_speed_factor = float(np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)) ** 2 + self._setpoint = self._desired_lateral_accel + low_speed_factor * self._desired_curvature + self._measurement = self._actual_lateral_accel + low_speed_factor * self._actual_curvature + # update past data roll = params.roll if calibrated_pose is not None: diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py new file mode 100644 index 000000000..c7227c2d5 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py @@ -0,0 +1,56 @@ +from parameterized import parameterized + +from cereal import car, log +from opendbc.car.car_helpers import interfaces +from opendbc.car.honda.values import CAR as HONDA +from opendbc.car.hyundai.values import CAR as HYUNDAI +from opendbc.car.toyota.values import CAR as TOYOTA +from opendbc.car.vehicle_model import VehicleModel +from openpilot.common.params import Params +from openpilot.selfdrive.car.helpers import convert_to_capnp +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.selfdrive.locationd.helpers import Pose +from openpilot.common.mock.generators import generate_livePose +from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces + + +class TestNeuralNetworkLateralControl: + + @parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN]) + def test_saturation(self, car_name): + params = Params() + params.put_bool("NeuralNetworkLateralControl", True) + + CarInterface = interfaces[car_name] + CP = CarInterface.get_non_essential_params(car_name) + CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name) + CI = CarInterface(CP, CP_SP) + + sunnypilot_interfaces.setup_interfaces(CP, CP_SP, params) + + CP_SP = convert_to_capnp(CP_SP) + VM = VehicleModel(CP) + + controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI) + + CS = car.CarState.new_message() + CS.vEgo = 30 + CS.steeringPressed = False + + params = log.LiveParametersData.new_message() + + lp = generate_livePose() + pose = Pose.from_live_pose(lp.livePose) + + # Saturate for curvature limited and controller limited + for _ in range(1000): + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True) + assert lac_log.saturated + + for _ in range(1000): + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False) + assert not lac_log.saturated + + for _ in range(1000): + _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False) + assert lac_log.saturated